Merge branch '2022'

wpimath/src/main/java/edu/wpi/first/math/Drake.java

@@ -4,8 +4,6 @@
 
 package edu.wpi.first.math;
 
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Num;
 import org.ejml.simple.SimpleMatrix;
 
 public final class Drake {

wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java

@@ -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.
+
+package edu.wpi.first.math;
+
+import java.util.Objects;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * A class for constructing arbitrary RxC matrices.
+ *
+ * @param <R> The number of rows of the desired matrix.
+ * @param <C> The number of columns of the desired matrix.
+ */
+public class MatBuilder<R extends Num, C extends Num> {
+  final Nat<R> m_rows;
+  final Nat<C> m_cols;
+
+  /**
+   * Fills the matrix with the given data, encoded in row major form. (The matrix is filled row by
+   * row, left to right with the given data).
+   *
+   * @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(
+          "Invalid matrix data provided. Wanted "
+              + this.m_rows.getNum()
+              + " x "
+              + this.m_cols.getNum()
+              + " matrix, but got "
+              + data.length
+              + " elements");
+    } else {
+      return new Matrix<>(new SimpleMatrix(this.m_rows.getNum(), this.m_cols.getNum(), true, data));
+    }
+  }
+
+  /**
+   * Creates a new {@link MatBuilder} with the given dimensions.
+   *
+   * @param rows The number of rows of the matrix.
+   * @param cols The number of columns of the matrix.
+   */
+  public MatBuilder(Nat<R> rows, Nat<C> cols) {
+    this.m_rows = Objects.requireNonNull(rows);
+    this.m_cols = Objects.requireNonNull(cols);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/MathUtil.java

@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+public final class MathUtil {
+  private MathUtil() {
+    throw new AssertionError("utility class");
+  }
+
+  /**
+   * Returns value clamped between low and high boundaries.
+   *
+   * @param value Value to clamp.
+   * @param low The lower boundary to which to clamp value.
+   * @param high The higher boundary to which to clamp value.
+   */
+  public static int clamp(int value, int low, int high) {
+    return Math.max(low, Math.min(value, high));
+  }
+
+  /**
+   * Returns value clamped between low and high boundaries.
+   *
+   * @param value Value to clamp.
+   * @param low The lower boundary to which to clamp value.
+   * @param high The higher boundary to which to clamp value.
+   */
+  public static double clamp(double value, double low, double high) {
+    return Math.max(low, Math.min(value, high));
+  }
+
+  /**
+   * Returns modulus of input.
+   *
+   * @param input Input value to wrap.
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  public static double inputModulus(double input, double minimumInput, double maximumInput) {
+    double modulus = maximumInput - minimumInput;
+
+    // Wrap input if it's above the maximum input
+    int numMax = (int) ((input - minimumInput) / modulus);
+    input -= numMax * modulus;
+
+    // Wrap input if it's below the minimum input
+    int numMin = (int) ((input - maximumInput) / modulus);
+    input -= numMin * modulus;
+
+    return input;
+  }
+
+  /**
+   * Wraps an angle to the range -pi to pi radians.
+   *
+   * @param angleRadians Angle to wrap in radians.
+   */
+  public static double angleModulus(double angleRadians) {
+    return inputModulus(angleRadians, -Math.PI, Math.PI);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/Matrix.java

@@ -0,0 +1,688 @@
+// 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.MatrixDimensionException;
+import org.ejml.data.DMatrixRMaj;
+import org.ejml.dense.row.CommonOps_DDRM;
+import org.ejml.dense.row.MatrixFeatures_DDRM;
+import org.ejml.dense.row.NormOps_DDRM;
+import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
+import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
+ *
+ * <p>This class is intended to be used alongside the state space library.
+ *
+ * @param <R> The number of rows in this matrix.
+ * @param <C> The number of columns in this matrix.
+ */
+@SuppressWarnings("PMD.ExcessivePublicCount")
+public class Matrix<R extends Num, C extends Num> {
+  protected final SimpleMatrix m_storage;
+
+  /**
+   * Constructs an empty zero matrix of the given dimensions.
+   *
+   * @param rows The number of rows of the matrix.
+   * @param columns The number of columns of the matrix.
+   */
+  public Matrix(Nat<R> rows, Nat<C> columns) {
+    this.m_storage =
+        new SimpleMatrix(
+            Objects.requireNonNull(rows).getNum(), Objects.requireNonNull(columns).getNum());
+  }
+
+  /**
+   * Constructs a new {@link Matrix} with the given storage. Caller should make sure that the
+   * provided generic bounds match the shape of the provided {@link Matrix}.
+   *
+   * <p>NOTE:It is not recommend to use this constructor unless the {@link SimpleMatrix} API is
+   * absolutely necessary due to the desired function not being accessible through the {@link
+   * Matrix} wrapper.
+   *
+   * @param storage The {@link SimpleMatrix} to back this value.
+   */
+  public Matrix(SimpleMatrix storage) {
+    this.m_storage = Objects.requireNonNull(storage);
+  }
+
+  /**
+   * Constructs a new matrix with the storage of the supplied matrix.
+   *
+   * @param other The {@link Matrix} to copy the storage of.
+   */
+  public Matrix(Matrix<R, C> other) {
+    this.m_storage = Objects.requireNonNull(other).getStorage().copy();
+  }
+
+  /**
+   * Gets the underlying {@link SimpleMatrix} that this {@link Matrix} wraps.
+   *
+   * <p>NOTE:The use of this method is heavily discouraged as this removes any guarantee of type
+   * safety. This should only be called if the {@link SimpleMatrix} API is absolutely necessary due
+   * to the desired function not being accessible through the {@link Matrix} wrapper.
+   *
+   * @return The underlying {@link SimpleMatrix} storage.
+   */
+  public SimpleMatrix getStorage() {
+    return m_storage;
+  }
+
+  /**
+   * Gets the number of columns in this matrix.
+   *
+   * @return The number of columns, according to the internal storage.
+   */
+  public final int getNumCols() {
+    return this.m_storage.numCols();
+  }
+
+  /**
+   * Gets the number of rows in this matrix.
+   *
+   * @return The number of rows, according to the internal storage.
+   */
+  public final int getNumRows() {
+    return this.m_storage.numRows();
+  }
+
+  /**
+   * Get an element of this matrix.
+   *
+   * @param row The row of the element.
+   * @param col The column of the element.
+   * @return The element in this matrix at row,col.
+   */
+  public final double get(int row, int col) {
+    return this.m_storage.get(row, col);
+  }
+
+  /**
+   * Sets the value at the given indices.
+   *
+   * @param row The row of the element.
+   * @param col The column of the element.
+   * @param value The value to insert at the given location.
+   */
+  public final void set(int row, int col, double value) {
+    this.m_storage.set(row, col, value);
+  }
+
+  /**
+   * Sets a row to a given row vector.
+   *
+   * @param row The row to set.
+   * @param val The row vector to set the given row to.
+   */
+  public final void setRow(int row, Matrix<N1, C> val) {
+    this.m_storage.setRow(row, 0, Objects.requireNonNull(val).m_storage.getDDRM().getData());
+  }
+
+  /**
+   * Sets a column to a given column vector.
+   *
+   * @param column The column to set.
+   * @param val The column vector to set the given row to.
+   */
+  public final void setColumn(int column, Matrix<R, N1> val) {
+    this.m_storage.setColumn(column, 0, Objects.requireNonNull(val).m_storage.getDDRM().getData());
+  }
+
+  /**
+   * Sets all the elements in "this" matrix equal to the specified value.
+   *
+   * @param value The value each element is set to.
+   */
+  public void fill(double value) {
+    this.m_storage.fill(value);
+  }
+
+  /**
+   * Returns the diagonal elements inside a vector or square matrix.
+   *
+   * <p>If "this" {@link Matrix} is a vector then a square matrix is returned. If a "this" {@link
+   * Matrix} is a matrix then a vector of diagonal elements is returned.
+   *
+   * @return The diagonal elements inside a vector or a square matrix.
+   */
+  public final Matrix<R, C> diag() {
+    return new Matrix<>(this.m_storage.diag());
+  }
+
+  /**
+   * Returns the largest element of this matrix.
+   *
+   * @return The largest element of this matrix.
+   */
+  public final double max() {
+    return CommonOps_DDRM.elementMax(this.m_storage.getDDRM());
+  }
+
+  /**
+   * Returns the absolute value of the element in this matrix with the largest absolute value.
+   *
+   * @return The absolute value of the element with the largest absolute value.
+   */
+  public final double maxAbs() {
+    return CommonOps_DDRM.elementMaxAbs(this.m_storage.getDDRM());
+  }
+
+  /**
+   * Returns the smallest element of this matrix.
+   *
+   * @return The smallest element of this matrix.
+   */
+  public final double minInternal() {
+    return CommonOps_DDRM.elementMin(this.m_storage.getDDRM());
+  }
+
+  /**
+   * Calculates the mean of the elements in this matrix.
+   *
+   * @return The mean value of this matrix.
+   */
+  public final double mean() {
+    return this.elementSum() / (double) this.m_storage.getNumElements();
+  }
+
+  /**
+   * Multiplies this matrix with another that has C rows.
+   *
+   * <p>As matrix multiplication is only defined if the number of columns in the first matrix
+   * matches the number of rows in the second, this operation will fail to compile under any other
+   * circumstances.
+   *
+   * @param other The other matrix to multiply by.
+   * @param <C2> The number of columns in the second matrix.
+   * @return The result of the matrix multiplication between "this" and the given matrix.
+   */
+  public final <C2 extends Num> Matrix<R, C2> times(Matrix<C, C2> other) {
+    return new Matrix<>(this.m_storage.mult(Objects.requireNonNull(other).m_storage));
+  }
+
+  /**
+   * Multiplies all the elements of this matrix by the given scalar.
+   *
+   * @param value The scalar value to multiply by.
+   * @return A new matrix with all the elements multiplied by the given value.
+   */
+  public Matrix<R, C> times(double value) {
+    return new Matrix<>(this.m_storage.scale(value));
+  }
+
+  /**
+   * Returns a matrix which is the result of an element by element multiplication of "this" and
+   * other.
+   *
+   * <p>c<sub>i,j</sub> = a<sub>i,j</sub>*other<sub>i,j</sub>
+   *
+   * @param other The other {@link Matrix} to preform element multiplication on.
+   * @return The element by element multiplication of "this" and other.
+   */
+  public final Matrix<R, C> elementTimes(Matrix<R, C> other) {
+    return new Matrix<>(this.m_storage.elementMult(Objects.requireNonNull(other).m_storage));
+  }
+
+  /**
+   * Subtracts the given value from all the elements of this matrix.
+   *
+   * @param value The value to subtract.
+   * @return The resultant matrix.
+   */
+  public final Matrix<R, C> minus(double value) {
+    return new Matrix<>(this.m_storage.minus(value));
+  }
+
+  /**
+   * Subtracts the given matrix from this matrix.
+   *
+   * @param value The matrix to subtract.
+   * @return The resultant matrix.
+   */
+  public final Matrix<R, C> minus(Matrix<R, C> value) {
+    return new Matrix<>(this.m_storage.minus(Objects.requireNonNull(value).m_storage));
+  }
+
+  /**
+   * Adds the given value to all the elements of this matrix.
+   *
+   * @param value The value to add.
+   * @return The resultant matrix.
+   */
+  public final Matrix<R, C> plus(double value) {
+    return new Matrix<>(this.m_storage.plus(value));
+  }
+
+  /**
+   * Adds the given matrix to this matrix.
+   *
+   * @param value The matrix to add.
+   * @return The resultant matrix.
+   */
+  public final Matrix<R, C> plus(Matrix<R, C> value) {
+    return new Matrix<>(this.m_storage.plus(Objects.requireNonNull(value).m_storage));
+  }
+
+  /**
+   * Divides all elements of this matrix by the given value.
+   *
+   * @param value The value to divide by.
+   * @return The resultant matrix.
+   */
+  public Matrix<R, C> div(int value) {
+    return new Matrix<>(this.m_storage.divide((double) value));
+  }
+
+  /**
+   * Divides all elements of this matrix by the given value.
+   *
+   * @param value The value to divide by.
+   * @return The resultant matrix.
+   */
+  public Matrix<R, C> div(double value) {
+    return new Matrix<>(this.m_storage.divide(value));
+  }
+
+  /**
+   * Calculates the transpose, M^T of this matrix.
+   *
+   * @return The transpose matrix.
+   */
+  public final Matrix<C, R> transpose() {
+    return new Matrix<>(this.m_storage.transpose());
+  }
+
+  /**
+   * Returns a copy of this matrix.
+   *
+   * @return A copy of this matrix.
+   */
+  public final Matrix<R, C> copy() {
+    return new Matrix<>(this.m_storage.copy());
+  }
+
+  /**
+   * Returns the inverse matrix of "this" matrix.
+   *
+   * @return The inverse of "this" matrix.
+   * @throws org.ejml.data.SingularMatrixException If "this" matrix is non-invertable.
+   */
+  public final Matrix<R, C> inv() {
+    return new Matrix<>(this.m_storage.invert());
+  }
+
+  /**
+   * Returns the solution x to the equation Ax = b, where A is "this" matrix.
+   *
+   * <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.
+   *
+   * @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));
+  }
+
+  /**
+   * Computes the matrix exponential using Eigen's solver. This method only works for square
+   * matrices, and will otherwise throw an {@link MatrixDimensionException}.
+   *
+   * @return The exponential of A.
+   */
+  public final Matrix<R, C> exp() {
+    if (this.getNumRows() != this.getNumCols()) {
+      throw new MatrixDimensionException(
+          "Non-square matrices cannot be exponentiated! "
+              + "This matrix is "
+              + this.getNumRows()
+              + " x "
+              + this.getNumCols());
+    }
+    Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
+    WPIMathJNI.exp(
+        this.m_storage.getDDRM().getData(),
+        this.getNumRows(),
+        toReturn.m_storage.getDDRM().getData());
+    return toReturn;
+  }
+
+  /**
+   * Computes the matrix power using Eigen's solver. This method only works for square matrices, and
+   * will otherwise throw an {@link MatrixDimensionException}.
+   *
+   * @param exponent The exponent.
+   * @return The exponential of A.
+   */
+  public final Matrix<R, C> pow(double exponent) {
+    if (this.getNumRows() != this.getNumCols()) {
+      throw new MatrixDimensionException(
+          "Non-square matrices cannot be raised to a power! "
+              + "This matrix is "
+              + this.getNumRows()
+              + " x "
+              + this.getNumCols());
+    }
+    Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
+    WPIMathJNI.pow(
+        this.m_storage.getDDRM().getData(),
+        this.getNumRows(),
+        exponent,
+        toReturn.m_storage.getDDRM().getData());
+    return toReturn;
+  }
+
+  /**
+   * Returns the determinant of this matrix.
+   *
+   * @return The determinant of this matrix.
+   */
+  public final double det() {
+    return this.m_storage.determinant();
+  }
+
+  /**
+   * Computes the Frobenius normal of the matrix.
+   *
+   * <p>normF = Sqrt{ &sum;<sub>i=1:m</sub> &sum;<sub>j=1:n</sub> { a<sub>ij</sub><sup>2</sup>} }
+   *
+   * @return The matrix's Frobenius normal.
+   */
+  public final double normF() {
+    return this.m_storage.normF();
+  }
+
+  /**
+   * Computes the induced p = 1 matrix norm.
+   *
+   * <p>||A||<sub>1</sub>= max(j=1 to n; sum(i=1 to m; |a<sub>ij</sub>|))
+   *
+   * @return The norm.
+   */
+  public final double normIndP1() {
+    return NormOps_DDRM.inducedP1(this.m_storage.getDDRM());
+  }
+
+  /**
+   * Computes the sum of all the elements in the matrix.
+   *
+   * @return Sum of all the elements.
+   */
+  public final double elementSum() {
+    return this.m_storage.elementSum();
+  }
+
+  /**
+   * Computes the trace of the matrix.
+   *
+   * @return The trace of the matrix.
+   */
+  public final double trace() {
+    return this.m_storage.trace();
+  }
+
+  /**
+   * Returns a matrix which is the result of an element by element power of "this" and b.
+   *
+   * <p>c<sub>i,j</sub> = a<sub>i,j</sub> ^ b
+   *
+   * @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));
+  }
+
+  /**
+   * Returns a matrix which is the result of an element by element power of "this" and b.
+   *
+   * <p>c<sub>i,j</sub> = a<sub>i,j</sub> ^ b
+   *
+   * @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));
+  }
+
+  /**
+   * Extracts a given row into a row vector with new underlying storage.
+   *
+   * @param row The row to extract a vector from.
+   * @return A row vector from the given row.
+   */
+  public final Matrix<N1, C> extractRowVector(int row) {
+    return new Matrix<>(this.m_storage.extractVector(true, row));
+  }
+
+  /**
+   * Extracts a given column into a column vector with new underlying storage.
+   *
+   * @param column The column to extract a vector from.
+   * @return A column vector from the given column.
+   */
+  public final Matrix<R, N1> extractColumnVector(int column) {
+    return new Matrix<>(this.m_storage.extractVector(false, column));
+  }
+
+  /**
+   * Extracts a matrix of a given size and start position with new underlying storage.
+   *
+   * @param height The number of rows of the extracted matrix.
+   * @param width The number of columns of the extracted matrix.
+   * @param startingRow The starting row of the extracted matrix.
+   * @param startingCol The starting column of the extracted matrix.
+   * @return The extracted matrix.
+   */
+  public final <R2 extends Num, C2 extends Num> Matrix<R2, C2> block(
+      Nat<R2> height, Nat<C2> width, int startingRow, int startingCol) {
+    return new Matrix<>(
+        this.m_storage.extractMatrix(
+            startingRow,
+            Objects.requireNonNull(height).getNum() + startingRow,
+            startingCol,
+            Objects.requireNonNull(width).getNum() + startingCol));
+  }
+
+  /**
+   * Assign a matrix of a given size and start position.
+   *
+   * @param startingRow The row to start at.
+   * @param startingCol The column to start at.
+   * @param other The matrix to assign the block to.
+   */
+  public <R2 extends Num, C2 extends Num> void assignBlock(
+      int startingRow, int startingCol, Matrix<R2, C2> other) {
+    this.m_storage.insertIntoThis(
+        startingRow, startingCol, Objects.requireNonNull(other).m_storage);
+  }
+
+  /**
+   * Extracts a submatrix from the supplied matrix and inserts it in a submatrix in "this". The
+   * shape of "this" is used to determine the size of the matrix extracted.
+   *
+   * @param startingRow The starting row in the supplied matrix to extract the submatrix.
+   * @param startingCol The starting column in the supplied matrix to extract the submatrix.
+   * @param other The matrix to extract the submatrix from.
+   */
+  public <R2 extends Num, C2 extends Num> void extractFrom(
+      int startingRow, int startingCol, Matrix<R2, C2> other) {
+    CommonOps_DDRM.extract(
+        other.m_storage.getDDRM(), startingRow, startingCol, this.m_storage.getDDRM());
+  }
+
+  /**
+   * 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.
+   * @return The decomposed matrix.
+   * @throws RuntimeException if the matrix could not be decomposed(ie. is not positive
+   *     semidefinite).
+   */
+  @SuppressWarnings("PMD.AvoidThrowingRawExceptionTypes")
+  public Matrix<R, C> lltDecompose(boolean lowerTriangular) {
+    SimpleMatrix temp = m_storage.copy();
+
+    CholeskyDecomposition_F64<DMatrixRMaj> chol =
+        DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
+    if (!chol.decompose(temp.getMatrix())) {
+      // check that the input is not all zeros -- if they are, we special case and return all
+      // zeros.
+      var matData = temp.getDDRM().data;
+      var isZeros = true;
+      for (double matDatum : matData) {
+        isZeros &= Math.abs(matDatum) < 1e-6;
+      }
+      if (isZeros) {
+        return new Matrix<>(new SimpleMatrix(temp.numRows(), temp.numCols()));
+      }
+
+      throw new RuntimeException(
+          "Cholesky decomposition failed! Input matrix:\n" + m_storage.toString());
+    }
+
+    return new Matrix<>(SimpleMatrix.wrap(chol.getT(null)));
+  }
+
+  /**
+   * Returns the row major data of this matrix as a double array.
+   *
+   * @return The row major data of this matrix as a double array.
+   */
+  public double[] getData() {
+    return m_storage.getDDRM().getData();
+  }
+
+  /**
+   * Creates the identity matrix of the given dimension.
+   *
+   * @param dim The dimension of the desired matrix as a {@link Nat}.
+   * @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()));
+  }
+
+  /**
+   * Creates the identity matrix of the given dimension.
+   *
+   * @param dim The dimension of the desired matrix as a {@link Num}.
+   * @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(D dim) {
+    return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
+  }
+
+  /**
+   * Entrypoint to the {@link 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<>(Objects.requireNonNull(rows), Objects.requireNonNull(cols));
+  }
+
+  /**
+   * Reassigns dimensions of a {@link Matrix} to allow for operations with other matrices that have
+   * wildcard dimensions.
+   *
+   * @param mat The {@link Matrix} to remove the dimensions from.
+   * @return The matrix with reassigned dimensions.
+   */
+  public static <R1 extends Num, C1 extends Num> Matrix<R1, C1> changeBoundsUnchecked(
+      Matrix<?, ?> mat) {
+    return new Matrix<>(mat.m_storage);
+  }
+
+  /**
+   * Checks if another {@link Matrix} is identical to "this" one within a specified tolerance.
+   *
+   * <p>This will check if each element is in tolerance of the corresponding element from the other
+   * {@link Matrix} or if the elements have the same symbolic meaning. For two elements to have the
+   * 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
+   * 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.
+   *
+   * @param other The {@link Matrix} to check against this one.
+   * @param tolerance The tolerance to check equality with.
+   * @return true if this matrix is identical to the one supplied.
+   */
+  public boolean isIdentical(Matrix<?, ?> other, double tolerance) {
+    return MatrixFeatures_DDRM.isIdentical(
+        this.m_storage.getDDRM(), other.m_storage.getDDRM(), tolerance);
+  }
+
+  /**
+   * Checks if another {@link Matrix} is equal to "this" within a specified tolerance.
+   *
+   * <p>This will check if each element is in tolerance of the corresponding element from the other
+   * {@link Matrix}.
+   *
+   * <p>tol &ge; |a<sub>ij</sub> - b<sub>ij</sub>|
+   *
+   * @param other The {@link Matrix} to check against this one.
+   * @param tolerance The tolerance to check equality with.
+   * @return true if this matrix is equal to the one supplied.
+   */
+  public boolean isEqual(Matrix<?, ?> other, double tolerance) {
+    return MatrixFeatures_DDRM.isEquals(
+        this.m_storage.getDDRM(), other.m_storage.getDDRM(), tolerance);
+  }
+
+  @Override
+  public String toString() {
+    return m_storage.toString();
+  }
+
+  /**
+   * Checks if an object is equal to this {@link Matrix}.
+   *
+   * <p>a<sub>ij</sub> == b<sub>ij</sub>
+   *
+   * @param other The Object to check against this {@link Matrix}.
+   * @return true if the object supplied is a {@link Matrix} and is equal to this matrix.
+   */
+  @Override
+  public boolean equals(Object other) {
+    if (this == other) {
+      return true;
+    }
+    if (!(other instanceof Matrix)) {
+      return false;
+    }
+
+    Matrix<?, ?> matrix = (Matrix<?, ?>) other;
+    if (MatrixFeatures_DDRM.hasUncountable(matrix.m_storage.getDDRM())) {
+      return false;
+    }
+    return MatrixFeatures_DDRM.isEquals(this.m_storage.getDDRM(), matrix.m_storage.getDDRM());
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_storage);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/MatrixUtils.java

@@ -0,0 +1,80 @@
+// 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);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/Num.java

@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+/** A number expressed as a java class. */
+public abstract class Num {
+  /**
+   * The number this is backing.
+   *
+   * @return The number represented by this class.
+   */
+  public abstract int getNum();
+}

wpimath/src/main/java/edu/wpi/first/math/Pair.java

@@ -0,0 +1,28 @@
+// 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;
+
+public class Pair<A, B> {
+  private final A m_first;
+  private final B m_second;
+
+  public Pair(A first, B second) {
+    m_first = first;
+    m_second = second;
+  }
+
+  public A getFirst() {
+    return m_first;
+  }
+
+  public B getSecond() {
+    return m_second;
+  }
+
+  @SuppressWarnings("ParameterName")
+  public static <A, B> Pair<A, B> of(A a, B b) {
+    return new Pair<>(a, b);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java

@@ -0,0 +1,255 @@
+// 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.function.BiFunction;
+import org.ejml.data.DMatrixRMaj;
+import org.ejml.dense.row.NormOps_DDRM;
+import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
+import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64;
+import org.ejml.simple.SimpleBase;
+import org.ejml.simple.SimpleMatrix;
+
+public final class SimpleMatrixUtils {
+  private SimpleMatrixUtils() {}
+
+  /**
+   * Compute the matrix exponential, e^M of the given matrix.
+   *
+   * @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;
+
+    if (A_L1 < 1.495585217958292e-002) {
+      Pair<SimpleMatrix, SimpleMatrix> pair = _pade3(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+    } else if (A_L1 < 2.539398330063230e-001) {
+      Pair<SimpleMatrix, SimpleMatrix> pair = _pade5(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+    } else if (A_L1 < 9.504178996162932e-001) {
+      Pair<SimpleMatrix, SimpleMatrix> pair = _pade7(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+    } else if (A_L1 < 2.097847961257068e+000) {
+      Pair<SimpleMatrix, SimpleMatrix> pair = _pade9(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, 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);
+    }
+  }
+
+  @SuppressWarnings({"LocalVariableName", "ParameterName", "LineLength"})
+  private static SimpleMatrix dispatchPade(
+      SimpleMatrix U,
+      SimpleMatrix V,
+      int nSquarings,
+      BiFunction<SimpleMatrix, SimpleMatrix, SimpleMatrix> solveProvider) {
+    SimpleMatrix P = U.plus(V);
+    SimpleMatrix Q = U.negative().plus(V);
+
+    SimpleMatrix R = solveProvider.apply(Q, P);
+
+    for (int i = 0; i < nSquarings; i++) {
+      R = R.mult(R);
+    }
+
+    return R;
+  }
+
+  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
+  private static Pair<SimpleMatrix, SimpleMatrix> _pade3(SimpleMatrix A) {
+    double[] b = new double[] {120, 60, 12, 1};
+    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+
+    SimpleMatrix A2 = A.mult(A);
+    SimpleMatrix U = A.mult(A2.mult(ident.scale(b[1]).plus(b[3])));
+    SimpleMatrix V = A2.scale(b[2]).plus(ident.scale(b[0]));
+    return new Pair<>(U, V);
+  }
+
+  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
+  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);
+    SimpleMatrix A4 = A2.mult(A2);
+
+    SimpleMatrix U = A.mult(A4.scale(b[5]).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
+    SimpleMatrix V = A4.scale(b[4]).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
+
+    return new Pair<>(U, V);
+  }
+
+  @SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
+  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);
+    SimpleMatrix A4 = A2.mult(A2);
+    SimpleMatrix A6 = A4.mult(A2);
+
+    SimpleMatrix U =
+        A.mult(A6.scale(b[7]).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
+    SimpleMatrix V =
+        A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
+
+    return new Pair<>(U, V);
+  }
+
+  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName", "LineLength"})
+  private static Pair<SimpleMatrix, SimpleMatrix> _pade9(SimpleMatrix A) {
+    double[] b =
+        new double[] {
+          17643225600.0, 8821612800.0, 2075673600, 302702400, 30270240, 2162160, 110880, 3960, 90, 1
+        };
+    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+    SimpleMatrix A2 = A.mult(A);
+    SimpleMatrix A4 = A2.mult(A2);
+    SimpleMatrix A6 = A4.mult(A2);
+    SimpleMatrix A8 = A6.mult(A2);
+
+    SimpleMatrix U =
+        A.mult(
+            A8.scale(b[9])
+                .plus(A6.scale(b[7]))
+                .plus(A4.scale(b[5]))
+                .plus(A2.scale(b[3]))
+                .plus(ident.scale(b[1])));
+    SimpleMatrix V =
+        A8.scale(b[8])
+            .plus(A6.scale(b[6]))
+            .plus(A4.scale(b[4]))
+            .plus(A2.scale(b[2]))
+            .plus(ident.scale(b[0]));
+
+    return new Pair<>(U, V);
+  }
+
+  @SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
+  private static Pair<SimpleMatrix, SimpleMatrix> _pade13(SimpleMatrix A) {
+    double[] b =
+        new double[] {
+          64764752532480000.0,
+          32382376266240000.0,
+          7771770303897600.0,
+          1187353796428800.0,
+          129060195264000.0,
+          10559470521600.0,
+          670442572800.0,
+          33522128640.0,
+          1323241920,
+          40840800,
+          960960,
+          16380,
+          182,
+          1
+        };
+    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+
+    SimpleMatrix A2 = A.mult(A);
+    SimpleMatrix A4 = A2.mult(A2);
+    SimpleMatrix A6 = A4.mult(A2);
+
+    SimpleMatrix U =
+        A.mult(
+            A6.scale(b[13])
+                .plus(A4.scale(b[11]))
+                .plus(A2.scale(b[9]))
+                .plus(A6.scale(b[7]))
+                .plus(A4.scale(b[5]))
+                .plus(A2.scale(b[3]))
+                .plus(ident.scale(b[1])));
+    SimpleMatrix V =
+        A6.mult(A6.scale(b[12]).plus(A4.scale(b[10])).plus(A2.scale(b[8])))
+            .plus(A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0])));
+
+    return new Pair<>(U, V);
+  }
+
+  private static SimpleMatrix eye(int rows, int cols) {
+    return SimpleMatrix.identity(Math.min(rows, cols));
+  }
+
+  /**
+   * The identy of a square matrix.
+   *
+   * @param rows the number of rows (and columns)
+   * @return the identiy matrix, rows x rows.
+   */
+  public static SimpleMatrix eye(int rows) {
+    return SimpleMatrix.identity(rows);
+  }
+
+  /**
+   * Decompose the given matrix using Cholesky Decomposition and return a view of the upper
+   * triangular matrix (if you want lower triangular see the other overload of this method.) If the
+   * input matrix is zeros, this will return the zero matrix.
+   *
+   * @param src The matrix to decompose.
+   * @return The decomposed matrix.
+   * @throws RuntimeException if the matrix could not be decomposed (ie. is not positive
+   *     semidefinite).
+   */
+  public static SimpleMatrix lltDecompose(SimpleMatrix src) {
+    return lltDecompose(src, false);
+  }
+
+  /**
+   * Decompose the given matrix using Cholesky Decomposition. If the input matrix is zeros, this
+   * will return the zero matrix.
+   *
+   * @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
+   *     semidefinite).
+   */
+  @SuppressWarnings("PMD.AvoidThrowingRawExceptionTypes")
+  public static SimpleMatrix lltDecompose(SimpleMatrix src, boolean lowerTriangular) {
+    SimpleMatrix temp = src.copy();
+
+    CholeskyDecomposition_F64<DMatrixRMaj> chol =
+        DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
+    if (!chol.decompose(temp.getMatrix())) {
+      // check that the input is not all zeros -- if they are, we special case and return all
+      // zeros.
+      var matData = temp.getDDRM().data;
+      var isZeros = true;
+      for (double matDatum : matData) {
+        isZeros &= Math.abs(matDatum) < 1e-6;
+      }
+      if (isZeros) {
+        return new SimpleMatrix(temp.numRows(), temp.numCols());
+      }
+
+      throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n" + src.toString());
+    }
+
+    return SimpleMatrix.wrap(chol.getT(null));
+  }
+
+  /**
+   * Computes the matrix exponential using Eigen's solver.
+   *
+   * @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());
+    return toReturn;
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java

@@ -0,0 +1,188 @@
+// 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 edu.wpi.first.math.numbers.N10;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N4;
+import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.numbers.N6;
+import edu.wpi.first.math.numbers.N7;
+import edu.wpi.first.math.numbers.N8;
+import edu.wpi.first.math.numbers.N9;
+
+/**
+ * A specialization of {@link MatBuilder} for constructing vectors (Nx1 matrices).
+ *
+ * @param <N> The dimension of the vector to be constructed.
+ */
+public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
+  public VecBuilder(Nat<N> rows) {
+    super(rows, Nat.N1());
+  }
+
+  private Vector<N> fillVec(double... data) {
+    return new Vector<>(fill(data));
+  }
+
+  /**
+   * Returns a 1x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   */
+  public static Vector<N1> fill(double n1) {
+    return new VecBuilder<>(Nat.N1()).fillVec(n1);
+  }
+
+  /**
+   * Returns a 2x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   */
+  public static Vector<N2> fill(double n1, double n2) {
+    return new VecBuilder<>(Nat.N2()).fillVec(n1, n2);
+  }
+
+  /**
+   * Returns a 3x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   */
+  public static Vector<N3> fill(double n1, double n2, double n3) {
+    return new VecBuilder<>(Nat.N3()).fillVec(n1, n2, n3);
+  }
+
+  /**
+   * Returns a 4x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   */
+  public static Vector<N4> fill(double n1, double n2, double n3, double n4) {
+    return new VecBuilder<>(Nat.N4()).fillVec(n1, n2, n3, n4);
+  }
+
+  /**
+   * Returns a 5x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   */
+  public static Vector<N5> fill(double n1, double n2, double n3, double n4, double n5) {
+    return new VecBuilder<>(Nat.N5()).fillVec(n1, n2, n3, n4, n5);
+  }
+
+  /**
+   * Returns a 6x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   * @param n6 the sixth element.
+   */
+  public static Vector<N6> fill(double n1, double n2, double n3, double n4, double n5, double n6) {
+    return new VecBuilder<>(Nat.N6()).fillVec(n1, n2, n3, n4, n5, n6);
+  }
+
+  /**
+   * Returns a 7x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   * @param n6 the sixth element.
+   * @param n7 the seventh element.
+   */
+  public static Vector<N7> fill(
+      double n1, double n2, double n3, double n4, double n5, double n6, double n7) {
+    return new VecBuilder<>(Nat.N7()).fillVec(n1, n2, n3, n4, n5, n6, n7);
+  }
+
+  /**
+   * Returns a 8x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   * @param n6 the sixth element.
+   * @param n7 the seventh element.
+   * @param n8 the eighth element.
+   */
+  public static Vector<N8> fill(
+      double n1, double n2, double n3, double n4, double n5, double n6, double n7, double n8) {
+    return new VecBuilder<>(Nat.N8()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8);
+  }
+
+  /**
+   * Returns a 9x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   * @param n6 the sixth element.
+   * @param n7 the seventh element.
+   * @param n8 the eighth element.
+   * @param n9 the ninth element.
+   */
+  public static Vector<N9> fill(
+      double n1,
+      double n2,
+      double n3,
+      double n4,
+      double n5,
+      double n6,
+      double n7,
+      double n8,
+      double n9) {
+    return new VecBuilder<>(Nat.N9()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9);
+  }
+
+  /**
+   * Returns a 10x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   * @param n6 the sixth element.
+   * @param n7 the seventh element.
+   * @param n8 the eighth element.
+   * @param n9 the ninth element.
+   * @param n10 the tenth element.
+   */
+  @SuppressWarnings("PMD.ExcessiveParameterList")
+  public static Vector<N10> fill(
+      double n1,
+      double n2,
+      double n3,
+      double n4,
+      double n5,
+      double n6,
+      double n7,
+      double n8,
+      double n9,
+      double n10) {
+    return new VecBuilder<>(Nat.N10()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/Vector.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;
+
+import edu.wpi.first.math.numbers.N1;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
+ *
+ * <p>This class is intended to be used alongside the state space library.
+ *
+ * @param <R> The number of rows in this matrix.
+ */
+public class Vector<R extends Num> extends Matrix<R, N1> {
+
+  /**
+   * Constructs an empty zero vector of the given dimensions.
+   *
+   * @param rows The number of rows of the vector.
+   */
+  public Vector(Nat<R> rows) {
+    super(rows, Nat.N1());
+  }
+
+  /**
+   * Constructs a new {@link Vector} with the given storage. Caller should make sure that the
+   * provided generic bounds match the shape of the provided {@link Vector}.
+   *
+   * <p>NOTE:It is not recommended to use this constructor unless the {@link SimpleMatrix} API is
+   * absolutely necessary due to the desired function not being accessible through the {@link
+   * Vector} wrapper.
+   *
+   * @param storage The {@link SimpleMatrix} to back this vector.
+   */
+  public Vector(SimpleMatrix storage) {
+    super(storage);
+  }
+
+  /**
+   * Constructs a new vector with the storage of the supplied matrix.
+   *
+   * @param other The {@link Vector} to copy the storage of.
+   */
+  public Vector(Matrix<R, N1> other) {
+    super(other);
+  }
+
+  @Override
+  public Vector<R> times(double value) {
+    return new Vector<>(this.m_storage.scale(value));
+  }
+
+  @Override
+  public Vector<R> div(int value) {
+    return new Vector<>(this.m_storage.divide(value));
+  }
+
+  @Override
+  public Vector<R> div(double value) {
+    return new Vector<>(this.m_storage.divide(value));
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java

@@ -0,0 +1,138 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+/**
+ * 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 kv;
+  public final double ka;
+
+  /**
+   * Creates a new ArmFeedforward with the specified gains. Units of the gain values will dictate
+   * units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kcos The gravity gain.
+   * @param kv The velocity gain.
+   * @param ka The acceleration gain.
+   */
+  public ArmFeedforward(double ks, double kcos, double kv, double ka) {
+    this.ks = ks;
+    this.kcos = kcos;
+    this.kv = kv;
+    this.ka = ka;
+  }
+
+  /**
+   * Creates a new ArmFeedforward with the specified gains. Acceleration gain is defaulted to zero.
+   * Units of the gain values will dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kcos The gravity gain.
+   * @param kv The velocity gain.
+   */
+  public ArmFeedforward(double ks, double kcos, double kv) {
+    this(ks, kcos, kv, 0);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param positionRadians The position (angle) setpoint.
+   * @param velocityRadPerSec The velocity setpoint.
+   * @param accelRadPerSecSquared The acceleration setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(
+      double positionRadians, double velocityRadPerSec, double accelRadPerSecSquared) {
+    return ks * Math.signum(velocityRadPerSec)
+        + kcos * Math.cos(positionRadians)
+        + kv * velocityRadPerSec
+        + ka * accelRadPerSecSquared;
+  }
+
+  /**
+   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
+   * zero).
+   *
+   * @param positionRadians The position (angle) setpoint.
+   * @param velocity The velocity setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double positionRadians, double velocity) {
+    return calculate(positionRadians, velocity, 0);
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply, a position, and an
+   * acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
+   * profile are simultaneously 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 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;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply, a position, and an
+   * acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
+   * profile are simultaneously 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 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;
+  }
+
+  /**
+   * Calculates the maximum achievable acceleration given a maximum voltage supply, a position, and
+   * a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
+   * profile are simultaneously achievable - enter the velocity constraint, and this will give you 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.
+   * @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;
+  }
+
+  /**
+   * Calculates the minimum achievable acceleration given a maximum voltage supply, a position, and
+   * a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
+   * profile are simultaneously achievable - enter the velocity constraint, and this will give you 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.
+   * @return The minimum possible acceleration at the given velocity.
+   */
+  public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) {
+    return maxAchievableAcceleration(-maxVoltage, angle, velocity);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java

@@ -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.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.NumericalJacobian;
+import java.util.function.BiFunction;
+import java.util.function.Function;
+
+/**
+ * Constructs a control-affine plant inversion model-based feedforward from given model dynamics.
+ *
+ * <p>If given the vector valued function as f(x, u) where x is the state vector and u is the input
+ * vector, the B matrix(continuous input matrix) is calculated through a {@link
+ * edu.wpi.first.math.system.NumericalJacobian}. In this case f has to be control-affine (of the
+ * form f(x) + Bu).
+ *
+ * <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (rDot - f(x))</strong>, where
+ * <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
+ *
+ * <p>This feedforward does not account for a dynamic B matrix, B is either determined or supplied
+ * when the feedforward is created and remains constant.
+ *
+ * <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;
+
+  private final double m_dt;
+
+  /** The model dynamics. */
+  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
+
+  /**
+   * Constructs a feedforward with given model dynamics as a function of state and input.
+   *
+   * @param states A {@link Nat} representing the number of states.
+   * @param inputs A {@link Nat} representing the number of inputs.
+   * @param f A vector-valued function of x, the state, and u, the input, that returns the
+   *     derivative of the state vector. HAS to be control-affine (of the form f(x) + Bu).
+   * @param dtSeconds The timestep between calls of calculate().
+   */
+  public ControlAffinePlantInversionFeedforward(
+      Nat<States> states,
+      Nat<Inputs> inputs,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      double dtSeconds) {
+    this.m_dt = dtSeconds;
+    this.m_f = f;
+    this.m_inputs = inputs;
+
+    this.m_B =
+        NumericalJacobian.numericalJacobianU(
+            states, inputs, m_f, new Matrix<>(states, Nat.N1()), new Matrix<>(inputs, Nat.N1()));
+
+    m_r = new Matrix<>(states, Nat.N1());
+    m_uff = new Matrix<>(inputs, Nat.N1());
+
+    reset(m_r);
+  }
+
+  /**
+   * Constructs a feedforward with given model dynamics as a function of state, and the plant's
+   * B(continuous input matrix) matrix.
+   *
+   * @param states A {@link Nat} representing the number of states.
+   * @param inputs A {@link Nat} representing the number of inputs.
+   * @param f A vector-valued function of x, the state, that returns the derivative of the state
+   *     vector.
+   * @param B Continuous input matrix of the plant being controlled.
+   * @param dtSeconds The timestep between calls of calculate().
+   */
+  public ControlAffinePlantInversionFeedforward(
+      Nat<States> states,
+      Nat<Inputs> inputs,
+      Function<Matrix<States, N1>, Matrix<States, N1>> f,
+      Matrix<States, Inputs> B,
+      double dtSeconds) {
+    this.m_dt = dtSeconds;
+    this.m_inputs = inputs;
+
+    this.m_f = (x, u) -> f.apply(x);
+    this.m_B = B;
+
+    m_r = new Matrix<>(states, Nat.N1());
+    m_uff = new Matrix<>(inputs, Nat.N1());
+
+    reset(m_r);
+  }
+
+  /**
+   * Returns the previously calculated feedforward as an input vector.
+   *
+   * @return The calculated feedforward.
+   */
+  public Matrix<Inputs, N1> getUff() {
+    return m_uff;
+  }
+
+  /**
+   * Returns an element of the previously calculated feedforward.
+   *
+   * @param row Row of uff.
+   * @return The row of the calculated feedforward.
+   */
+  public double getUff(int row) {
+    return m_uff.get(row, 0);
+  }
+
+  /**
+   * Returns the current reference vector r.
+   *
+   * @return The current reference vector.
+   */
+  public Matrix<States, N1> getR() {
+    return m_r;
+  }
+
+  /**
+   * Returns an element of the current reference vector r.
+   *
+   * @param row Row of r.
+   * @return The row of the current reference vector.
+   */
+  public double getR(int row) {
+    return m_r.get(row, 0);
+  }
+
+  /**
+   * Resets the feedforward with a specified initial state vector.
+   *
+   * @param initialState The initial state vector.
+   */
+  public void reset(Matrix<States, N1> initialState) {
+    m_r = initialState;
+    m_uff.fill(0.0);
+  }
+
+  /** Resets the feedforward with a zero initial state vector. */
+  public void reset() {
+    m_r.fill(0.0);
+    m_uff.fill(0.0);
+  }
+
+  /**
+   * Calculate the feedforward with only the desired future reference. This uses the internally
+   * stored "current" reference.
+   *
+   * <p>If this method is used the initial state of the system is the one set using {@link
+   * LinearPlantInversionFeedforward#reset(Matrix)}. 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.
+   */
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> nextR) {
+    return calculate(m_r, nextR);
+  }
+
+  /**
+   * Calculate the feedforward with current and future reference vectors.
+   *
+   * @param r The reference state of the current timestep (k).
+   * @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);
+
+    m_uff = m_B.solve(rDot.minus(m_f.apply(r, new Matrix<>(m_inputs, Nat.N1()))));
+
+    m_r = nextR;
+    return m_uff;
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.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;
+
+/**
+ * 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;
+  public final double kv;
+  public final double ka;
+
+  /**
+   * Creates a new ElevatorFeedforward with the specified gains. Units of the gain values will
+   * dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kg The gravity gain.
+   * @param kv The velocity gain.
+   * @param ka The acceleration gain.
+   */
+  public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
+    this.ks = ks;
+    this.kg = kg;
+    this.kv = kv;
+    this.ka = ka;
+  }
+
+  /**
+   * Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is defaulted to
+   * zero. Units of the gain values will dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kg The gravity gain.
+   * @param kv The velocity gain.
+   */
+  public ElevatorFeedforward(double ks, double kg, double kv) {
+    this(ks, kg, kv, 0);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param velocity The velocity setpoint.
+   * @param acceleration The acceleration setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity, double acceleration) {
+    return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration;
+  }
+
+  /**
+   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
+   * zero).
+   *
+   * @param velocity The velocity setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity) {
+    return calculate(velocity, 0);
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply and an acceleration.
+   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
+   * simultaneously 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 elevator.
+   * @param acceleration The acceleration of the elevator.
+   * @return The maximum possible velocity at the given acceleration.
+   */
+  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
+    // Assume max velocity is positive
+    return (maxVoltage - ks - kg - acceleration * ka) / kv;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply and an acceleration.
+   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
+   * simultaneously 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 elevator.
+   * @param acceleration The acceleration of the elevator.
+   * @return The minimum possible velocity at the given acceleration.
+   */
+  public double minAchievableVelocity(double maxVoltage, double acceleration) {
+    // Assume min velocity is negative, ks flips sign
+    return (-maxVoltage + ks - kg - acceleration * ka) / kv;
+  }
+
+  /**
+   * Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity.
+   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
+   * simultaneously achievable - enter the velocity constraint, and this will give you a
+   * simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param velocity The velocity of the elevator.
+   * @return The maximum possible acceleration at the given velocity.
+   */
+  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
+    return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / ka;
+  }
+
+  /**
+   * Calculates the minimum achievable acceleration given a maximum voltage supply and a velocity.
+   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
+   * simultaneously achievable - enter the velocity constraint, and this will give you a
+   * simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param velocity The velocity of the elevator.
+   * @return The minimum possible acceleration at the given velocity.
+   */
+  public double minAchievableAcceleration(double maxVoltage, double velocity) {
+    return maxAchievableAcceleration(-maxVoltage, velocity);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.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.controller;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.math.Discretization;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.LinearSystem;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * Constructs a plant inversion model-based feedforward from a {@link LinearSystem}.
+ *
+ * <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (r_k+1 - A r_k) </strong>,
+ * where <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
+ *
+ * <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;
+
+  /**
+   * Constructs a feedforward with the given plant.
+   *
+   * @param plant The plant being controlled.
+   * @param dtSeconds Discretization timestep.
+   */
+  public LinearPlantInversionFeedforward(
+      LinearSystem<States, Inputs, Outputs> plant, double dtSeconds) {
+    this(plant.getA(), plant.getB(), dtSeconds);
+  }
+
+  /**
+   * Constructs a feedforward with the given coefficients.
+   *
+   * @param A Continuous system matrix of the plant being controlled.
+   * @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);
+    this.m_A = discABPair.getFirst();
+    this.m_B = discABPair.getSecond();
+
+    m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
+    m_uff = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
+
+    reset(m_r);
+  }
+
+  /**
+   * Returns the previously calculated feedforward as an input vector.
+   *
+   * @return The calculated feedforward.
+   */
+  public Matrix<Inputs, N1> getUff() {
+    return m_uff;
+  }
+
+  /**
+   * Returns an element of the previously calculated feedforward.
+   *
+   * @param row Row of uff.
+   * @return The row of the calculated feedforward.
+   */
+  public double getUff(int row) {
+    return m_uff.get(row, 0);
+  }
+
+  /**
+   * Returns the current reference vector r.
+   *
+   * @return The current reference vector.
+   */
+  public Matrix<States, N1> getR() {
+    return m_r;
+  }
+
+  /**
+   * Returns an element of the current reference vector r.
+   *
+   * @param row Row of r.
+   * @return The row of the current reference vector.
+   */
+  public double getR(int row) {
+    return m_r.get(row, 0);
+  }
+
+  /**
+   * Resets the feedforward with a specified initial state vector.
+   *
+   * @param initialState The initial state vector.
+   */
+  public void reset(Matrix<States, N1> initialState) {
+    m_r = initialState;
+    m_uff.fill(0.0);
+  }
+
+  /** Resets the feedforward with a zero initial state vector. */
+  public void reset() {
+    m_r.fill(0.0);
+    m_uff.fill(0.0);
+  }
+
+  /**
+   * Calculate the feedforward with only the desired future reference. This uses the internally
+   * stored "current" reference.
+   *
+   * <p>If this method is used the initial state of the system is the one set using {@link
+   * LinearPlantInversionFeedforward#reset(Matrix)}. 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.
+   */
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> nextR) {
+    return calculate(m_r, nextR);
+  }
+
+  /**
+   * Calculate the feedforward with current and future reference vectors.
+   *
+   * @param r The reference state of the current timestep (k).
+   * @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))));
+
+    m_r = nextR;
+    return m_uff;
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java

@@ -0,0 +1,232 @@
+// 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.Drake;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.math.Discretization;
+import edu.wpi.first.math.math.StateSpaceUtil;
+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 a linear-quadratic regulator (LQR). LQRs use
+ * the control law u = K(r - x).
+ *
+ * <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;
+
+  /**
+   * Constructs a controller with the given coefficients and plant. Rho is defaulted to 1.
+   *
+   * @param plant The plant being controlled.
+   * @param qelms The maximum desired error tolerance for each state.
+   * @param relms The maximum desired control effort for each input.
+   * @param dtSeconds Discretization timestep.
+   */
+  public LinearQuadraticRegulator(
+      LinearSystem<States, Inputs, Outputs> plant,
+      Vector<States> qelms,
+      Vector<Inputs> relms,
+      double dtSeconds) {
+    this(
+        plant.getA(),
+        plant.getB(),
+        StateSpaceUtil.makeCostMatrix(qelms),
+        StateSpaceUtil.makeCostMatrix(relms),
+        dtSeconds);
+  }
+
+  /**
+   * 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 qelms The maximum desired error tolerance for each state.
+   * @param relms The maximum desired control effort for each input.
+   * @param dtSeconds Discretization timestep.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public LinearQuadraticRegulator(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Vector<States> qelms,
+      Vector<Inputs> relms,
+      double dtSeconds) {
+    this(
+        A,
+        B,
+        StateSpaceUtil.makeCostMatrix(qelms),
+        StateSpaceUtil.makeCostMatrix(relms),
+        dtSeconds);
+  }
+
+  /**
+   * 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 dtSeconds Discretization timestep.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public LinearQuadraticRegulator(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R,
+      double dtSeconds) {
+    var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R);
+
+    var temp = discB.transpose().times(S).times(discB).plus(R);
+
+    m_K = temp.solve(discB.transpose().times(S).times(discA));
+
+    m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
+    m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
+
+    reset();
+  }
+
+  /**
+   * 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.
+   */
+  public Matrix<Inputs, N1> getU() {
+    return m_u;
+  }
+
+  /**
+   * Returns an element of the control input vector u.
+   *
+   * @param row Row of u.
+   * @return The row of the control input vector.
+   */
+  public double getU(int row) {
+    return m_u.get(row, 0);
+  }
+
+  /**
+   * Returns the reference vector r.
+   *
+   * @return The reference vector.
+   */
+  public Matrix<States, N1> getR() {
+    return m_r;
+  }
+
+  /**
+   * Returns an element of the reference vector r.
+   *
+   * @param row Row of r.
+   * @return The row of the reference vector.
+   */
+  public double getR(int row) {
+    return m_r.get(row, 0);
+  }
+
+  /**
+   * Returns the controller matrix K.
+   *
+   * @return the controller matrix K.
+   */
+  public Matrix<Inputs, States> getK() {
+    return m_K;
+  }
+
+  /** Resets the controller. */
+  public void reset() {
+    m_r.fill(0.0);
+    m_u.fill(0.0);
+  }
+
+  /**
+   * Returns the next output of the controller.
+   *
+   * @param x The current state x.
+   */
+  @SuppressWarnings("ParameterName")
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> x) {
+    m_u = m_K.times(m_r.minus(x));
+    return m_u;
+  }
+
+  /**
+   * Returns the next output of the controller.
+   *
+   * @param x The current state x.
+   * @param nextR the next reference vector r.
+   */
+  @SuppressWarnings("ParameterName")
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> x, Matrix<States, N1> nextR) {
+    m_r = nextR;
+    return calculate(x);
+  }
+
+  /**
+   * Adjusts LQR controller gain to compensate for a pure time delay in the input.
+   *
+   * <p>Linear-Quadratic regulator controller gains tend to be aggressive. If sensor measurements
+   * are time-delayed too long, the LQR may be unstable. However, if we know the amount of delay, we
+   * can compute the control based on where the system will be after the time delay.
+   *
+   * <p>See https://file.tavsys.net/control/controls-engineering-in-frc.pdf appendix C.4 for a
+   * derivation.
+   *
+   * @param plant The plant being controlled.
+   * @param dtSeconds Discretization timestep in seconds.
+   * @param inputDelaySeconds Input time delay in seconds.
+   */
+  public void latencyCompensate(
+      LinearSystem<States, Inputs, Outputs> plant, double dtSeconds, double inputDelaySeconds) {
+    var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dtSeconds);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    m_K = m_K.times((discA.minus(discB.times(m_K))).pow(inputDelaySeconds / dtSeconds));
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java

@@ -0,0 +1,166 @@
+// 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.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.Trajectory;
+
+/**
+ * Ramsete is a nonlinear time-varying feedback controller for unicycle models that drives the model
+ * to a desired pose along a two-dimensional trajectory. Why would we need a nonlinear control law
+ * in addition to the linear ones we have used so far like PID? If we use the original approach with
+ * PID controllers for left and right position and velocity states, the controllers only deal with
+ * the local pose. If the robot deviates from the path, there is no way for the controllers to
+ * correct and the robot may not reach the desired global pose. This is due to multiple endpoints
+ * existing for the robot which have the same encoder path arc lengths.
+ *
+ * <p>Instead of using wheel path arc lengths (which are in the robot's local coordinate frame),
+ * nonlinear controllers like pure pursuit and Ramsete use global pose. The controller uses this
+ * extra information to guide a linear reference tracker like the PID controllers back in by
+ * adjusting the references of the PID controllers.
+ *
+ * <p>The paper "Control of Wheeled Mobile Robots: An Experimental Overview" describes a nonlinear
+ * controller for a wheeled vehicle with unicycle-like kinematics; a global pose consisting of x, y,
+ * and theta; and a desired pose consisting of x_d, y_d, and theta_d. We call it Ramsete because
+ * that's the acronym for the title of the book it came from in Italian ("Robotica Articolata e
+ * Mobile per i SErvizi e le TEcnologie").
+ *
+ * <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">Controls
+ * Engineering in the FIRST Robotics Competition</a> section on Ramsete unicycle controller for a
+ * 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();
+  private Pose2d m_poseTolerance = new Pose2d();
+  private boolean m_enabled = true;
+
+  /**
+   * Construct a Ramsete unicycle controller.
+   *
+   * @param b Tuning parameter (b &gt; 0) for which larger values make convergence more aggressive
+   *     like a proportional term.
+   * @param zeta Tuning parameter (0 &lt; zeta &lt; 1) for which larger values provide more damping
+   *     in response.
+   */
+  @SuppressWarnings("ParameterName")
+  public RamseteController(double b, double zeta) {
+    m_b = b;
+    m_zeta = zeta;
+  }
+
+  /**
+   * Construct a Ramsete unicycle controller. The default arguments for b and zeta of 2.0 and 0.7
+   * have been well-tested to produce desirable results.
+   */
+  public RamseteController() {
+    this(2.0, 0.7);
+  }
+
+  /** Returns 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 next output of the Ramsete 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 linearVelocityRefMeters The desired linear velocity in meters per second.
+   * @param angularVelocityRefRadiansPerSecond The desired angular velocity in radians per second.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public ChassisSpeeds calculate(
+      Pose2d currentPose,
+      Pose2d poseRef,
+      double linearVelocityRefMeters,
+      double angularVelocityRefRadiansPerSecond) {
+    if (!m_enabled) {
+      return new ChassisSpeeds(linearVelocityRefMeters, 0.0, angularVelocityRefRadiansPerSecond);
+    }
+
+    m_poseError = poseRef.relativeTo(currentPose);
+
+    // Aliases for equation readability
+    final double eX = m_poseError.getX();
+    final double eY = m_poseError.getY();
+    final double eTheta = m_poseError.getRotation().getRadians();
+    final double vRef = linearVelocityRefMeters;
+    final double omegaRef = angularVelocityRefRadiansPerSecond;
+
+    double k = 2.0 * m_zeta * Math.sqrt(Math.pow(omegaRef, 2) + m_b * Math.pow(vRef, 2));
+
+    return new ChassisSpeeds(
+        vRef * m_poseError.getRotation().getCos() + k * eX,
+        0.0,
+        omegaRef + k * eTheta + m_b * vRef * sinc(eTheta) * eY);
+  }
+
+  /**
+   * Returns the next output of the Ramsete 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.
+   */
+  @SuppressWarnings("LocalVariableName")
+  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;
+  }
+
+  /**
+   * Returns sin(x) / x.
+   *
+   * @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;
+    } else {
+      return Math.sin(x) / x;
+    }
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java

@@ -0,0 +1,121 @@
+// 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;
+
+/** 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;
+  public final double ka;
+
+  /**
+   * Creates a new SimpleMotorFeedforward with the specified gains. Units of the gain values will
+   * dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kv The velocity gain.
+   * @param ka The acceleration gain.
+   */
+  public SimpleMotorFeedforward(double ks, double kv, double ka) {
+    this.ks = ks;
+    this.kv = kv;
+    this.ka = ka;
+  }
+
+  /**
+   * Creates a new SimpleMotorFeedforward with the specified gains. Acceleration gain is defaulted
+   * to zero. Units of the gain values will dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kv The velocity gain.
+   */
+  public SimpleMotorFeedforward(double ks, double kv) {
+    this(ks, kv, 0);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param velocity The velocity setpoint.
+   * @param acceleration The acceleration setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity, double acceleration) {
+    return ks * Math.signum(velocity) + kv * velocity + ka * acceleration;
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
+   * zero).
+   *
+   * @param velocity The velocity setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity) {
+    return calculate(velocity, 0);
+  }
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply and an acceleration.
+   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
+   * simultaneously 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 motor.
+   * @param acceleration The acceleration of the motor.
+   * @return The maximum possible velocity at the given acceleration.
+   */
+  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
+    // Assume max velocity is positive
+    return (maxVoltage - ks - acceleration * ka) / kv;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply and an acceleration.
+   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
+   * simultaneously 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 motor.
+   * @param acceleration The acceleration of the motor.
+   * @return The minimum possible velocity at the given acceleration.
+   */
+  public double minAchievableVelocity(double maxVoltage, double acceleration) {
+    // Assume min velocity is negative, ks flips sign
+    return (-maxVoltage + ks - acceleration * ka) / kv;
+  }
+
+  /**
+   * Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity.
+   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
+   * simultaneously achievable - enter the velocity constraint, and this will give you a
+   * simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param velocity The velocity of the motor.
+   * @return The maximum possible acceleration at the given velocity.
+   */
+  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
+    return (maxVoltage - ks * Math.signum(velocity) - velocity * kv) / ka;
+  }
+
+  /**
+   * Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity.
+   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
+   * simultaneously achievable - enter the velocity constraint, and this will give you a
+   * simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param velocity The velocity of the motor.
+   * @return The minimum possible acceleration at the given velocity.
+   */
+  public double minAchievableAcceleration(double maxVoltage, double velocity) {
+    return maxAchievableAcceleration(-maxVoltage, velocity);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/estimator/AngleStatistics.java

@@ -0,0 +1,111 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+import java.util.function.BiFunction;
+import org.ejml.simple.SimpleMatrix;
+
+public final class AngleStatistics {
+  private AngleStatistics() {
+    // Utility class
+  }
+
+  /**
+   * Subtracts a and b while normalizing the resulting value in the selected row as if it were an
+   * angle.
+   *
+   * @param a A vector to subtract from.
+   * @param b A vector to subtract with.
+   * @param angleStateIdx The row containing angles to be normalized.
+   */
+  public static <S extends Num> Matrix<S, N1> angleResidual(
+      Matrix<S, N1> a, Matrix<S, N1> b, int angleStateIdx) {
+    Matrix<S, N1> ret = a.minus(b);
+    ret.set(angleStateIdx, 0, MathUtil.angleModulus(ret.get(angleStateIdx, 0)));
+
+    return ret;
+  }
+
+  /**
+   * Returns a function that subtracts two vectors while normalizing the resulting value in the
+   * selected row as if it were an angle.
+   *
+   * @param angleStateIdx The row containing angles to be normalized.
+   */
+  public static <S extends Num>
+      BiFunction<Matrix<S, N1>, Matrix<S, N1>, Matrix<S, N1>> angleResidual(int angleStateIdx) {
+    return (a, b) -> angleResidual(a, b, angleStateIdx);
+  }
+
+  /**
+   * Adds a and b while normalizing the resulting value in the selected row as an angle.
+   *
+   * @param a A vector to add with.
+   * @param b A vector to add with.
+   * @param angleStateIdx The row containing angles to be normalized.
+   */
+  public static <S extends Num> Matrix<S, N1> angleAdd(
+      Matrix<S, N1> a, Matrix<S, N1> b, int angleStateIdx) {
+    Matrix<S, N1> ret = a.plus(b);
+    ret.set(angleStateIdx, 0, MathUtil.angleModulus(ret.get(angleStateIdx, 0)));
+
+    return ret;
+  }
+
+  /**
+   * Returns a function that adds two vectors while normalizing the resulting value in the selected
+   * row as an angle.
+   *
+   * @param angleStateIdx The row containing angles to be normalized.
+   */
+  public static <S extends Num> BiFunction<Matrix<S, N1>, Matrix<S, N1>, Matrix<S, N1>> angleAdd(
+      int angleStateIdx) {
+    return (a, b) -> angleAdd(a, b, angleStateIdx);
+  }
+
+  /**
+   * Computes the mean of sigmas with the weights Wm while computing a special angle mean for a
+   * select row.
+   *
+   * @param sigmas Sigma points.
+   * @param Wm Weights for the mean.
+   * @param angleStateIdx The row containing the angles.
+   */
+  @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();
+    Matrix<N1, ?> sinAngleSigmas = new Matrix<>(new SimpleMatrix(1, sigmas.getNumCols()));
+    Matrix<N1, ?> cosAngleSigmas = new Matrix<>(new SimpleMatrix(1, sigmas.getNumCols()));
+    for (int i = 0; i < angleSigmas.length; i++) {
+      sinAngleSigmas.set(0, i, Math.sin(angleSigmas[i]));
+      cosAngleSigmas.set(0, i, Math.cos(angleSigmas[i]));
+    }
+
+    double sumSin = sinAngleSigmas.times(Matrix.changeBoundsUnchecked(Wm)).elementSum();
+    double sumCos = cosAngleSigmas.times(Matrix.changeBoundsUnchecked(Wm)).elementSum();
+
+    Matrix<S, N1> ret = sigmas.times(Matrix.changeBoundsUnchecked(Wm));
+    ret.set(angleStateIdx, 0, Math.atan2(sumSin, sumCos));
+
+    return ret;
+  }
+
+  /**
+   * Returns a function that computes the mean of sigmas with the weights Wm while computing a
+   * special angle mean for a select row.
+   *
+   * @param angleStateIdx The row containing the angles.
+   */
+  @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);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.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.estimator;
+
+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.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.math.math.Discretization;
+import edu.wpi.first.math.math.StateSpaceUtil;
+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.wpiutil.WPIUtilJNI;
+import java.util.function.BiConsumer;
+
+/**
+ * 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
+ * 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>Our state-space system is:
+ *
+ * <p><strong> x = [[x, y, theta, dist_l, dist_r]]^T </strong> in the field coordinate system
+ * (dist_* are wheel distances.)
+ *
+ * <p><strong> u = [[vx, vy, omega]]^T </strong> (robot-relative velocities) -- 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]]^T </strong> from vision, or <strong>y = [[dist_l, dist_r, theta]]
+ * </strong> from encoders and gyro.
+ */
+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 double m_nominalDt; // Seconds
+  private double m_prevTimeSeconds = -1.0;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  private Matrix<N3, N3> m_visionDiscreteR;
+
+  /**
+   * 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]^T,
+   *     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]^T, 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]^T, with units in meters and radians.
+   */
+  public DifferentialDrivePoseEstimator(
+      Rotation2d gyroAngle,
+      Pose2d initialPoseMeters,
+      Matrix<N5, N1> stateStdDevs,
+      Matrix<N3, N1> localMeasurementStdDevs,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    this(
+        gyroAngle,
+        initialPoseMeters,
+        stateStdDevs,
+        localMeasurementStdDevs,
+        visionMeasurementStdDevs,
+        0.02);
+  }
+
+  /**
+   * 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]^T,
+   *     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]^T, 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]^T, with units in meters and radians.
+   * @param nominalDtSeconds The time in seconds between each robot loop.
+   */
+  @SuppressWarnings("ParameterName")
+  public DifferentialDrivePoseEstimator(
+      Rotation2d gyroAngle,
+      Pose2d initialPoseMeters,
+      Matrix<N5, N1> stateStdDevs,
+      Matrix<N3, N1> localMeasurementStdDevs,
+      Matrix<N3, N1> visionMeasurementStdDevs,
+      double nominalDtSeconds) {
+    m_nominalDt = nominalDtSeconds;
+
+    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<>();
+
+    // 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_visionDiscreteR,
+                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));
+  }
+
+  /**
+   * Sets the pose estimator's trust of global measurements. This might be used to change trust in
+   * vision measurements after the autonomous period, or to change trust as distance to a vision
+   * target increases.
+   *
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]^T, with units in meters and radians.
+   */
+  public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
+    var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
+    m_visionDiscreteR = Discretization.discretizeR(visionContR, m_nominalDt);
+  }
+
+  @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)));
+  }
+
+  /**
+   * 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.
+   */
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+    m_previousAngle = poseMeters.getRotation();
+    m_observer.setXhat(fillStateVector(poseMeters, 0.0, 0.0));
+    m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
+  }
+
+  /**
+   * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
+   *
+   * @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)));
+  }
+
+  /**
+   * Add a vision measurement to the Unscented 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.
+   *
+   * @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.
+   */
+  public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+    m_latencyCompensator.applyPastGlobalMeasurement(
+        Nat.N3(),
+        m_observer,
+        m_nominalDt,
+        StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
+        m_visionCorrect,
+        timestampSeconds);
+  }
+
+  /**
+   * Add a vision measurement to the Unscented 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>Note that the vision measurement standard deviations passed into this method will continue
+   * to apply to future measurements until a subsequent call to {@link
+   * DifferentialDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
+   *
+   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
+   *     don't use your own time source by calling {@link
+   *     DifferentialDrivePoseEstimator#updateWithTime} 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]^T, with units in meters and radians.
+   */
+  public void addVisionMeasurement(
+      Pose2d visionRobotPoseMeters,
+      double timestampSeconds,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder information. Note that 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}.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose2d update(
+      Rotation2d gyroAngle,
+      DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond,
+      double distanceLeftMeters,
+      double distanceRightMeters) {
+    return updateWithTime(
+        WPIUtilJNI.now() * 1.0e-6,
+        gyroAngle,
+        wheelVelocitiesMetersPerSecond,
+        distanceLeftMeters,
+        distanceRightMeters);
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder information. Note that 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}.
+   * @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);
+
+    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);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java

@@ -0,0 +1,360 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.Drake;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.math.Discretization;
+import edu.wpi.first.math.math.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.NumericalJacobian;
+import java.util.function.BiFunction;
+
+/**
+ * Kalman filters combine predictions from a model and measurements to give an estimate of the true
+ * system state. This is useful because many states cannot be measured directly as a result of
+ * sensor noise, or because the state is "hidden".
+ *
+ * <p>The Extended Kalman filter is just like the {@link KalmanFilter Kalman filter}, but we make a
+ * linear approximation of nonlinear dynamics and/or nonlinear measurement models. This means that
+ * the EKF works with nonlinear systems.
+ */
+@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;
+  private BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> m_addFuncX;
+
+  private final Matrix<States, States> m_contQ;
+  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;
+
+  /**
+   * Constructs an extended Kalman filter.
+   *
+   * @param states a Nat representing the number of states.
+   * @param inputs a Nat representing the number of inputs.
+   * @param outputs a Nat representing the number of outputs.
+   * @param f A vector-valued function of x and u that returns the derivative of the state vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param stateStdDevs Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dtSeconds Nominal discretization timestep.
+   */
+  @SuppressWarnings("ParameterName")
+  public ExtendedKalmanFilter(
+      Nat<States> states,
+      Nat<Inputs> inputs,
+      Nat<Outputs> outputs,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
+      Matrix<States, N1> stateStdDevs,
+      Matrix<Outputs, N1> measurementStdDevs,
+      double dtSeconds) {
+    this(
+        states,
+        inputs,
+        outputs,
+        f,
+        h,
+        stateStdDevs,
+        measurementStdDevs,
+        Matrix::minus,
+        Matrix::plus,
+        dtSeconds);
+  }
+
+  /**
+   * Constructs an extended Kalman filter.
+   *
+   * @param states a Nat representing the number of states.
+   * @param inputs a Nat representing the number of inputs.
+   * @param outputs a Nat representing the number of outputs.
+   * @param f A vector-valued function of x and u that returns the derivative of the state vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param stateStdDevs Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @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.
+   * @param dtSeconds Nominal discretization timestep.
+   */
+  @SuppressWarnings({"ParameterName", "PMD.ExcessiveParameterList"})
+  public ExtendedKalmanFilter(
+      Nat<States> states,
+      Nat<Inputs> inputs,
+      Nat<Outputs> outputs,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
+      Matrix<States, N1> stateStdDevs,
+      Matrix<Outputs, N1> measurementStdDevs,
+      BiFunction<Matrix<Outputs, N1>, Matrix<Outputs, N1>, Matrix<Outputs, N1>> residualFuncY,
+      BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX,
+      double dtSeconds) {
+    m_states = states;
+    m_outputs = outputs;
+
+    m_f = f;
+    m_h = h;
+
+    m_residualFuncY = residualFuncY;
+    m_addFuncX = addFuncX;
+
+    m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
+    this.m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+    m_dtSeconds = dtSeconds;
+
+    reset();
+
+    final var contA =
+        NumericalJacobian.numericalJacobianX(
+            states, states, f, m_xHat, new Matrix<>(inputs, Nat.N1()));
+    final var C =
+        NumericalJacobian.numericalJacobianX(
+            outputs, states, h, m_xHat, new Matrix<>(inputs, Nat.N1()));
+
+    final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
+    final var discA = discPair.getFirst();
+    final var discQ = discPair.getSecond();
+
+    final var discR = Discretization.discretizeR(m_contR, dtSeconds);
+
+    // IsStabilizable(A^T, C^T) will tell us if the system is observable.
+    boolean isObservable = StateSpaceUtil.isStabilizable(discA.transpose(), C.transpose());
+    if (isObservable && outputs.getNum() <= states.getNum()) {
+      m_initP =
+          Drake.discreteAlgebraicRiccatiEquation(discA.transpose(), C.transpose(), discQ, discR);
+    } else {
+      m_initP = new Matrix<>(states, states);
+    }
+
+    m_P = m_initP;
+  }
+
+  /**
+   * Returns the error covariance matrix P.
+   *
+   * @return the error covariance matrix P.
+   */
+  @Override
+  public Matrix<States, States> getP() {
+    return m_P;
+  }
+
+  /**
+   * Returns an element of the error covariance matrix P.
+   *
+   * @param row Row of P.
+   * @param col Column of P.
+   * @return the value of the error covariance matrix P at (i, j).
+   */
+  @Override
+  public double getP(int row, int col) {
+    return m_P.get(row, col);
+  }
+
+  /**
+   * Sets the entire error covariance matrix P.
+   *
+   * @param newP The new value of P to use.
+   */
+  @Override
+  public void setP(Matrix<States, States> newP) {
+    m_P = newP;
+  }
+
+  /**
+   * Returns the state estimate x-hat.
+   *
+   * @return the state estimate x-hat.
+   */
+  @Override
+  public Matrix<States, N1> getXhat() {
+    return m_xHat;
+  }
+
+  /**
+   * 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.
+   */
+  @Override
+  public double getXhat(int row) {
+    return m_xHat.get(row, 0);
+  }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xHat The state estimate x-hat.
+   */
+  @SuppressWarnings("ParameterName")
+  @Override
+  public void setXhat(Matrix<States, N1> xHat) {
+    m_xHat = xHat;
+  }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  @Override
+  public void setXhat(int row, double value) {
+    m_xHat.set(row, 0, value);
+  }
+
+  @Override
+  public void reset() {
+    m_xHat = new Matrix<>(m_states, Nat.N1());
+    m_P = m_initP;
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @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);
+  }
+
+  /**
+   * 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 dtSeconds Timestep for prediction.
+   */
+  @SuppressWarnings("ParameterName")
+  public void predict(
+      Matrix<Inputs, N1> u,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      double dtSeconds) {
+    // Find continuous A
+    final var contA = NumericalJacobian.numericalJacobianX(m_states, m_states, f, m_xHat, u);
+
+    // Find discrete A and Q
+    final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
+    final var discA = discPair.getFirst();
+    final var discQ = discPair.getSecond();
+
+    m_xHat = NumericalIntegration.rk4(f, m_xHat, u, dtSeconds);
+    m_P = discA.times(m_P).times(discA.transpose()).plus(discQ);
+    m_dtSeconds = dtSeconds;
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @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);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
+   * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
+   * of this function).
+   *
+   * @param <Rows> Number of rows in the result of f(x, u).
+   * @param rows Number of rows in the result of f(x, u).
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param R Discrete 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);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
+   * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
+   * of this function).
+   *
+   * @param <Rows> Number of rows in the result of f(x, u).
+   * @param rows Number of rows in the result of f(x, u).
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param R Discrete 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,
+      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 S = C.times(m_P).times(C.transpose()).plus(discR);
+
+    // We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
+    // efficiently.
+    //
+    // K = PC^T S^-1
+    // KS = PC^T
+    // (KS)^T = (PC^T)^T
+    // S^T K^T = CP^T
+    //
+    // The solution of Ax = b can be found via x = A.solve(b).
+    //
+    // K^T = S^T.solve(CP^T)
+    // K = (S^T.solve(CP^T))^T
+    //
+    // Now we have the Kalman gain
+    final Matrix<States, Rows> K = S.transpose().solve(C.times(m_P.transpose())).transpose();
+
+    m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, h.apply(m_xHat, u))));
+    m_P = Matrix.eye(m_states).minus(K.times(C)).times(m_P);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java

@@ -0,0 +1,199 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.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.math.Discretization;
+import edu.wpi.first.math.math.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.LinearSystem;
+
+/**
+ * A Kalman filter combines predictions from a model and measurements to give an estimate of the
+ * true system state. This is useful because many states cannot be measured directly as a result of
+ * sensor noise, or because the state is "hidden".
+ *
+ * <p>Kalman filters use a K gain matrix to determine whether to trust the model or measurements
+ * more. Kalman filter theory uses statistics to compute an optimal K gain which minimizes the sum
+ * of squares error in the state estimate. This K gain is used to correct the state estimate by some
+ * amount of the difference between the actual measurements and the measurements predicted by the
+ * model.
+ *
+ * <p>For more on the underlying math, read
+ * 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;
+
+  /**
+   * Constructs a state-space observer with the given plant.
+   *
+   * @param states A Nat representing the states of the system.
+   * @param outputs A Nat representing the outputs of the system.
+   * @param plant The plant used for the prediction step.
+   * @param stateStdDevs Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dtSeconds Nominal discretization timestep.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public KalmanFilter(
+      Nat<States> states,
+      Nat<Outputs> outputs,
+      LinearSystem<States, Inputs, Outputs> plant,
+      Matrix<States, N1> stateStdDevs,
+      Matrix<Outputs, N1> measurementStdDevs,
+      double dtSeconds) {
+    this.m_states = states;
+
+    this.m_plant = plant;
+
+    var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
+    var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+
+    var pair = Discretization.discretizeAQTaylor(plant.getA(), contQ, dtSeconds);
+    var discA = pair.getFirst();
+    var discQ = pair.getSecond();
+
+    var discR = Discretization.discretizeR(contR, dtSeconds);
+
+    var C = plant.getC();
+
+    // isStabilizable(A^T, C^T) will tell us if the system is observable.
+    var isObservable = StateSpaceUtil.isStabilizable(discA.transpose(), C.transpose());
+    if (!isObservable) {
+      MathSharedStore.reportError(
+          "The system passed to the Kalman filter is not observable!",
+          Thread.currentThread().getStackTrace());
+      throw new IllegalArgumentException(
+          "The system passed to the Kalman filter is not observable!");
+    }
+
+    var P =
+        new Matrix<>(
+            Drake.discreteAlgebraicRiccatiEquation(discA.transpose(), C.transpose(), discQ, discR));
+
+    var S = C.times(P).times(C.transpose()).plus(discR);
+
+    // We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
+    // efficiently.
+    //
+    // K = PC^T S^-1
+    // KS = PC^T
+    // (KS)^T = (PC^T)^T
+    // S^T K^T = CP^T
+    //
+    // The solution of Ax = b can be found via x = A.solve(b).
+    //
+    // K^T = S^T.solve(CP^T)
+    // K = (S^T.solve(CP^T))^T
+    m_K =
+        new Matrix<>(
+            S.transpose().getStorage().solve((C.times(P.transpose())).getStorage()).transpose());
+
+    reset();
+  }
+
+  public void reset() {
+    m_xHat = new Matrix<>(m_states, Nat.N1());
+  }
+
+  /**
+   * Returns the steady-state Kalman gain matrix K.
+   *
+   * @return The steady-state Kalman gain matrix K.
+   */
+  public Matrix<States, Outputs> getK() {
+    return m_K;
+  }
+
+  /**
+   * Returns an element of the steady-state Kalman gain matrix K.
+   *
+   * @param row Row of K.
+   * @param col Column of K.
+   * @return the element (i, j) of the steady-state Kalman gain matrix K.
+   */
+  public double getK(int row, int col) {
+    return m_K.get(row, col);
+  }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xhat The state estimate x-hat.
+   */
+  public void setXhat(Matrix<States, N1> xhat) {
+    this.m_xHat = xhat;
+  }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  public void setXhat(int row, double value) {
+    m_xHat.set(row, 0, value);
+  }
+
+  /**
+   * Returns the state estimate x-hat.
+   *
+   * @return The state estimate x-hat.
+   */
+  public Matrix<States, N1> getXhat() {
+    return m_xHat;
+  }
+
+  /**
+   * Returns an element of the state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @return the state estimate x-hat at i.
+   */
+  public double getXhat(int row) {
+    return m_xHat.get(row, 0);
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @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);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @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();
+    m_xHat = m_xHat.plus(m_K.times(y.minus(C.times(m_xHat).plus(D.times(u)))));
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.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.estimator;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Map;
+import java.util.function.BiConsumer;
+
+public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O extends Num> {
+  private static final int kMaxPastObserverStates = 300;
+
+  private final List<Map.Entry<Double, ObserverSnapshot>> m_pastObserverSnapshots;
+
+  KalmanFilterLatencyCompensator() {
+    m_pastObserverSnapshots = new ArrayList<>();
+  }
+
+  /**
+   * Add past observer states to the observer snapshots list.
+   *
+   * @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.
+   */
+  @SuppressWarnings("ParameterName")
+  public void addObserverState(
+      KalmanTypeFilter<S, I, O> observer,
+      Matrix<I, N1> u,
+      Matrix<O, N1> localY,
+      double timestampSeconds) {
+    m_pastObserverSnapshots.add(
+        Map.entry(timestampSeconds, new ObserverSnapshot(observer, u, localY)));
+
+    if (m_pastObserverSnapshots.size() > kMaxPastObserverStates) {
+      m_pastObserverSnapshots.remove(0);
+    }
+  }
+
+  /**
+   * Add past global measurements (such as from vision)to the estimator.
+   *
+   * @param <R> The rows in the global measurement vector.
+   * @param rows The rows in the global measurement vector.
+   * @param observer The observer to apply the past global measurement.
+   * @param nominalDtSeconds The nominal timestep.
+   * @param globalMeasurement The measurement.
+   * @param globalMeasurementCorrect The function take calls correct() on the observer.
+   * @param globalMeasurementTimestampSeconds The timestamp of the measurement.
+   */
+  @SuppressWarnings({"ParameterName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  public <R extends Num> void applyPastGlobalMeasurement(
+      Nat<R> rows,
+      KalmanTypeFilter<S, I, O> observer,
+      double nominalDtSeconds,
+      Matrix<R, N1> globalMeasurement,
+      BiConsumer<Matrix<I, N1>, Matrix<R, N1>> globalMeasurementCorrect,
+      double globalMeasurementTimestampSeconds) {
+    if (m_pastObserverSnapshots.isEmpty()) {
+      // State map was empty, which means that we got a past measurement right at startup. The only
+      // thing we can really do is ignore the measurement.
+      return;
+    }
+
+    // This index starts at one because we use the previous state later on, and we always want to
+    // have a "previous state".
+    int maxIdx = m_pastObserverSnapshots.size() - 1;
+    int low = 1;
+    int high = Math.max(maxIdx, 1);
+
+    while (low != high) {
+      int mid = (low + high) / 2;
+      if (m_pastObserverSnapshots.get(mid).getKey() < globalMeasurementTimestampSeconds) {
+        // This index and everything under it are less than the requested timestamp. Therefore, we
+        // can discard them.
+        low = mid + 1;
+      } else {
+        // t is at least as large as the element at this index. This means that anything after it
+        // cannot be what we are looking for.
+        high = mid;
+      }
+    }
+
+    // We are simply assigning this index to a new variable to avoid confusion
+    // with variable names.
+    int index = low;
+    double timestamp = globalMeasurementTimestampSeconds;
+    int indexOfClosestEntry =
+        Math.abs(timestamp - m_pastObserverSnapshots.get(index - 1).getKey())
+                <= Math.abs(
+                    timestamp - m_pastObserverSnapshots.get(Math.min(index, maxIdx)).getKey())
+            ? index - 1
+            : index;
+
+    double lastTimestamp =
+        m_pastObserverSnapshots.get(indexOfClosestEntry).getKey() - nominalDtSeconds;
+
+    // We will now go back in time to the state of the system at the time when
+    // the measurement was captured. We will reset the observer to that state,
+    // and apply correction based on the measurement. Then, we will go back
+    // through all observer states until the present and apply past inputs to
+    // get the present estimated state.
+    for (int i = indexOfClosestEntry; i < m_pastObserverSnapshots.size(); i++) {
+      var key = m_pastObserverSnapshots.get(i).getKey();
+      var snapshot = m_pastObserverSnapshots.get(i).getValue();
+
+      if (i == indexOfClosestEntry) {
+        observer.setP(snapshot.errorCovariances);
+        observer.setXhat(snapshot.xHat);
+      }
+
+      observer.predict(snapshot.inputs, key - lastTimestamp);
+      observer.correct(snapshot.inputs, snapshot.localMeasurements);
+
+      if (i == indexOfClosestEntry) {
+        // Note that the measurement is at a timestep close but probably not exactly equal to the
+        // timestep for which we called predict.
+        // This makes the assumption that the dt is small enough that the difference between the
+        // measurement time and the time that the inputs were captured at is very small.
+        globalMeasurementCorrect.accept(snapshot.inputs, globalMeasurement);
+      }
+      lastTimestamp = key;
+
+      m_pastObserverSnapshots.set(
+          i,
+          Map.entry(
+              key, new ObserverSnapshot(observer, snapshot.inputs, snapshot.localMeasurements)));
+    }
+  }
+
+  /** 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();
+      this.errorCovariances = observer.getP();
+
+      inputs = u;
+      localMeasurements = localY;
+    }
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.Matrix;
+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();
+
+  double getP(int i, int j);
+
+  void setP(Matrix<States, States> newP);
+
+  Matrix<States, N1> getXhat();
+
+  double getXhat(int i);
+
+  void setXhat(Matrix<States, N1> xHat);
+
+  void setXhat(int i, double value);
+
+  void reset();
+
+  void predict(Matrix<Inputs, N1> u, double dtSeconds);
+
+  void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y);
+}

wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java

@@ -0,0 +1,299 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.math.math.Discretization;
+import edu.wpi.first.math.math.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpiutil.WPIUtilJNI;
+import java.util.function.BiConsumer;
+
+/**
+ * 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}.
+ *
+ * <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#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>Our state-space system is:
+ *
+ * <p><strong> x = [[x, y, theta]]^T </strong> in the field-coordinate system.
+ *
+ * <p><strong> u = [[vx, vy, theta]]^T </strong> in the field-coordinate system.
+ *
+ * <p><strong> y = [[x, y, theta]]^T </strong> in field coords from vision, or <strong> y =
+ * [[theta]]^T </strong> from the gyro.
+ */
+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 double m_nominalDt; // Seconds
+  private double m_prevTimeSeconds = -1.0;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  private Matrix<N3, N3> m_visionDiscreteR;
+
+  /**
+   * 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]^T, 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]^T, with units in meters and radians.
+   */
+  public MecanumDrivePoseEstimator(
+      Rotation2d gyroAngle,
+      Pose2d initialPoseMeters,
+      MecanumDriveKinematics kinematics,
+      Matrix<N3, N1> stateStdDevs,
+      Matrix<N1, N1> localMeasurementStdDevs,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    this(
+        gyroAngle,
+        initialPoseMeters,
+        kinematics,
+        stateStdDevs,
+        localMeasurementStdDevs,
+        visionMeasurementStdDevs,
+        0.02);
+  }
+
+  /**
+   * 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]^T, 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]^T, with units in meters and radians.
+   * @param nominalDtSeconds The time in seconds between each robot loop.
+   */
+  @SuppressWarnings("ParameterName")
+  public MecanumDrivePoseEstimator(
+      Rotation2d gyroAngle,
+      Pose2d initialPoseMeters,
+      MecanumDriveKinematics kinematics,
+      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);
+    m_kinematics = kinematics;
+    m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+
+    // Initialize vision R
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+
+    m_visionCorrect =
+        (u, y) ->
+            m_observer.correct(
+                Nat.N3(),
+                u,
+                y,
+                (x, u1) -> x,
+                m_visionDiscreteR,
+                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));
+  }
+
+  /**
+   * Sets the pose estimator's trust of global measurements. This might be used to change trust in
+   * vision measurements after the autonomous period, or to change trust as distance to a vision
+   * target increases.
+   *
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]^T, with units in meters and radians.
+   */
+  public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
+    var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
+    m_visionDiscreteR = Discretization.discretizeR(visionContR, m_nominalDt);
+  }
+
+  /**
+   * 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.
+   */
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+    m_previousAngle = poseMeters.getRotation();
+    m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
+    m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
+  }
+
+  /**
+   * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
+   *
+   * @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)));
+  }
+
+  /**
+   * Add a vision measurement to the Unscented 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.
+   *
+   * @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.
+   */
+  public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+    m_latencyCompensator.applyPastGlobalMeasurement(
+        Nat.N3(),
+        m_observer,
+        m_nominalDt,
+        StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
+        m_visionCorrect,
+        timestampSeconds);
+  }
+
+  /**
+   * Add a vision measurement to the Unscented 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>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]^T, with units in meters and radians.
+   */
+  public void addVisionMeasurement(
+      Pose2d visionRobotPoseMeters,
+      double timestampSeconds,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+  }
+
+  /**
+   * 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.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param wheelSpeeds The current speeds of the mecanum drive wheels.
+   * @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);
+  }
+
+  /**
+   * 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.
+   *
+   * @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.
+   * @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;
+
+    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);
+
+    return getEstimatedPosition();
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java

@@ -0,0 +1,159 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * Generates sigma points and weights according to Van der Merwe's 2004 dissertation[1] for the
+ * UnscentedKalmanFilter class.
+ *
+ * <p>It parametrizes the sigma points using alpha, beta, kappa terms, and is the version seen in
+ * most publications. Unless you know better, this should be your default choice.
+ *
+ * <p>States is the dimensionality of the state. 2*States+1 weights will be generated.
+ *
+ * <p>[1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic Inference in Dynamic
+ * State-Space Models" (Doctoral dissertation)
+ */
+public class MerweScaledSigmaPoints<S extends Num> {
+
+  private final double m_alpha;
+  private final int m_kappa;
+  private final Nat<S> m_states;
+  private Matrix<?, N1> m_wm;
+  private Matrix<?, N1> m_wc;
+
+  /**
+   * Constructs a generator for Van der Merwe scaled sigma points.
+   *
+   * @param states an instance of Num that represents the number of states.
+   * @param alpha Determines the spread of the sigma points around the mean. Usually a small
+   *     positive value (1e-3).
+   * @param beta Incorporates prior knowledge of the distribution of the mean. For Gaussian
+   *     distributions, beta = 2 is optimal.
+   * @param kappa Secondary scaling parameter usually set to 0 or 3 - States.
+   */
+  public MerweScaledSigmaPoints(Nat<S> states, double alpha, double beta, int kappa) {
+    this.m_states = states;
+    this.m_alpha = alpha;
+    this.m_kappa = kappa;
+
+    computeWeights(beta);
+  }
+
+  /**
+   * Constructs a generator for Van der Merwe scaled sigma points with default values for alpha,
+   * beta, and kappa.
+   *
+   * @param states an instance of Num that represents the number of states.
+   */
+  public MerweScaledSigmaPoints(Nat<S> states) {
+    this(states, 1e-3, 2, 3 - states.getNum());
+  }
+
+  /**
+   * Returns number of sigma points for each variable in the state x.
+   *
+   * @return The number of sigma points for each variable in the state x.
+   */
+  public int getNumSigmas() {
+    return 2 * m_states.getNum() + 1;
+  }
+
+  /**
+   * Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P)
+   * 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
+   *     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) {
+    double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum();
+
+    var intermediate = P.times(lambda + m_states.getNum());
+    var U = intermediate.lltDecompose(true); // Lower triangular
+
+    // 2 * states + 1 by states
+    Matrix<S, ?> sigmas =
+        new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
+    sigmas.setColumn(0, x);
+    for (int k = 0; k < m_states.getNum(); k++) {
+      var xPlusU = x.plus(U.extractColumnVector(k));
+      var xMinusU = x.minus(U.extractColumnVector(k));
+      sigmas.setColumn(k + 1, xPlusU);
+      sigmas.setColumn(m_states.getNum() + k + 1, xMinusU);
+    }
+
+    return new Matrix<>(sigmas);
+  }
+
+  /**
+   * Computes the weights for the scaled unscented Kalman filter.
+   *
+   * @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);
+
+    Matrix<?, N1> wM = new Matrix<>(new SimpleMatrix(2 * m_states.getNum() + 1, 1));
+    Matrix<?, N1> wC = new Matrix<>(new SimpleMatrix(2 * m_states.getNum() + 1, 1));
+    wM.fill(c);
+    wC.fill(c);
+
+    wM.set(0, 0, lambda / (m_states.getNum() + lambda));
+    wC.set(0, 0, lambda / (m_states.getNum() + lambda) + (1 - Math.pow(m_alpha, 2) + beta));
+
+    this.m_wm = wM;
+    this.m_wc = wC;
+  }
+
+  /**
+   * Returns the weight for each sigma point for the mean.
+   *
+   * @return the weight for each sigma point for the mean.
+   */
+  public Matrix<?, N1> getWm() {
+    return m_wm;
+  }
+
+  /**
+   * Returns an element of the weight for each sigma point for the mean.
+   *
+   * @param element Element of vector to return.
+   * @return the element i's weight for the mean.
+   */
+  public double getWm(int element) {
+    return m_wm.get(element, 0);
+  }
+
+  /**
+   * Returns the weight for each sigma point for the covariance.
+   *
+   * @return the weight for each sigma point for the covariance.
+   */
+  public Matrix<?, N1> getWc() {
+    return m_wc;
+  }
+
+  /**
+   * Returns an element of the weight for each sigma point for the covariance.
+   *
+   * @param element Element of vector to return.
+   * @return The element I's weight for the covariance.
+   */
+  public double getWc(int element) {
+    return m_wc.get(element, 0);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java

@@ -0,0 +1,299 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.math.Discretization;
+import edu.wpi.first.math.math.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpiutil.WPIUtilJNI;
+import java.util.function.BiConsumer;
+
+/**
+ * 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}.
+ *
+ * <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#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>Our state-space system is:
+ *
+ * <p><strong> x = [[x, y, theta]]^T </strong> in the field-coordinate system.
+ *
+ * <p><strong> u = [[vx, vy, omega]]^T </strong> in the field-coordinate system.
+ *
+ * <p><strong> y = [[x, y, theta]]^T </strong> in field coords from vision, or <strong> y =
+ * [[theta]]^T </strong> from the gyro.
+ */
+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 double m_nominalDt; // Seconds
+  private double m_prevTimeSeconds = -1.0;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  private Matrix<N3, N3> m_visionDiscreteR;
+
+  /**
+   * 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]^T, 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]^T, with units in meters and radians.
+   */
+  public SwerveDrivePoseEstimator(
+      Rotation2d gyroAngle,
+      Pose2d initialPoseMeters,
+      SwerveDriveKinematics kinematics,
+      Matrix<N3, N1> stateStdDevs,
+      Matrix<N1, N1> localMeasurementStdDevs,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    this(
+        gyroAngle,
+        initialPoseMeters,
+        kinematics,
+        stateStdDevs,
+        localMeasurementStdDevs,
+        visionMeasurementStdDevs,
+        0.02);
+  }
+
+  /**
+   * 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]^T, 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]^T, with units in meters and radians.
+   * @param nominalDtSeconds The time in seconds between each robot loop.
+   */
+  @SuppressWarnings("ParameterName")
+  public SwerveDrivePoseEstimator(
+      Rotation2d gyroAngle,
+      Pose2d initialPoseMeters,
+      SwerveDriveKinematics kinematics,
+      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);
+    m_kinematics = kinematics;
+    m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+
+    // Initialize vision R
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+
+    m_visionCorrect =
+        (u, y) ->
+            m_observer.correct(
+                Nat.N3(),
+                u,
+                y,
+                (x, u1) -> x,
+                m_visionDiscreteR,
+                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));
+  }
+
+  /**
+   * Sets the pose estimator's trust of global measurements. This might be used to change trust in
+   * vision measurements after the autonomous period, or to change trust as distance to a vision
+   * target increases.
+   *
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]^T, with units in meters and radians.
+   */
+  public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
+    var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
+    m_visionDiscreteR = Discretization.discretizeR(visionContR, m_nominalDt);
+  }
+
+  /**
+   * 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.
+   */
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+    m_previousAngle = poseMeters.getRotation();
+    m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
+    m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
+  }
+
+  /**
+   * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
+   *
+   * @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)));
+  }
+
+  /**
+   * Add a vision measurement to the Unscented 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.
+   *
+   * @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.
+   */
+  public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+    m_latencyCompensator.applyPastGlobalMeasurement(
+        Nat.N3(),
+        m_observer,
+        m_nominalDt,
+        StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
+        m_visionCorrect,
+        timestampSeconds);
+  }
+
+  /**
+   * Add a vision measurement to the Unscented 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>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]^T, with units in meters and radians.
+   */
+  public void addVisionMeasurement(
+      Pose2d visionRobotPoseMeters,
+      double timestampSeconds,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+  }
+
+  /**
+   * 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.
+   *
+   * @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.
+   */
+  public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
+    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
+  }
+
+  /**
+   * 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.
+   *
+   * @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.
+   * @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;
+
+    var angle = gyroAngle.plus(m_gyroOffset);
+    var omega = angle.minus(m_previousAngle).getRadians() / dt;
+
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(moduleStates);
+    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);
+
+    return getEstimatedPosition();
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java

@@ -0,0 +1,421 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.math.Discretization;
+import edu.wpi.first.math.math.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.NumericalJacobian;
+import java.util.function.BiFunction;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * A Kalman filter combines predictions from a model and measurements to give an estimate of the
+ * true ystem state. This is useful because many states cannot be measured directly as a result of
+ * sensor noise, or because the state is "hidden".
+ *
+ * <p>The Unscented Kalman filter is similar to the {@link KalmanFilter Kalman filter}, except that
+ * it propagates carefully chosen points called sigma points through the non-linear model to obtain
+ * an estimate of the true covariance (as opposed to a linearized version of it). This means that
+ * the UKF works with nonlinear systems.
+ */
+@SuppressWarnings({"MemberName", "ClassTypeParameterName", "PMD.TooManyFields"})
+public class UnscentedKalmanFilter<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;
+
+  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
+  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> m_h;
+
+  private BiFunction<Matrix<States, ?>, Matrix<?, N1>, Matrix<States, N1>> m_meanFuncX;
+  private BiFunction<Matrix<Outputs, ?>, Matrix<?, N1>, Matrix<Outputs, N1>> m_meanFuncY;
+  private BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> m_residualFuncX;
+  private BiFunction<Matrix<Outputs, N1>, Matrix<Outputs, N1>, Matrix<Outputs, N1>> m_residualFuncY;
+  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 final Matrix<States, States> m_contQ;
+  private final Matrix<Outputs, Outputs> m_contR;
+  private Matrix<States, ?> m_sigmasF;
+  private double m_dtSeconds;
+
+  private final MerweScaledSigmaPoints<States> m_pts;
+
+  /**
+   * Constructs an Unscented Kalman Filter.
+   *
+   * @param states A Nat representing the number of states.
+   * @param outputs A Nat representing the number of outputs.
+   * @param f A vector-valued function of x and u that returns the derivative of the state vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param stateStdDevs Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param nominalDtSeconds Nominal discretization timestep.
+   */
+  @SuppressWarnings("LambdaParameterName")
+  public UnscentedKalmanFilter(
+      Nat<States> states,
+      Nat<Outputs> outputs,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
+      Matrix<States, N1> stateStdDevs,
+      Matrix<Outputs, N1> measurementStdDevs,
+      double nominalDtSeconds) {
+    this(
+        states,
+        outputs,
+        f,
+        h,
+        stateStdDevs,
+        measurementStdDevs,
+        (sigmas, Wm) -> sigmas.times(Matrix.changeBoundsUnchecked(Wm)),
+        (sigmas, Wm) -> sigmas.times(Matrix.changeBoundsUnchecked(Wm)),
+        Matrix::minus,
+        Matrix::minus,
+        Matrix::plus,
+        nominalDtSeconds);
+  }
+
+  /**
+   * Constructs an unscented Kalman filter with custom mean, residual, and addition functions. Using
+   * custom functions for arithmetic can be useful if you have angles in the state or measurements,
+   * because they allow you to correctly account for the modular nature of angle arithmetic.
+   *
+   * @param states A Nat representing the number of states.
+   * @param outputs A Nat representing the number of outputs.
+   * @param f A vector-valued function of x and u that returns the derivative of the state vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param stateStdDevs Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param meanFuncX A function that computes the mean of 2 * States + 1 state vectors using a
+   *     given set of weights.
+   * @param meanFuncY A function that computes the mean of 2 * States + 1 measurement vectors using
+   *     a given set of weights.
+   * @param residualFuncX A function that computes the residual of two state vectors (i.e. it
+   *     subtracts them.)
+   * @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.
+   * @param nominalDtSeconds Nominal discretization timestep.
+   */
+  @SuppressWarnings({"ParameterName", "PMD.ExcessiveParameterList"})
+  public UnscentedKalmanFilter(
+      Nat<States> states,
+      Nat<Outputs> outputs,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
+      Matrix<States, N1> stateStdDevs,
+      Matrix<Outputs, N1> measurementStdDevs,
+      BiFunction<Matrix<States, ?>, Matrix<?, N1>, Matrix<States, N1>> meanFuncX,
+      BiFunction<Matrix<Outputs, ?>, Matrix<?, N1>, Matrix<Outputs, N1>> meanFuncY,
+      BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX,
+      BiFunction<Matrix<Outputs, N1>, Matrix<Outputs, N1>, Matrix<Outputs, N1>> residualFuncY,
+      BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX,
+      double nominalDtSeconds) {
+    this.m_states = states;
+    this.m_outputs = outputs;
+
+    m_f = f;
+    m_h = h;
+
+    m_meanFuncX = meanFuncX;
+    m_meanFuncY = meanFuncY;
+    m_residualFuncX = residualFuncX;
+    m_residualFuncY = residualFuncY;
+    m_addFuncX = addFuncX;
+
+    m_dtSeconds = nominalDtSeconds;
+
+    m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
+    m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+
+    m_pts = new MerweScaledSigmaPoints<>(states);
+
+    reset();
+  }
+
+  @SuppressWarnings({"ParameterName", "LocalVariableName", "PMD.CyclomaticComplexity"})
+  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) {
+    if (sigmas.getNumRows() != dim.getNum() || sigmas.getNumCols() != 2 * s.getNum() + 1) {
+      throw new IllegalArgumentException(
+          "Sigmas must be covDim by 2 * states + 1! Got "
+              + sigmas.getNumRows()
+              + " by "
+              + sigmas.getNumCols());
+    }
+
+    if (Wm.getNumRows() != 2 * s.getNum() + 1 || Wm.getNumCols() != 1) {
+      throw new IllegalArgumentException(
+          "Wm must be 2 * states + 1 by 1! Got " + Wm.getNumRows() + " by " + Wm.getNumCols());
+    }
+
+    if (Wc.getNumRows() != 2 * s.getNum() + 1 || Wc.getNumCols() != 1) {
+      throw new IllegalArgumentException(
+          "Wc must be 2 * states + 1 by 1! Got " + Wc.getNumRows() + " by " + Wc.getNumCols());
+    }
+
+    // New mean is usually just the sum of the sigmas * weight:
+    // dot = \Sigma^n_1 (W[k]*Xi[k])
+    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, C> P =
+        y.times(Matrix.changeBoundsUnchecked(Wc.diag()))
+            .times(Matrix.changeBoundsUnchecked(y.transpose()));
+
+    return new Pair<>(x, P);
+  }
+
+  /**
+   * Returns the error covariance matrix P.
+   *
+   * @return the error covariance matrix P.
+   */
+  @Override
+  public Matrix<States, States> getP() {
+    return m_P;
+  }
+
+  /**
+   * Returns an element of the error covariance matrix P.
+   *
+   * @param row Row of P.
+   * @param col Column of P.
+   * @return the value of the error covariance matrix P at (i, j).
+   */
+  @Override
+  public double getP(int row, int col) {
+    return m_P.get(row, col);
+  }
+
+  /**
+   * Sets the entire error covariance matrix P.
+   *
+   * @param newP The new value of P to use.
+   */
+  @Override
+  public void setP(Matrix<States, States> newP) {
+    m_P = newP;
+  }
+
+  /**
+   * Returns the state estimate x-hat.
+   *
+   * @return the state estimate x-hat.
+   */
+  @Override
+  public Matrix<States, N1> getXhat() {
+    return m_xHat;
+  }
+
+  /**
+   * 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.
+   */
+  @Override
+  public double getXhat(int row) {
+    return m_xHat.get(row, 0);
+  }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xHat The state estimate x-hat.
+   */
+  @SuppressWarnings("ParameterName")
+  @Override
+  public void setXhat(Matrix<States, N1> xHat) {
+    m_xHat = xHat;
+  }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  @Override
+  public void setXhat(int row, double value) {
+    m_xHat.set(row, 0, value);
+  }
+
+  /** Resets the observer. */
+  @Override
+  public void reset() {
+    m_xHat = new Matrix<>(m_states, Nat.N1());
+    m_P = new Matrix<>(m_states, m_states);
+    m_sigmasF = new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @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 sigmas = m_pts.sigmaPoints(m_xHat, m_P);
+
+    for (int i = 0; i < m_pts.getNumSigmas(); ++i) {
+      Matrix<States, N1> x = sigmas.extractColumnVector(i);
+
+      m_sigmasF.setColumn(i, NumericalIntegration.rk4(m_f, x, u, dtSeconds));
+    }
+
+    var ret =
+        unscentedTransform(
+            m_states,
+            m_states,
+            m_sigmasF,
+            m_pts.getWm(),
+            m_pts.getWc(),
+            m_meanFuncX,
+            m_residualFuncX);
+
+    m_xHat = ret.getFirst();
+    m_P = ret.getSecond().plus(discQ);
+    m_dtSeconds = dtSeconds;
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @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_meanFuncY, m_residualFuncY, m_residualFuncX, m_addFuncX);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
+   * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
+   * of this function).
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param R Measurement noise covariance matrix.
+   */
+  @SuppressWarnings({"ParameterName", "LambdaParameterName", "LocalVariableName"})
+  public <R extends Num> void correct(
+      Nat<R> rows,
+      Matrix<Inputs, N1> u,
+      Matrix<R, N1> y,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<R, N1>> h,
+      Matrix<R, R> R) {
+    BiFunction<Matrix<R, ?>, Matrix<?, N1>, Matrix<R, N1>> meanFuncY =
+        (sigmas, Wm) -> sigmas.times(Matrix.changeBoundsUnchecked(Wm));
+    BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX =
+        Matrix::minus;
+    BiFunction<Matrix<R, N1>, Matrix<R, N1>, Matrix<R, N1>> residualFuncY = Matrix::minus;
+    BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX = Matrix::plus;
+    correct(rows, u, y, h, R, meanFuncY, residualFuncY, residualFuncX, addFuncX);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
+   * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
+   * of this function).
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param R Measurement noise covariance matrix.
+   * @param meanFuncY A function that computes the mean of 2 * States + 1 measurement vectors using
+   *     a given set of weights.
+   * @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
+   *     subtracts them.)
+   * @param residualFuncX A function that computes the residual of two state vectors (i.e. it
+   *     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,
+      Matrix<R, N1> y,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<R, N1>> h,
+      Matrix<R, R> R,
+      BiFunction<Matrix<R, ?>, Matrix<?, N1>, Matrix<R, N1>> meanFuncY,
+      BiFunction<Matrix<R, N1>, Matrix<R, N1>, Matrix<R, N1>> residualFuncY,
+      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);
+
+    // 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);
+    for (int i = 0; i < m_pts.getNumSigmas(); i++) {
+      Matrix<R, N1> hRet = h.apply(sigmas.extractColumnVector(i), u);
+      sigmasH.setColumn(i, hRet);
+    }
+
+    // Mean and covariance of prediction passed through unscented transform
+    var transRet =
+        unscentedTransform(
+            m_states, rows, sigmasH, m_pts.getWm(), m_pts.getWc(), meanFuncY, residualFuncY);
+    var yHat = transRet.getFirst();
+    var Py = transRet.getSecond().plus(discR);
+
+    // Compute cross covariance of the state and the measurements
+    Matrix<States, R> Pxy = new Matrix<>(m_states, rows);
+    for (int i = 0; i < m_pts.getNumSigmas(); i++) {
+      // Pxy += (sigmas_f[:, i] - xHat) * (sigmas_h[:, i] - yHat)^T * W_c[i]
+      var dx = residualFuncX.apply(m_sigmasF.extractColumnVector(i), m_xHat);
+      var dy = residualFuncY.apply(sigmasH.extractColumnVector(i), yHat).transpose();
+
+      Pxy = Pxy.plus(dx.times(dy).times(m_pts.getWc(i)));
+    }
+
+    // K = P_{xy} Py^-1
+    // K^T = P_y^T^-1 P_{xy}^T
+    // P_y^T K^T = P_{xy}^T
+    // K^T = P_y^T.solve(P_{xy}^T)
+    // K = (P_y^T.solve(P_{xy}^T)^T
+    Matrix<States, R> K = new Matrix<>(Py.transpose().solve(Pxy.transpose()).transpose());
+
+    // xHat + K * (y - yHat)
+    m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, yHat)));
+    m_P = m_P.minus(K.times(Py).times(K.transpose()));
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java

@@ -0,0 +1,168 @@
+// 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.filter;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.wpiutil.CircularBuffer;
+import java.util.Arrays;
+
+/**
+ * This class implements a linear, digital filter. All types of FIR and IIR filters are supported.
+ * Static factory methods are provided to create commonly used types of filters.
+ *
+ * <p>Filters are of the form: y[n] = (b0*x[n] + b1*x[n-1] + ... + bP*x[n-P]) - (a0*y[n-1] +
+ * a2*y[n-2] + ... + aQ*y[n-Q])
+ *
+ * <p>Where: y[n] is the output at time "n" x[n] is the input at time "n" y[n-1] is the output from
+ * the LAST time step ("n-1") x[n-1] is the input from the LAST time step ("n-1") b0...bP are the
+ * "feedforward" (FIR) gains a0...aQ are the "feedback" (IIR) gains IMPORTANT! Note the "-" sign in
+ * front of the feedback term! This is a common convention in signal processing.
+ *
+ * <p>What can linear filters do? Basically, they can filter, or diminish, the effects of
+ * undesirable input frequencies. High frequencies, or rapid changes, can be indicative of sensor
+ * noise or be otherwise undesirable. A "low pass" filter smooths out the signal, reducing the
+ * impact of these high frequency components. Likewise, a "high pass" filter gets rid of slow-moving
+ * signal components, letting you detect large changes more easily.
+ *
+ * <p>Example FRC applications of filters: - Getting rid of noise from an analog sensor input (note:
+ * the roboRIO's FPGA can do this faster in hardware) - Smoothing out joystick input to prevent the
+ * wheels from slipping or the robot from tipping - Smoothing motor commands so that unnecessary
+ * strain isn't put on electrical or mechanical components - If you use clever gains, you can make a
+ * PID controller out of this class!
+ *
+ * <p>For more on filters, we highly recommend the following articles:<br>
+ * https://en.wikipedia.org/wiki/Linear_filter<br>
+ * https://en.wikipedia.org/wiki/Iir_filter<br>
+ * https://en.wikipedia.org/wiki/Fir_filter<br>
+ *
+ * <p>Note 1: calculate() should be called by the user on a known, regular period. You can use a
+ * Notifier for this or do it "inline" with code in a periodic function.
+ *
+ * <p>Note 2: For ALL filters, gains are necessarily a function of frequency. If you make a filter
+ * that works well for you at, say, 100Hz, you will most definitely need to adjust the gains if you
+ * then want to run it at 200Hz! Combining this with Note 1 - the impetus is on YOU as a developer
+ * to make sure calculate() gets called at the desired, constant frequency!
+ */
+public class LinearFilter {
+  private final CircularBuffer m_inputs;
+  private final CircularBuffer m_outputs;
+  private final double[] m_inputGains;
+  private final double[] m_outputGains;
+
+  private static int instances;
+
+  /**
+   * Create a linear FIR or IIR filter.
+   *
+   * @param ffGains The "feed forward" or FIR gains.
+   * @param fbGains The "feed back" or IIR gains.
+   */
+  public LinearFilter(double[] ffGains, double[] fbGains) {
+    m_inputs = new CircularBuffer(ffGains.length);
+    m_outputs = new CircularBuffer(fbGains.length);
+    m_inputGains = Arrays.copyOf(ffGains, ffGains.length);
+    m_outputGains = Arrays.copyOf(fbGains, fbGains.length);
+
+    instances++;
+    MathSharedStore.reportUsage(MathUsageId.kFilter_Linear, instances);
+  }
+
+  /**
+   * Creates a one-pole IIR low-pass filter of the form: y[n] = (1-gain)*x[n] + gain*y[n-1] where
+   * gain = e^(-dt / T), T is the time constant in seconds.
+   *
+   * <p>Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency above which the
+   * input starts to attenuate.
+   *
+   * <p>This filter is stable for time constants greater than zero.
+   *
+   * @param timeConstant The discrete-time time constant in seconds.
+   * @param period The period in seconds between samples taken by the user.
+   */
+  public static LinearFilter singlePoleIIR(double timeConstant, double period) {
+    double gain = Math.exp(-period / timeConstant);
+    double[] ffGains = {1.0 - gain};
+    double[] fbGains = {-gain};
+
+    return new LinearFilter(ffGains, fbGains);
+  }
+
+  /**
+   * Creates a first-order high-pass filter of the form: y[n] = gain*x[n] + (-gain)*x[n-1] +
+   * gain*y[n-1] where gain = e^(-dt / T), T is the time constant in seconds.
+   *
+   * <p>Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency below which the
+   * input starts to attenuate.
+   *
+   * <p>This filter is stable for time constants greater than zero.
+   *
+   * @param timeConstant The discrete-time time constant in seconds.
+   * @param period The period in seconds between samples taken by the user.
+   */
+  public static LinearFilter highPass(double timeConstant, double period) {
+    double gain = Math.exp(-period / timeConstant);
+    double[] ffGains = {gain, -gain};
+    double[] fbGains = {-gain};
+
+    return new LinearFilter(ffGains, fbGains);
+  }
+
+  /**
+   * Creates a K-tap FIR moving average filter of the form: y[n] = 1/k * (x[k] + x[k-1] + ... +
+   * x[0]).
+   *
+   * <p>This filter is always stable.
+   *
+   * @param taps The number of samples to average over. Higher = smoother but slower.
+   * @throws IllegalArgumentException if number of taps is less than 1.
+   */
+  public static LinearFilter movingAverage(int taps) {
+    if (taps <= 0) {
+      throw new IllegalArgumentException("Number of taps was not at least 1");
+    }
+
+    double[] ffGains = new double[taps];
+    for (int i = 0; i < ffGains.length; i++) {
+      ffGains[i] = 1.0 / taps;
+    }
+
+    double[] fbGains = new double[0];
+
+    return new LinearFilter(ffGains, fbGains);
+  }
+
+  /** Reset the filter state. */
+  public void reset() {
+    m_inputs.clear();
+    m_outputs.clear();
+  }
+
+  /**
+   * Calculates the next value of the filter.
+   *
+   * @param input Current input value.
+   * @return The filtered value at this step
+   */
+  public double calculate(double input) {
+    double retVal = 0.0;
+
+    // Rotate the inputs
+    m_inputs.addFirst(input);
+
+    // Calculate the new value
+    for (int i = 0; i < m_inputGains.length; i++) {
+      retVal += m_inputs.get(i) * m_inputGains[i];
+    }
+    for (int i = 0; i < m_outputGains.length; i++) {
+      retVal -= m_outputs.get(i) * m_outputGains[i];
+    }
+
+    // Rotate the outputs
+    m_outputs.addFirst(retVal);
+
+    return retVal;
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java

@@ -0,0 +1,80 @@
+// 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.filter;
+
+import edu.wpi.first.wpiutil.CircularBuffer;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+/**
+ * A class that implements a moving-window median filter. Useful for reducing measurement noise,
+ * especially with processes that generate occasional, extreme outliers (such as values from vision
+ * processing, LIDAR, or ultrasonic sensors).
+ */
+public class MedianFilter {
+  private final CircularBuffer m_valueBuffer;
+  private final List<Double> m_orderedValues;
+  private final int m_size;
+
+  /**
+   * Creates a new MedianFilter.
+   *
+   * @param size The number of samples in the moving window.
+   */
+  public MedianFilter(int size) {
+    // Circular buffer of values currently in the window, ordered by time
+    m_valueBuffer = new CircularBuffer(size);
+    // List of values currently in the window, ordered by value
+    m_orderedValues = new ArrayList<>(size);
+    // Size of rolling window
+    m_size = size;
+  }
+
+  /**
+   * Calculates the moving-window median for the next value of the input stream.
+   *
+   * @param next The next input value.
+   * @return The median of the moving window, updated to include the next value.
+   */
+  public double calculate(double next) {
+    // Find insertion point for next value
+    int index = Collections.binarySearch(m_orderedValues, next);
+
+    // Deal with binarySearch behavior for element not found
+    if (index < 0) {
+      index = Math.abs(index + 1);
+    }
+
+    // Place value at proper insertion point
+    m_orderedValues.add(index, next);
+
+    int curSize = m_orderedValues.size();
+
+    // If buffer is at max size, pop element off of end of circular buffer
+    // and remove from ordered list
+    if (curSize > m_size) {
+      m_orderedValues.remove(m_valueBuffer.removeLast());
+      curSize = curSize - 1;
+    }
+
+    // Add next value to circular buffer
+    m_valueBuffer.addFirst(next);
+
+    if (curSize % 2 == 1) {
+      // If size is odd, return middle element of sorted list
+      return m_orderedValues.get(curSize / 2);
+    } else {
+      // If size is even, return average of middle elements
+      return (m_orderedValues.get(curSize / 2 - 1) + m_orderedValues.get(curSize / 2)) / 2.0;
+    }
+  }
+
+  /** Resets the filter, clearing the window of all elements. */
+  public void reset() {
+    m_orderedValues.clear();
+    m_valueBuffer.clear();
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java

@@ -0,0 +1,245 @@
+// 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 java.util.Objects;
+
+/** Represents a 2d pose containing translational and rotational elements. */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class 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})
+   */
+  public Pose2d() {
+    m_translation = new Translation2d();
+    m_rotation = new Rotation2d();
+  }
+
+  /**
+   * 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 Pose2d(
+      @JsonProperty(required = true, value = "translation") Translation2d translation,
+      @JsonProperty(required = true, value = "rotation") Rotation2d rotation) {
+    m_translation = translation;
+    m_rotation = rotation;
+  }
+
+  /**
+   * Convenience constructors that takes in x and y values directly instead of having to construct a
+   * 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.
+   */
+  public Pose2d(double x, double y, Rotation2d rotation) {
+    m_translation = new Translation2d(x, y);
+    m_rotation = rotation;
+  }
+
+  /**
+   * 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]
+   *
+   * @param other The transform to transform the pose by.
+   * @return The transformed pose.
+   */
+  public Pose2d plus(Transform2d other) {
+    return transformBy(other);
+  }
+
+  /**
+   * Returns the Transform2d 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 Transform2d minus(Pose2d other) {
+    final var pose = this.relativeTo(other);
+    return new Transform2d(pose.getTranslation(), pose.getRotation());
+  }
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return The translational component of the pose.
+   */
+  @JsonProperty
+  public Translation2d 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 rotational component of the transformation.
+   *
+   * @return The rotational component of the pose.
+   */
+  @JsonProperty
+  public Rotation2d getRotation() {
+    return m_rotation;
+  }
+
+  /**
+   * 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 Pose2d transformBy(Transform2d other) {
+    return new Pose2d(
+        m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
+        m_rotation.plus(other.getRotation()));
+  }
+
+  /**
+   * Returns the other pose relative to the current 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 Pose2d relativeTo(Pose2d other) {
+    var transform = new Transform2d(other, this);
+    return new Pose2d(transform.getTranslation(), transform.getRotation());
+  }
+
+  /**
+   * Obtain a new Pose2d from a (constant curvature) velocity.
+   *
+   * <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">Controls
+   * Engineering in the FIRST Robotics Competition</a> section 10.2 "Pose exponential" for a
+   * derivation.
+   *
+   * <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 Twist2d{0.01, 0.0,
+   *     toRadians(0.5)}
+   * @return The new pose of the robot.
+   */
+  public Pose2d exp(Twist2d twist) {
+    double dx = twist.dx;
+    double dy = twist.dy;
+    double dtheta = twist.dtheta;
+
+    double sinTheta = Math.sin(dtheta);
+    double cosTheta = Math.cos(dtheta);
+
+    double s;
+    double c;
+    if (Math.abs(dtheta) < 1E-9) {
+      s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
+      c = 0.5 * dtheta;
+    } else {
+      s = sinTheta / dtheta;
+      c = (1 - cosTheta) / dtheta;
+    }
+    var transform =
+        new Transform2d(
+            new Translation2d(dx * s - dy * c, dx * c + dy * s),
+            new Rotation2d(cosTheta, sinTheta));
+
+    return this.plus(transform);
+  }
+
+  /**
+   * 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.
+   *
+   * @param end The end pose for the transformation.
+   * @return The twist that maps this to end.
+   */
+  public Twist2d log(Pose2d end) {
+    final var transform = end.relativeTo(this);
+    final var dtheta = transform.getRotation().getRadians();
+    final var halfDtheta = dtheta / 2.0;
+
+    final var cosMinusOne = transform.getRotation().getCos() - 1;
+
+    double halfThetaByTanOfHalfDtheta;
+    if (Math.abs(cosMinusOne) < 1E-9) {
+      halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
+    } else {
+      halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne;
+    }
+
+    Translation2d translationPart =
+        transform
+            .getTranslation()
+            .rotateBy(new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta))
+            .times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
+
+    return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
+  }
+
+  /**
+   * Checks equality between this Pose2d 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 Pose2d) {
+      return ((Pose2d) obj).m_translation.equals(m_translation)
+          && ((Pose2d) obj).m_rotation.equals(m_rotation);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_translation, m_rotation);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java

@@ -0,0 +1,201 @@
+// 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 java.util.Objects;
+
+/** A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine). */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Rotation2d {
+  private final double m_value;
+  private final double m_cos;
+  private final double m_sin;
+
+  /** Constructs a Rotation2d with a default angle of 0 degrees. */
+  public Rotation2d() {
+    m_value = 0.0;
+    m_cos = 1.0;
+    m_sin = 0.0;
+  }
+
+  /**
+   * Constructs a Rotation2d with the given radian value. The x and y don't have to be normalized.
+   *
+   * @param value The value of the angle in radians.
+   */
+  @JsonCreator
+  public Rotation2d(@JsonProperty(required = true, value = "radians") double value) {
+    m_value = value;
+    m_cos = Math.cos(value);
+    m_sin = Math.sin(value);
+  }
+
+  /**
+   * Constructs a Rotation2d with the given x and y (cosine and sine) components.
+   *
+   * @param x The x component or cosine of the rotation.
+   * @param y The y component or sine of the rotation.
+   */
+  public Rotation2d(double x, double y) {
+    double magnitude = Math.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 = Math.atan2(m_sin, m_cos);
+  }
+
+  /**
+   * Constructs and returns a Rotation2d with the given degree value.
+   *
+   * @param degrees The value of the angle in degrees.
+   * @return The rotation object with the desired angle value.
+   */
+  public static Rotation2d fromDegrees(double degrees) {
+    return new Rotation2d(Math.toRadians(degrees));
+  }
+
+  /**
+   * 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}
+   *
+   * @param other The rotation to add.
+   * @return The sum of the two rotations.
+   */
+  public Rotation2d plus(Rotation2d other) {
+    return rotateBy(other);
+  }
+
+  /**
+   * 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}
+   *
+   * @param other The rotation to subtract.
+   * @return The difference between the two rotations.
+   */
+  public Rotation2d minus(Rotation2d other) {
+    return rotateBy(other.unaryMinus());
+  }
+
+  /**
+   * Takes the inverse of the current rotation. This is simply the negative of the current angular
+   * value.
+   *
+   * @return The inverse of the current rotation.
+   */
+  public Rotation2d unaryMinus() {
+    return new Rotation2d(-m_value);
+  }
+
+  /**
+   * Multiplies the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Rotation2d.
+   */
+  public Rotation2d times(double scalar) {
+    return new Rotation2d(m_value * scalar);
+  }
+
+  /**
+   * Adds the new rotation to the current rotation using a rotation matrix.
+   *
+   * <p>The matrix multiplication is as follows:
+   *
+   * <pre>
+   * [cos_new]   [other.cos, -other.sin][cos]
+   * [sin_new] = [other.sin,  other.cos][sin]
+   * value_new = atan2(sin_new, cos_new)
+   * </pre>
+   *
+   * @param other The rotation to rotate by.
+   * @return The new rotated Rotation2d.
+   */
+  public Rotation2d rotateBy(Rotation2d other) {
+    return new Rotation2d(
+        m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos);
+  }
+
+  /**
+   * Returns the radian value of the rotation.
+   *
+   * @return The radian value of the rotation.
+   */
+  @JsonProperty
+  public double getRadians() {
+    return m_value;
+  }
+
+  /**
+   * Returns the degree value of the rotation.
+   *
+   * @return The degree value of the rotation.
+   */
+  public double getDegrees() {
+    return Math.toDegrees(m_value);
+  }
+
+  /**
+   * Returns the cosine of the rotation.
+   *
+   * @return The cosine of the rotation.
+   */
+  public double getCos() {
+    return m_cos;
+  }
+
+  /**
+   * Returns the sine of the rotation.
+   *
+   * @return The sine of the rotation.
+   */
+  public double getSin() {
+    return m_sin;
+  }
+
+  /**
+   * Returns the tangent of the rotation.
+   *
+   * @return The tangent of the rotation.
+   */
+  public double getTan() {
+    return m_sin / m_cos;
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
+  }
+
+  /**
+   * Checks equality between this Rotation2d 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 Rotation2d) {
+      var other = (Rotation2d) obj;
+      return Math.hypot(m_cos - other.m_cos, m_sin - other.m_sin) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_value);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java

@@ -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.
+
+package edu.wpi.first.math.geometry;
+
+import java.util.Objects;
+
+/** Represents a transformation for a Pose2d. */
+public class Transform2d {
+  private final Translation2d m_translation;
+  private final Rotation2d 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 Transform2d(Pose2d initial, Pose2d 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 Transform2d(Translation2d translation, Rotation2d rotation) {
+    m_translation = translation;
+    m_rotation = rotation;
+  }
+
+  /** Constructs the identity transform -- maps an initial pose to itself. */
+  public Transform2d() {
+    m_translation = new Translation2d();
+    m_rotation = new Rotation2d();
+  }
+
+  /**
+   * Scales the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform2d.
+   */
+  public Transform2d times(double scalar) {
+    return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar));
+  }
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return The translational component of the transform.
+   */
+  public Translation2d 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 rotational component of the transformation.
+   *
+   * @return Reference to the rotational component of the transform.
+   */
+  public Rotation2d getRotation() {
+    return m_rotation;
+  }
+
+  /**
+   * Invert the transformation. This is useful for undoing a transformation.
+   *
+   * @return The inverted transformation.
+   */
+  public Transform2d 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 Transform2d(
+        getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
+        getRotation().unaryMinus());
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Transform2d(%s, %s)", m_translation, m_rotation);
+  }
+
+  /**
+   * Checks equality between this Transform2d 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 Transform2d) {
+      return ((Transform2d) obj).m_translation.equals(m_translation)
+          && ((Transform2d) obj).m_rotation.equals(m_rotation);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_translation, m_rotation);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java

@@ -0,0 +1,199 @@
+// 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 java.util.Objects;
+
+/**
+ * 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.
+ */
+@SuppressWarnings({"ParameterName", "MemberName"})
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Translation2d {
+  private final double m_x;
+  private final double m_y;
+
+  /** Constructs a Translation2d with X and Y components equal to zero. */
+  public Translation2d() {
+    this(0.0, 0.0);
+  }
+
+  /**
+   * Constructs a Translation2d with the X and Y components equal to the provided values.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   */
+  @JsonCreator
+  public Translation2d(
+      @JsonProperty(required = true, value = "x") double x,
+      @JsonProperty(required = true, value = "y") double y) {
+    m_x = x;
+    m_y = y;
+  }
+
+  /**
+   * Constructs a Translation2d 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 Translation2d(double distance, Rotation2d angle) {
+    m_x = distance * angle.getCos();
+    m_y = distance * angle.getSin();
+  }
+
+  /**
+   * 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)
+   *
+   * @param other The translation to compute the distance to.
+   * @return The distance between the two translations.
+   */
+  public double getDistance(Translation2d other) {
+    return Math.hypot(other.m_x - m_x, other.m_y - m_y);
+  }
+
+  /**
+   * 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 norm, or distance from the origin to the translation.
+   *
+   * @return The norm of the translation.
+   */
+  public double getNorm() {
+    return Math.hypot(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]
+   *
+   * <p>For example, rotating a Translation2d of {2, 0} by 90 degrees will return a Translation2d of
+   * {0, 2}.
+   *
+   * @param other The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  public Translation2d rotateBy(Rotation2d other) {
+    return new Translation2d(
+        m_x * other.getCos() - m_y * other.getSin(), m_x * other.getSin() + m_y * other.getCos());
+  }
+
+  /**
+   * Adds two translations in 2d space and returns the sum. This is similar to vector addition.
+   *
+   * <p>For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} = Translation2d{3.0, 8.0}
+   *
+   * @param other The translation to add.
+   * @return The sum of the translations.
+   */
+  public Translation2d plus(Translation2d other) {
+    return new Translation2d(m_x + other.m_x, m_y + other.m_y);
+  }
+
+  /**
+   * Subtracts the other translation from the other translation and returns the difference.
+   *
+   * <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.
+   */
+  public Translation2d minus(Translation2d other) {
+    return new Translation2d(m_x - other.m_x, m_y - other.m_y);
+  }
+
+  /**
+   * 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.
+   *
+   * @return The inverse of the current translation.
+   */
+  public Translation2d unaryMinus() {
+    return new Translation2d(-m_x, -m_y);
+  }
+
+  /**
+   * Multiplies the translation by a scalar and returns the new translation.
+   *
+   * <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.
+   */
+  public Translation2d times(double scalar) {
+    return new Translation2d(m_x * scalar, m_y * scalar);
+  }
+
+  /**
+   * Divides the translation by a scalar and returns the new translation.
+   *
+   * <p>For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The reference to the new mutated object.
+   */
+  public Translation2d div(double scalar) {
+    return new Translation2d(m_x / scalar, m_y / scalar);
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
+  }
+
+  /**
+   * Checks equality between this Translation2d 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 Translation2d) {
+      return Math.abs(((Translation2d) obj).m_x - m_x) < 1E-9
+          && Math.abs(((Translation2d) obj).m_y - m_y) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_x, m_y);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java

@@ -0,0 +1,66 @@
+// 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 arc since the last pose update. We can use ideas from differential
+ * calculus to create new Pose2ds 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;
+
+  /** Linear "dy" component. */
+  public double dy;
+
+  /** Angular "dtheta" component (radians). */
+  public double dtheta;
+
+  public Twist2d() {}
+
+  /**
+   * Constructs a Twist2d with the given values.
+   *
+   * @param dx Change in x direction relative to robot.
+   * @param dy Change in y direction relative to robot.
+   * @param dtheta Change in angle relative to robot.
+   */
+  public Twist2d(double dx, double dy, double dtheta) {
+    this.dx = dx;
+    this.dy = dy;
+    this.dtheta = dtheta;
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);
+  }
+
+  /**
+   * Checks equality between this Twist2d 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 Twist2d) {
+      return Math.abs(((Twist2d) obj).dx - dx) < 1E-9
+          && Math.abs(((Twist2d) obj).dy - dy) < 1E-9
+          && Math.abs(((Twist2d) obj).dtheta - dtheta) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(dx, dy, dtheta);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java

@@ -0,0 +1,78 @@
+// 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;
+
+/**
+ * Represents the speed of a robot chassis. Although this struct contains similar members compared
+ * to a Twist2d, they do NOT represent the same thing. Whereas a Twist2d represents a change in pose
+ * w.r.t to the robot frame of reference, this ChassisSpeeds struct represents a velocity w.r.t to
+ * the robot frame of reference.
+ *
+ * <p>A strictly non-holonomic drivetrain, such as a differential drive, should never have a dy
+ * component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum
+ * will often have all three components.
+ */
+@SuppressWarnings("MemberName")
+public class ChassisSpeeds {
+  /** Represents forward velocity w.r.t the robot frame of reference. (Fwd is +) */
+  public double vxMetersPerSecond;
+
+  /** Represents sideways velocity w.r.t the robot frame of reference. (Left is +) */
+  public double vyMetersPerSecond;
+
+  /** Represents the angular velocity of the robot frame. (CCW is +) */
+  public double omegaRadiansPerSecond;
+
+  /** Constructs a ChassisSpeeds with zeros for dx, dy, and theta. */
+  public ChassisSpeeds() {}
+
+  /**
+   * Constructs a ChassisSpeeds object.
+   *
+   * @param vxMetersPerSecond Forward velocity.
+   * @param vyMetersPerSecond Sideways velocity.
+   * @param omegaRadiansPerSecond Angular velocity.
+   */
+  public ChassisSpeeds(
+      double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond) {
+    this.vxMetersPerSecond = vxMetersPerSecond;
+    this.vyMetersPerSecond = vyMetersPerSecond;
+    this.omegaRadiansPerSecond = omegaRadiansPerSecond;
+  }
+
+  /**
+   * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
+   * object.
+   *
+   * @param vxMetersPerSecond The component of speed in the x direction relative to the field.
+   *     Positive x is away from your alliance wall.
+   * @param vyMetersPerSecond The component of speed in the y direction relative to the field.
+   *     Positive y is to your left when standing behind the alliance wall.
+   * @param omegaRadiansPerSecond The angular rate of the robot.
+   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
+   *     considered to be zero when it is facing directly away from your alliance station wall.
+   *     Remember that this should be CCW positive.
+   * @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
+   */
+  public static ChassisSpeeds fromFieldRelativeSpeeds(
+      double vxMetersPerSecond,
+      double vyMetersPerSecond,
+      double omegaRadiansPerSecond,
+      Rotation2d robotAngle) {
+    return new ChassisSpeeds(
+        vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(),
+        -vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(),
+        omegaRadiansPerSecond);
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)",
+        vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java

@@ -0,0 +1,61 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+
+/**
+ * Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel
+ * velocities for a differential drive.
+ *
+ * <p>Inverse kinematics converts a desired chassis speed into left and right velocity components
+ * 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;
+
+  /**
+   * Constructs a differential drive kinematics object.
+   *
+   * @param trackWidthMeters The track width of the drivetrain. Theoretically, this is the distance
+   *     between the left wheels and right wheels. However, the empirical value may be larger than
+   *     the physical measured value due to scrubbing effects.
+   */
+  public DifferentialDriveKinematics(double trackWidthMeters) {
+    this.trackWidthMeters = trackWidthMeters;
+    MathSharedStore.reportUsage(MathUsageId.kKinematics_DifferentialDrive, 1);
+  }
+
+  /**
+   * Returns a chassis speed from left and right component velocities using forward kinematics.
+   *
+   * @param wheelSpeeds The left and right velocities.
+   * @return The chassis speed.
+   */
+  public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
+    return new ChassisSpeeds(
+        (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2,
+        0,
+        (wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond) / trackWidthMeters);
+  }
+
+  /**
+   * Returns left and right component velocities from a chassis speed using inverse kinematics.
+   *
+   * @param chassisSpeeds The linear and angular (dx and dtheta) components that represent the
+   *     chassis' speed.
+   * @return The left and right velocities.
+   */
+  public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
+    return new DifferentialDriveWheelSpeeds(
+        chassisSpeeds.vxMetersPerSecond
+            - trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond,
+        chassisSpeeds.vxMetersPerSecond
+            + trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java

@@ -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.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+
+/**
+ * Class for differential drive odometry. Odometry allows you to track the robot's position on the
+ * field over the course of a match using readings from 2 encoders and a gyroscope.
+ *
+ * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
+ * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
+ *
+ * <p>It is important that you reset your encoders to zero before using this class. Any subsequent
+ * pose resets also require the encoders to be reset to zero.
+ */
+public class DifferentialDriveOdometry {
+  private Pose2d m_poseMeters;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  private double m_prevLeftDistance;
+  private double m_prevRightDistance;
+
+  /**
+   * Constructs a DifferentialDriveOdometry object.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param initialPoseMeters The starting position of the robot on the field.
+   */
+  public DifferentialDriveOdometry(Rotation2d gyroAngle, Pose2d initialPoseMeters) {
+    m_poseMeters = initialPoseMeters;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPoseMeters.getRotation();
+    MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
+  }
+
+  /**
+   * Constructs a DifferentialDriveOdometry object with the default pose at the origin.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  public DifferentialDriveOdometry(Rotation2d gyroAngle) {
+    this(gyroAngle, 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.
+   */
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+    m_poseMeters = poseMeters;
+    m_previousAngle = poseMeters.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+
+    m_prevLeftDistance = 0.0;
+    m_prevRightDistance = 0.0;
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   *
+   * @return The pose of the robot (x and y are in meters).
+   */
+  public Pose2d getPoseMeters() {
+    return m_poseMeters;
+  }
+
+  /**
+   * Updates the robot position on the field using distance measurements from encoders. This method
+   * is more numerically accurate than using velocities to integrate the pose and is also
+   * advantageous for teams that are using lower CPR encoders.
+   *
+   * @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.
+   * @return The new pose of the robot.
+   */
+  public Pose2d update(
+      Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
+    double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
+    double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
+
+    m_prevLeftDistance = leftDistanceMeters;
+    m_prevRightDistance = rightDistanceMeters;
+
+    double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
+    var angle = gyroAngle.plus(m_gyroOffset);
+
+    var newPose =
+        m_poseMeters.exp(
+            new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
+
+    m_previousAngle = angle;
+
+    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+    return m_poseMeters;
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java

@@ -0,0 +1,55 @@
+// 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;
+
+/** 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;
+
+  /** Speed of the right side of the robot. */
+  public double rightMetersPerSecond;
+
+  /** Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds. */
+  public DifferentialDriveWheelSpeeds() {}
+
+  /**
+   * Constructs a DifferentialDriveWheelSpeeds.
+   *
+   * @param leftMetersPerSecond The left speed.
+   * @param rightMetersPerSecond The right speed.
+   */
+  public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMetersPerSecond) {
+    this.leftMetersPerSecond = leftMetersPerSecond;
+    this.rightMetersPerSecond = rightMetersPerSecond;
+  }
+
+  /**
+   * Normalizes the wheel speeds using some max attainable speed. Sometimes, after inverse
+   * kinematics, the requested speed from a/several modules may be above the max attainable speed
+   * for the driving motor on that module. To fix this issue, one can "normalize" all the wheel
+   * speeds to make sure that all requested module speeds are below the absolute threshold, while
+   * maintaining the ratio of speeds between modules.
+   *
+   * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
+   */
+  public void normalize(double attainableMaxSpeedMetersPerSecond) {
+    double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond));
+
+    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
+      leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
+      rightMetersPerSecond =
+          rightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
+    }
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)",
+        leftMetersPerSecond, rightMetersPerSecond);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java

@@ -0,0 +1,171 @@
+// 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.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.geometry.Translation2d;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual
+ * wheel speeds.
+ *
+ * <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.
+ *
+ * <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
+ * system (more equations than variables), we use a least-squares approximation.
+ *
+ * <p>The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds] We take the
+ * Moore-Penrose pseudoinverse of [wheelLocations] and then multiply by [wheelSpeeds] to get our
+ * chassis speeds.
+ *
+ * <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
+ * field using encoders and a gyro.
+ */
+public class MecanumDriveKinematics {
+  private final SimpleMatrix m_inverseKinematics;
+  private final SimpleMatrix m_forwardKinematics;
+
+  private final Translation2d m_frontLeftWheelMeters;
+  private final Translation2d m_frontRightWheelMeters;
+  private final Translation2d m_rearLeftWheelMeters;
+  private final Translation2d m_rearRightWheelMeters;
+
+  private Translation2d m_prevCoR = new Translation2d();
+
+  /**
+   * Constructs a mecanum drive kinematics object.
+   *
+   * @param frontLeftWheelMeters The location of the front-left wheel relative to the physical
+   *     center of the robot.
+   * @param frontRightWheelMeters The location of the front-right wheel relative to the physical
+   *     center of the robot.
+   * @param rearLeftWheelMeters The location of the rear-left wheel relative to the physical center
+   *     of the robot.
+   * @param rearRightWheelMeters The location of the rear-right wheel relative to the physical
+   *     center of the robot.
+   */
+  public MecanumDriveKinematics(
+      Translation2d frontLeftWheelMeters,
+      Translation2d frontRightWheelMeters,
+      Translation2d rearLeftWheelMeters,
+      Translation2d rearRightWheelMeters) {
+    m_frontLeftWheelMeters = frontLeftWheelMeters;
+    m_frontRightWheelMeters = frontRightWheelMeters;
+    m_rearLeftWheelMeters = rearLeftWheelMeters;
+    m_rearRightWheelMeters = rearRightWheelMeters;
+
+    m_inverseKinematics = new SimpleMatrix(4, 3);
+
+    setInverseKinematics(
+        frontLeftWheelMeters, frontRightWheelMeters, rearLeftWheelMeters, rearRightWheelMeters);
+    m_forwardKinematics = m_inverseKinematics.pseudoInverse();
+
+    MathSharedStore.reportUsage(MathUsageId.kKinematics_MecanumDrive, 1);
+  }
+
+  /**
+   * Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This
+   * method is often used to convert joystick values into wheel speeds.
+   *
+   * <p>This function also supports variable centers of rotation. During normal operations, the
+   * center of rotation is usually the same as the physical center of the robot; therefore, the
+   * 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.
+   *
+   * @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
+   *     component, the robot will rotate around that corner.
+   * @return The wheel speeds. Use caution because they are not normalized. Sometimes, a user input
+   *     may cause one of the wheel speeds to go above the attainable max velocity. Use the {@link
+   *     MecanumDriveWheelSpeeds#normalize(double)} function to rectify this issue.
+   */
+  public MecanumDriveWheelSpeeds toWheelSpeeds(
+      ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
+    // We have a new center of rotation. We need to compute the matrix again.
+    if (!centerOfRotationMeters.equals(m_prevCoR)) {
+      var fl = m_frontLeftWheelMeters.minus(centerOfRotationMeters);
+      var fr = m_frontRightWheelMeters.minus(centerOfRotationMeters);
+      var rl = m_rearLeftWheelMeters.minus(centerOfRotationMeters);
+      var rr = m_rearRightWheelMeters.minus(centerOfRotationMeters);
+
+      setInverseKinematics(fl, fr, rl, rr);
+      m_prevCoR = centerOfRotationMeters;
+    }
+
+    var chassisSpeedsVector = new SimpleMatrix(3, 1);
+    chassisSpeedsVector.setColumn(
+        0,
+        0,
+        chassisSpeeds.vxMetersPerSecond,
+        chassisSpeeds.vyMetersPerSecond,
+        chassisSpeeds.omegaRadiansPerSecond);
+
+    var wheelsMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
+    return new MecanumDriveWheelSpeeds(
+        wheelsMatrix.get(0, 0),
+        wheelsMatrix.get(1, 0),
+        wheelsMatrix.get(2, 0),
+        wheelsMatrix.get(3, 0));
+  }
+
+  /**
+   * Performs inverse kinematics. See {@link #toWheelSpeeds(ChassisSpeeds, Translation2d)} for more
+   * information.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @return The wheel speeds.
+   */
+  public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
+    return toWheelSpeeds(chassisSpeeds, new Translation2d());
+  }
+
+  /**
+   * Performs forward kinematics to return the resulting chassis state from the given wheel speeds.
+   * This method is often used for odometry -- determining the robot's position on the field using
+   * data from the real-world speed of each wheel on the robot.
+   *
+   * @param wheelSpeeds The current mecanum drive wheel speeds.
+   * @return The resulting chassis speed.
+   */
+  public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
+    var wheelSpeedsMatrix = new SimpleMatrix(4, 1);
+    wheelSpeedsMatrix.setColumn(
+        0,
+        0,
+        wheelSpeeds.frontLeftMetersPerSecond,
+        wheelSpeeds.frontRightMetersPerSecond,
+        wheelSpeeds.rearLeftMetersPerSecond,
+        wheelSpeeds.rearRightMetersPerSecond);
+    var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsMatrix);
+
+    return new ChassisSpeeds(
+        chassisSpeedsVector.get(0, 0),
+        chassisSpeedsVector.get(1, 0),
+        chassisSpeedsVector.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.
+   * @param fr The location of the front-right wheel relative to the physical center of the robot.
+   * @param rl The location of the rear-left wheel relative to the physical center of the robot.
+   * @param rr The location of the rear-right wheel relative to the physical center of the robot.
+   */
+  private void setInverseKinematics(
+      Translation2d fl, Translation2d fr, Translation2d rl, Translation2d rr) {
+    m_inverseKinematics.setRow(0, 0, 1, -1, -(fl.getX() + fl.getY()));
+    m_inverseKinematics.setRow(1, 0, 1, 1, fr.getX() - fr.getY());
+    m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY());
+    m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY()));
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java

@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+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;
+
+  /** Voltage of the front right motor. */
+  public double frontRightVoltage;
+
+  /** Voltage of the rear left motor. */
+  public double rearLeftVoltage;
+
+  /** Voltage of the rear right motor. */
+  public double rearRightVoltage;
+
+  /** Constructs a MecanumDriveMotorVoltages with zeros for all member fields. */
+  public MecanumDriveMotorVoltages() {}
+
+  /**
+   * Constructs a MecanumDriveMotorVoltages.
+   *
+   * @param frontLeftVoltage Voltage of the front left motor.
+   * @param frontRightVoltage Voltage of the front right motor.
+   * @param rearLeftVoltage Voltage of the rear left motor.
+   * @param rearRightVoltage Voltage of the rear right motor.
+   */
+  public MecanumDriveMotorVoltages(
+      double frontLeftVoltage,
+      double frontRightVoltage,
+      double rearLeftVoltage,
+      double rearRightVoltage) {
+    this.frontLeftVoltage = frontLeftVoltage;
+    this.frontRightVoltage = frontRightVoltage;
+    this.rearLeftVoltage = rearLeftVoltage;
+    this.rearRightVoltage = rearRightVoltage;
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "MecanumDriveMotorVoltages(Front Left: %.2f V, Front Right: %.2f V, "
+            + "Rear Left: %.2f V, Rear Right: %.2f V)",
+        frontLeftVoltage, frontRightVoltage, rearLeftVoltage, rearRightVoltage);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java

@@ -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.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.MathSharedStore;
+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.wpiutil.WPIUtilJNI;
+
+/**
+ * Class for mecanum drive odometry. Odometry allows you to track the robot's position on the field
+ * over a course of a match using readings from your mecanum wheel encoders.
+ *
+ * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
+ * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
+ */
+public class MecanumDriveOdometry {
+  private final MecanumDriveKinematics m_kinematics;
+  private Pose2d m_poseMeters;
+  private double m_prevTimeSeconds = -1;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  /**
+   * Constructs a MecanumDriveOdometry object.
+   *
+   * @param kinematics The mecanum drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param initialPoseMeters The starting position of the robot on the field.
+   */
+  public MecanumDriveOdometry(
+      MecanumDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPoseMeters) {
+    m_kinematics = kinematics;
+    m_poseMeters = initialPoseMeters;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPoseMeters.getRotation();
+    MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
+  }
+
+  /**
+   * Constructs a MecanumDriveOdometry object with the default pose at the origin.
+   *
+   * @param kinematics The mecanum drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) {
+    this(kinematics, gyroAngle, new Pose2d());
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
+   * automatically takes care of offsetting the gyro angle.
+   *
+   * @param poseMeters The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+    m_poseMeters = poseMeters;
+    m_previousAngle = poseMeters.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   *
+   * @return The pose of the robot (x and y are in meters).
+   */
+  public Pose2d getPoseMeters() {
+    return m_poseMeters;
+  }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and integration of the pose
+   * over time. This method takes in 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 wheelSpeeds The current wheel speeds.
+   * @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;
+
+    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()));
+
+    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 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);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java

@@ -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.
+
+package edu.wpi.first.math.kinematics;
+
+import java.util.stream.DoubleStream;
+
+@SuppressWarnings("MemberName")
+public class MecanumDriveWheelSpeeds {
+  /** Speed of the front left wheel. */
+  public double frontLeftMetersPerSecond;
+
+  /** Speed of the front right wheel. */
+  public double frontRightMetersPerSecond;
+
+  /** Speed of the rear left wheel. */
+  public double rearLeftMetersPerSecond;
+
+  /** Speed of the rear right wheel. */
+  public double rearRightMetersPerSecond;
+
+  /** Constructs a MecanumDriveWheelSpeeds with zeros for all member fields. */
+  public MecanumDriveWheelSpeeds() {}
+
+  /**
+   * Constructs a MecanumDriveWheelSpeeds.
+   *
+   * @param frontLeftMetersPerSecond Speed of the front left wheel.
+   * @param frontRightMetersPerSecond Speed of the front right wheel.
+   * @param rearLeftMetersPerSecond Speed of the rear left wheel.
+   * @param rearRightMetersPerSecond Speed of the rear right wheel.
+   */
+  public MecanumDriveWheelSpeeds(
+      double frontLeftMetersPerSecond,
+      double frontRightMetersPerSecond,
+      double rearLeftMetersPerSecond,
+      double rearRightMetersPerSecond) {
+    this.frontLeftMetersPerSecond = frontLeftMetersPerSecond;
+    this.frontRightMetersPerSecond = frontRightMetersPerSecond;
+    this.rearLeftMetersPerSecond = rearLeftMetersPerSecond;
+    this.rearRightMetersPerSecond = rearRightMetersPerSecond;
+  }
+
+  /**
+   * Normalizes the wheel speeds using some max attainable speed. Sometimes, after inverse
+   * kinematics, the requested speed from a/several modules may be above the max attainable speed
+   * for the driving motor on that module. To fix this issue, one can "normalize" all the wheel
+   * speeds to make sure that all requested module speeds are below the absolute threshold, while
+   * maintaining the ratio of speeds between modules.
+   *
+   * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
+   */
+  public void normalize(double attainableMaxSpeedMetersPerSecond) {
+    double realMaxSpeed =
+        DoubleStream.of(
+                frontLeftMetersPerSecond,
+                frontRightMetersPerSecond,
+                rearLeftMetersPerSecond,
+                rearRightMetersPerSecond)
+            .max()
+            .getAsDouble();
+
+    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
+      frontLeftMetersPerSecond =
+          frontLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
+      frontRightMetersPerSecond =
+          frontRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
+      rearLeftMetersPerSecond =
+          rearLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
+      rearRightMetersPerSecond =
+          rearRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
+    }
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "MecanumDriveWheelSpeeds(Front Left: %.2f m/s, Front Right: %.2f m/s, "
+            + "Rear Left: %.2f m/s, Rear Right: %.2f m/s)",
+        frontLeftMetersPerSecond,
+        frontRightMetersPerSecond,
+        rearLeftMetersPerSecond,
+        rearRightMetersPerSecond);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java

@@ -0,0 +1,194 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import java.util.Arrays;
+import java.util.Collections;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual
+ * module states (speed and angle).
+ *
+ * <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.
+ *
+ * <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
+ * system (more equations than variables), we use a least-squares approximation.
+ *
+ * <p>The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds] We take the
+ * Moore-Penrose pseudoinverse of [moduleLocations] and then multiply by [moduleStates] to get our
+ * chassis speeds.
+ *
+ * <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
+ * field using encoders and a gyro.
+ */
+public class SwerveDriveKinematics {
+  private final SimpleMatrix m_inverseKinematics;
+  private final SimpleMatrix m_forwardKinematics;
+
+  private final int m_numModules;
+  private final Translation2d[] m_modules;
+  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.
+   *
+   * @param wheelsMeters The locations of the wheels relative to the physical center of the robot.
+   */
+  public SwerveDriveKinematics(Translation2d... wheelsMeters) {
+    if (wheelsMeters.length < 2) {
+      throw new IllegalArgumentException("A swerve drive requires at least two modules");
+    }
+    m_numModules = wheelsMeters.length;
+    m_modules = Arrays.copyOf(wheelsMeters, m_numModules);
+    m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
+
+    for (int i = 0; i < m_numModules; i++) {
+      m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY());
+      m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX());
+    }
+    m_forwardKinematics = m_inverseKinematics.pseudoInverse();
+
+    MathSharedStore.reportUsage(MathUsageId.kKinematics_SwerveDrive, 1);
+  }
+
+  /**
+   * Performs inverse kinematics to return the module states from a desired chassis velocity. This
+   * method is often used to convert joystick values into module speeds and angles.
+   *
+   * <p>This function also supports variable centers of rotation. During normal operations, the
+   * center of rotation is usually the same as the physical center of the robot; therefore, the
+   * 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.
+   *
+   * @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
+   *     component, the robot will rotate around that corner.
+   * @return An array containing the module states. Use caution because these module states are not
+   *     normalized. Sometimes, a user input may cause one of the module speeds to go above the
+   *     attainable max velocity. Use the {@link #normalizeWheelSpeeds(SwerveModuleState[], double)
+   *     normalizeWheelSpeeds} function to rectify this issue.
+   */
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  public SwerveModuleState[] toSwerveModuleStates(
+      ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
+    if (!centerOfRotationMeters.equals(m_prevCoR)) {
+      for (int i = 0; i < m_numModules; i++) {
+        m_inverseKinematics.setRow(
+            i * 2 + 0,
+            0, /* Start Data */
+            1,
+            0,
+            -m_modules[i].getY() + centerOfRotationMeters.getY());
+        m_inverseKinematics.setRow(
+            i * 2 + 1,
+            0, /* Start Data */
+            0,
+            1,
+            +m_modules[i].getX() - centerOfRotationMeters.getX());
+      }
+      m_prevCoR = centerOfRotationMeters;
+    }
+
+    var chassisSpeedsVector = new SimpleMatrix(3, 1);
+    chassisSpeedsVector.setColumn(
+        0,
+        0,
+        chassisSpeeds.vxMetersPerSecond,
+        chassisSpeeds.vyMetersPerSecond,
+        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);
+      double y = moduleStatesMatrix.get(i * 2 + 1, 0);
+
+      double speed = Math.hypot(x, y);
+      Rotation2d angle = new Rotation2d(x, y);
+
+      moduleStates[i] = new SwerveModuleState(speed, angle);
+    }
+
+    return moduleStates;
+  }
+
+  /**
+   * Performs inverse kinematics. See {@link #toSwerveModuleStates(ChassisSpeeds, Translation2d)}
+   * toSwerveModuleStates for more information.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @return An array containing the module states.
+   */
+  public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds) {
+    return toSwerveModuleStates(chassisSpeeds, new Translation2d());
+  }
+
+  /**
+   * Performs forward kinematics to return the resulting chassis state from the given module states.
+   * This method is often used for odometry -- determining the robot's position on the field using
+   * data from the real-world speed and angle of each module on the robot.
+   *
+   * @param wheelStates The state of the modules (as a SwerveModuleState type) as measured from
+   *     respective encoders and gyros. The order of the swerve module states should be same as
+   *     passed into the constructor of this class.
+   * @return The resulting chassis speed.
+   */
+  public ChassisSpeeds toChassisSpeeds(SwerveModuleState... wheelStates) {
+    if (wheelStates.length != m_numModules) {
+      throw new IllegalArgumentException(
+          "Number of modules is not consistent with number of wheel locations provided in "
+              + "constructor");
+    }
+    var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1);
+
+    for (int i = 0; i < m_numModules; i++) {
+      var module = wheelStates[i];
+      moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos());
+      moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin());
+    }
+
+    var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix);
+    return new ChassisSpeeds(
+        chassisSpeedsVector.get(0, 0),
+        chassisSpeedsVector.get(1, 0),
+        chassisSpeedsVector.get(2, 0));
+  }
+
+  /**
+   * Normalizes the wheel speeds using some max attainable speed. Sometimes, after inverse
+   * kinematics, the requested speed from a/several modules may be above the max attainable speed
+   * for the driving motor on that module. To fix this issue, one can "normalize" all the wheel
+   * speeds to make sure that all requested module speeds are 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 attainableMaxSpeedMetersPerSecond The absolute max speed that a module can reach.
+   */
+  public static void normalizeWheelSpeeds(
+      SwerveModuleState[] moduleStates, double attainableMaxSpeedMetersPerSecond) {
+    double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
+    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
+      for (SwerveModuleState moduleState : moduleStates) {
+        moduleState.speedMetersPerSecond =
+            moduleState.speedMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
+      }
+    }
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java

@@ -0,0 +1,129 @@
+// 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.MathSharedStore;
+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.wpiutil.WPIUtilJNI;
+
+/**
+ * Class for swerve drive odometry. Odometry allows you to track the robot's position on the field
+ * over a course of a match using readings from your swerve drive encoders and swerve azimuth
+ * encoders.
+ *
+ * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
+ * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
+ */
+public class SwerveDriveOdometry {
+  private final SwerveDriveKinematics m_kinematics;
+  private Pose2d m_poseMeters;
+  private double m_prevTimeSeconds = -1;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  /**
+   * Constructs a SwerveDriveOdometry object.
+   *
+   * @param kinematics The swerve drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param initialPose The starting position of the robot on the field.
+   */
+  public SwerveDriveOdometry(
+      SwerveDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPose) {
+    m_kinematics = kinematics;
+    m_poseMeters = initialPose;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPose.getRotation();
+    MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
+  }
+
+  /**
+   * Constructs a SwerveDriveOdometry object with the default pose at the origin.
+   *
+   * @param kinematics The swerve drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) {
+    this(kinematics, gyroAngle, new Pose2d());
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
+   * automatically takes care of offsetting the gyro angle.
+   *
+   * @param pose The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  public void resetPosition(Pose2d pose, Rotation2d gyroAngle) {
+    m_poseMeters = pose;
+    m_previousAngle = pose.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   *
+   * @return The pose of the robot (x and y are in meters).
+   */
+  public Pose2d getPoseMeters() {
+    return m_poseMeters;
+  }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and integration of the pose
+   * over time. This method takes in 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.
+   * @return The new pose of the robot.
+   */
+  public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
+    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java

@@ -0,0 +1,71 @@
+// 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;
+
+/** 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;
+
+  /** Angle of the module. */
+  public Rotation2d angle = Rotation2d.fromDegrees(0);
+
+  /** Constructs a SwerveModuleState with zeros for speed and angle. */
+  public SwerveModuleState() {}
+
+  /**
+   * Constructs a SwerveModuleState.
+   *
+   * @param speedMetersPerSecond The speed of the wheel of the module.
+   * @param angle The angle of the module.
+   */
+  public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) {
+    this.speedMetersPerSecond = speedMetersPerSecond;
+    this.angle = angle;
+  }
+
+  /**
+   * Compares two swerve module states. One swerve module is "greater" than the other if its speed
+   * is higher than the other.
+   *
+   * @param o The other swerve module.
+   * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
+   */
+  @Override
+  @SuppressWarnings("ParameterName")
+  public int compareTo(SwerveModuleState o) {
+    return Double.compare(this.speedMetersPerSecond, o.speedMetersPerSecond);
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond, angle);
+  }
+
+  /**
+   * Minimize the change in heading the desired swerve module state would require by potentially
+   * reversing the direction the wheel spins. If this is used with the PIDController class's
+   * continuous input functionality, the furthest a wheel will ever rotate is 90 degrees.
+   *
+   * @param desiredState The desired state.
+   * @param currentAngle The current module angle.
+   */
+  public static SwerveModuleState optimize(
+      SwerveModuleState desiredState, Rotation2d currentAngle) {
+    var delta = desiredState.angle.minus(currentAngle);
+    if (Math.abs(delta.getDegrees()) > 90.0) {
+      return new SwerveModuleState(
+          -desiredState.speedMetersPerSecond,
+          desiredState.angle.rotateBy(Rotation2d.fromDegrees(180.0)));
+    } else {
+      return new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
+    }
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/math/Discretization.java

@@ -0,0 +1,168 @@
+// 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.math;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.Pair;
+import org.ejml.simple.SimpleMatrix;
+
+@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+public final class Discretization {
+  private Discretization() {
+    // Utility class
+  }
+
+  /**
+   * Discretizes the given continuous A matrix.
+   *
+   * @param <States> Num representing the number of states.
+   * @param contA Continuous system matrix.
+   * @param dtSeconds Discretization timestep.
+   * @return the discrete matrix system.
+   */
+  public static <States extends Num> Matrix<States, States> discretizeA(
+      Matrix<States, States> contA, double dtSeconds) {
+    return contA.times(dtSeconds).exp();
+  }
+
+  /**
+   * Discretizes the given continuous A and B matrices.
+   *
+   * <p>Rather than solving a (States + Inputs) x (States + Inputs) matrix exponential like in
+   * DiscretizeAB(), we take advantage of the structure of the block matrix of A and B.
+   *
+   * <p>1) The exponential of A*t, which is only N x N, is relatively cheap. 2) The upper-right
+   * quarter of the (States + Inputs) x (States + Inputs) matrix, which we can approximate using a
+   * taylor series to several terms and still be substantially cheaper than taking the big
+   * exponential.
+   *
+   * @param states Nat representing the states of the system.
+   * @param contA Continuous system matrix.
+   * @param contB Continuous input matrix.
+   * @param dtseconds Discretization timestep.
+   */
+  public static <States extends Num, Inputs extends Num>
+      Pair<Matrix<States, States>, Matrix<States, Inputs>> discretizeABTaylor(
+          Nat<States> states,
+          Matrix<States, States> contA,
+          Matrix<States, Inputs> contB,
+          double dtseconds) {
+    Matrix<States, States> lastTerm = Matrix.eye(states);
+    double lastCoeff = dtseconds;
+
+    var phi12 = lastTerm.times(lastCoeff);
+
+    // i = 6 i.e. 5th order should be enough precision
+    for (int i = 2; i < 6; ++i) {
+      lastTerm = contA.times(lastTerm);
+      lastCoeff *= dtseconds / ((double) i);
+
+      phi12 = phi12.plus(lastTerm.times(lastCoeff));
+    }
+
+    var discB = phi12.times(contB);
+
+    var discA = discretizeA(contA, dtseconds);
+
+    return Pair.of(discA, discB);
+  }
+
+  /**
+   * Discretizes the given continuous A and Q matrices.
+   *
+   * <p>Rather than solving a 2N x 2N matrix exponential like in DiscretizeQ() (which is expensive),
+   * we take advantage of the structure of the block matrix of A and Q.
+   *
+   * <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.
+   *
+   * @param <States> Nat representing the number of states.
+   * @param contA Continuous system matrix.
+   * @param contQ Continuous process noise covariance matrix.
+   * @param dtSeconds Discretization timestep.
+   * @return a pair representing the discrete system matrix and process noise covariance matrix.
+   */
+  @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) {
+    Matrix<States, States> Q = (contQ.plus(contQ.transpose())).div(2.0);
+
+    Matrix<States, States> lastTerm = Q.copy();
+    double lastCoeff = dtSeconds;
+
+    // A^T^n
+    Matrix<States, States> Atn = contA.transpose();
+    Matrix<States, States> phi12 = lastTerm.times(lastCoeff);
+
+    // i = 6 i.e. 6th order should be enough precision
+    for (int i = 2; i < 6; ++i) {
+      lastTerm = contA.times(-1).times(lastTerm).plus(Q.times(Atn));
+      lastCoeff *= dtSeconds / ((double) i);
+
+      phi12 = phi12.plus(lastTerm.times(lastCoeff));
+
+      Atn = Atn.times(contA.transpose());
+    }
+
+    var discA = discretizeA(contA, dtSeconds);
+    Q = discA.times(phi12);
+
+    // Make Q symmetric if it isn't already
+    var discQ = Q.plus(Q.transpose()).div(2.0);
+
+    return new Pair<>(discA, discQ);
+  }
+
+  /**
+   * Returns a discretized version of the provided continuous measurement noise covariance matrix.
+   * Note that dt=0.0 divides R by zero.
+   *
+   * @param <O> Nat representing the number of outputs.
+   * @param R 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);
+  }
+
+  /**
+   * Discretizes the given continuous A and B matrices.
+   *
+   * @param <States> Nat representing the states of the system.
+   * @param <Inputs> Nat representing the inputs to the system.
+   * @param contA Continuous system matrix.
+   * @param contB Continuous input matrix.
+   * @param dtSeconds Discretization timestep.
+   * @return a Pair representing discA and diskB.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public static <States extends Num, Inputs extends Num>
+      Pair<Matrix<States, States>, Matrix<States, Inputs>> discretizeAB(
+          Matrix<States, States> contA, Matrix<States, Inputs> contB, double dtSeconds) {
+    var scaledA = contA.times(dtSeconds);
+    var scaledB = contB.times(dtSeconds);
+
+    var contSize = contB.getNumRows() + contB.getNumCols();
+    var Mcont = new Matrix<>(new SimpleMatrix(contSize, contSize));
+    Mcont.assignBlock(0, 0, scaledA);
+    Mcont.assignBlock(0, scaledA.getNumCols(), scaledB);
+    var Mdisc = Mcont.exp();
+
+    var discA =
+        new Matrix<States, States>(new SimpleMatrix(contB.getNumRows(), contB.getNumRows()));
+    var discB =
+        new Matrix<States, Inputs>(new SimpleMatrix(contB.getNumRows(), contB.getNumCols()));
+
+    discA.extractFrom(0, 0, Mdisc);
+    discB.extractFrom(0, contB.getNumRows(), Mdisc);
+
+    return new Pair<>(discA, discB);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/math/StateSpaceUtil.java

@@ -0,0 +1,187 @@
+// 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.math;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.WPIMathJNI;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N4;
+import java.util.Random;
+import org.ejml.simple.SimpleMatrix;
+
+@SuppressWarnings("ParameterName")
+public final class StateSpaceUtil {
+  private StateSpaceUtil() {
+    // Utility class
+  }
+
+  /**
+   * Creates a covariance matrix from the given vector for use with Kalman filters.
+   *
+   * <p>Each element is squared and placed on the covariance matrix diagonal.
+   *
+   * @param <States> Num representing the states of the system.
+   * @param states A Nat representing the states of the system.
+   * @param stdDevs For a Q matrix, its elements are the standard deviations of each state from how
+   *     the model behaves. For an R matrix, its elements are the standard deviations for each
+   *     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);
+    for (int i = 0; i < states.getNum(); i++) {
+      result.set(i, i, Math.pow(stdDevs.get(i, 0), 2));
+    }
+    return result;
+  }
+
+  /**
+   * Creates a vector of normally distributed white noise with the given noise intensities for each
+   * element.
+   *
+   * @param <N> Num representing the dimensionality of the noise vector to create.
+   * @param stdDevs A matrix whose elements are the standard deviations of each element of the noise
+   *     vector.
+   * @return White noise vector.
+   */
+  public static <N extends Num> Matrix<N, N1> makeWhiteNoiseVector(Matrix<N, N1> stdDevs) {
+    var rand = new Random();
+
+    Matrix<N, N1> result = new Matrix<>(new SimpleMatrix(stdDevs.getNumRows(), 1));
+    for (int i = 0; i < stdDevs.getNumRows(); i++) {
+      result.set(i, 0, rand.nextGaussian() * stdDevs.get(i, 0));
+    }
+    return result;
+  }
+
+  /**
+   * 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.
+   *
+   * @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.
+   * @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()));
+    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)));
+    }
+
+    return result;
+  }
+
+  /**
+   * Returns true if (A, B) is a stabilizable pair.
+   *
+   * <p>(A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
+   * absolute values less than one, where an eigenvalue is uncontrollable if rank(lambda * I - A, B)
+   * %3C n where n is number of states.
+   *
+   * @param <States> Num representing the size of A.
+   * @param <Inputs> Num representing the columns of B.
+   * @param A System matrix.
+   * @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());
+  }
+
+  /**
+   * Convert a {@link Pose2d} to a vector of [x, y, theta], where theta is in radians.
+   *
+   * @param pose A pose to convert to a vector.
+   * @return The given pose in vector form, with the third element, theta, in radians.
+   */
+  public static Matrix<N3, N1> poseToVector(Pose2d pose) {
+    return VecBuilder.fill(pose.getX(), pose.getY(), pose.getRotation().getRadians());
+  }
+
+  /**
+   * Clamp the input u to the min and max.
+   *
+   * @param u The input to clamp.
+   * @param umin The minimum input magnitude.
+   * @param umax The maximum input magnitude.
+   * @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));
+    for (int i = 0; i < u.getNumRows(); i++) {
+      result.set(i, 0, MathUtil.clamp(u.get(i, 0), umin.get(i, 0), umax.get(i, 0)));
+    }
+    return result;
+  }
+
+  /**
+   * Normalize all inputs if any excedes the maximum magnitude. Useful for systems such as
+   * differential drivetrains.
+   *
+   * @param u The input vector.
+   * @param maxMagnitude The maximum magnitude any input can have.
+   * @param <I> The number of inputs.
+   * @return The normalizedInput
+   */
+  public static <I extends Num> Matrix<I, N1> normalizeInputVector(
+      Matrix<I, N1> u, double maxMagnitude) {
+    double maxValue = u.maxAbs();
+    boolean isCapped = maxValue > maxMagnitude;
+
+    if (isCapped) {
+      return u.times(maxMagnitude / maxValue);
+    }
+    return u;
+  }
+
+  /**
+   * Convert a {@link Pose2d} to a vector of [x, y, cos(theta), sin(theta)], where theta is in
+   * radians.
+   *
+   * @param pose A pose to convert to a vector.
+   */
+  public static Matrix<N4, N1> poseTo4dVector(Pose2d pose) {
+    return VecBuilder.fill(
+        pose.getTranslation().getX(),
+        pose.getTranslation().getY(),
+        pose.getRotation().getCos(),
+        pose.getRotation().getSin());
+  }
+
+  /**
+   * Convert a {@link Pose2d} to a vector of [x, y, theta], where theta is in radians.
+   *
+   * @param pose A pose to convert to a vector.
+   * @return The given pose in vector form, with the third element, theta, in radians.
+   */
+  public static Matrix<N3, N1> poseTo3dVector(Pose2d pose) {
+    return VecBuilder.fill(
+        pose.getTranslation().getX(),
+        pose.getTranslation().getY(),
+        pose.getRotation().getRadians());
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java

@@ -0,0 +1,137 @@
+// 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.spline;
+
+import org.ejml.simple.SimpleMatrix;
+
+public class CubicHermiteSpline extends Spline {
+  private static SimpleMatrix hermiteBasis;
+  private final SimpleMatrix m_coefficients;
+
+  /**
+   * Constructs a cubic hermite spline with the specified control vectors. Each control vector
+   * contains info about the location of the point and its first derivative.
+   *
+   * @param xInitialControlVector The control vector for the initial point in the x dimension.
+   * @param xFinalControlVector The control vector for the final point in the x dimension.
+   * @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,
+      double[] yInitialControlVector,
+      double[] yFinalControlVector) {
+    super(3);
+
+    // Populate the coefficients for the actual spline equations.
+    // Row 0 is x coefficients
+    // Row 1 is y coefficients
+    final var hermite = makeHermiteBasis();
+    final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+    final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+    final var xCoeffs = (hermite.mult(x)).transpose();
+    final var yCoeffs = (hermite.mult(y)).transpose();
+
+    m_coefficients = new SimpleMatrix(6, 4);
+
+    for (int i = 0; i < 4; i++) {
+      m_coefficients.set(0, i, xCoeffs.get(0, i));
+      m_coefficients.set(1, i, yCoeffs.get(0, i));
+
+      // Populate Row 2 and Row 3 with the derivatives of the equations above.
+      // Then populate row 4 and 5 with the second derivatives.
+      // Here, we are multiplying by (3 - i) to manually take the derivative. The
+      // power of the term in index 0 is 3, index 1 is 2 and so on. To find the
+      // coefficient of the derivative, we can use the power rule and multiply
+      // the existing coefficient by its power.
+      m_coefficients.set(2, i, m_coefficients.get(0, i) * (3 - i));
+      m_coefficients.set(3, i, m_coefficients.get(1, i) * (3 - i));
+    }
+
+    for (int i = 0; i < 3; i++) {
+      // Here, we are multiplying by (2 - i) to manually take the derivative. The
+      // power of the term in index 0 is 2, index 1 is 1 and so on. To find the
+      // coefficient of the derivative, we can use the power rule and multiply
+      // the existing coefficient by its power.
+      m_coefficients.set(4, i, m_coefficients.get(2, i) * (2 - i));
+      m_coefficients.set(5, i, m_coefficients.get(3, i) * (2 - i));
+    }
+  }
+
+  /**
+   * Returns the coefficients matrix.
+   *
+   * @return The coefficients matrix.
+   */
+  @Override
+  protected SimpleMatrix getCoefficients() {
+    return m_coefficients;
+  }
+
+  /**
+   * Returns the hermite basis matrix for cubic hermite spline interpolation.
+   *
+   * @return The hermite basis matrix for cubic hermite spline interpolation.
+   */
+  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.
+      //
+      // 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)    ] = [ 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 ]
+      //
+      // 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) ]
+      hermiteBasis =
+          new SimpleMatrix(
+              4,
+              4,
+              true,
+              new double[] {
+                +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 hermiteBasis;
+  }
+
+  /**
+   * Returns the control vector for each dimension as a matrix from the user-provided arrays in the
+   * constructor.
+   *
+   * @param initialVector The control vector for the initial point.
+   * @param finalVector The control vector for the final point.
+   * @return The control vector matrix for a dimension.
+   */
+  private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
+    if (initialVector.length != 2 || finalVector.length != 2) {
+      throw new IllegalArgumentException("Size of vectors must be 2");
+    }
+    return new SimpleMatrix(
+        4,
+        1,
+        true,
+        new double[] {
+          initialVector[0], initialVector[1],
+          finalVector[0], finalVector[1]
+        });
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.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.spline;
+
+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;
+
+  // Represents the curvature.
+  public double curvatureRadPerMeter;
+
+  /**
+   * Constructs a PoseWithCurvature.
+   *
+   * @param poseMeters The pose.
+   * @param curvatureRadPerMeter The curvature.
+   */
+  public PoseWithCurvature(Pose2d poseMeters, double curvatureRadPerMeter) {
+    this.poseMeters = poseMeters;
+    this.curvatureRadPerMeter = curvatureRadPerMeter;
+  }
+
+  /** Constructs a PoseWithCurvature with default values. */
+  public PoseWithCurvature() {
+    poseMeters = new Pose2d();
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java

@@ -0,0 +1,145 @@
+// 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.spline;
+
+import org.ejml.simple.SimpleMatrix;
+
+public class QuinticHermiteSpline extends Spline {
+  private static SimpleMatrix hermiteBasis;
+  private final SimpleMatrix m_coefficients;
+
+  /**
+   * Constructs a quintic hermite spline with the specified control vectors. Each control vector
+   * contains into about the location of the point, its first derivative, and its second derivative.
+   *
+   * @param xInitialControlVector The control vector for the initial point in the x dimension.
+   * @param xFinalControlVector The control vector for the final point in the x dimension.
+   * @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,
+      double[] yInitialControlVector,
+      double[] yFinalControlVector) {
+    super(5);
+
+    // Populate the coefficients for the actual spline equations.
+    // Row 0 is x coefficients
+    // Row 1 is y coefficients
+    final var hermite = makeHermiteBasis();
+    final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+    final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+    final var xCoeffs = (hermite.mult(x)).transpose();
+    final var yCoeffs = (hermite.mult(y)).transpose();
+
+    m_coefficients = new SimpleMatrix(6, 6);
+
+    for (int i = 0; i < 6; i++) {
+      m_coefficients.set(0, i, xCoeffs.get(0, i));
+      m_coefficients.set(1, i, yCoeffs.get(0, i));
+    }
+    for (int i = 0; i < 6; i++) {
+      // Populate Row 2 and Row 3 with the derivatives of the equations above.
+      // Here, we are multiplying by (5 - i) to manually take the derivative. The
+      // power of the term in index 0 is 5, index 1 is 4 and so on. To find the
+      // coefficient of the derivative, we can use the power rule and multiply
+      // the existing coefficient by its power.
+      m_coefficients.set(2, i, m_coefficients.get(0, i) * (5 - i));
+      m_coefficients.set(3, i, m_coefficients.get(1, i) * (5 - i));
+    }
+    for (int i = 0; i < 5; i++) {
+      // Then populate row 4 and 5 with the second derivatives.
+      // Here, we are multiplying by (4 - i) to manually take the derivative. The
+      // power of the term in index 0 is 4, index 1 is 3 and so on. To find the
+      // coefficient of the derivative, we can use the power rule and multiply
+      // the existing coefficient by its power.
+      m_coefficients.set(4, i, m_coefficients.get(2, i) * (4 - i));
+      m_coefficients.set(5, i, m_coefficients.get(3, i) * (4 - i));
+    }
+  }
+
+  /**
+   * Returns the coefficients matrix.
+   *
+   * @return The coefficients matrix.
+   */
+  @Override
+  protected SimpleMatrix getCoefficients() {
+    return m_coefficients;
+  }
+
+  /**
+   * Returns the hermite basis matrix for quintic hermite spline interpolation.
+   *
+   * @return The hermite basis matrix for quintic hermite spline interpolation.
+   */
+  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.
+      //
+      // 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)     ] = [  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 ]
+      //
+      // 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) ]
+      hermiteBasis =
+          new SimpleMatrix(
+              6,
+              6,
+              true,
+              new double[] {
+                -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, +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
+                +00.0, +01.0, +00.0, +00.0, +00.0, +00.0, +01.0, +00.0, +00.0, +00.0, +00.0, +00.0
+              });
+    }
+    return hermiteBasis;
+  }
+
+  /**
+   * Returns the control vector for each dimension as a matrix from the user-provided arrays in the
+   * constructor.
+   *
+   * @param initialVector The control vector for the initial point.
+   * @param finalVector The control vector for the final point.
+   * @return The control vector matrix for a dimension.
+   */
+  private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
+    if (initialVector.length != 3 || finalVector.length != 3) {
+      throw new IllegalArgumentException("Size of vectors must be 3");
+    }
+    return new SimpleMatrix(
+        6,
+        1,
+        true,
+        new double[] {
+          initialVector[0], initialVector[1], initialVector[2],
+          finalVector[0], finalVector[1], finalVector[2]
+        });
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java

@@ -0,0 +1,105 @@
+// 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.spline;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import java.util.Arrays;
+import org.ejml.simple.SimpleMatrix;
+
+/** Represents a two-dimensional parametric spline that interpolates between two points. */
+public abstract class Spline {
+  private final int m_degree;
+
+  /**
+   * Constructs a spline with the given degree.
+   *
+   * @param degree The degree of the spline.
+   */
+  Spline(int degree) {
+    m_degree = degree;
+  }
+
+  /**
+   * Returns the coefficients of the spline.
+   *
+   * @return The coefficients of the spline.
+   */
+  protected abstract SimpleMatrix getCoefficients();
+
+  /**
+   * Gets the pose and curvature at some point t on the spline.
+   *
+   * @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();
+
+    // Populate the polynomial bases.
+    for (int i = 0; i <= m_degree; i++) {
+      polynomialBases.set(i, 0, Math.pow(t, m_degree - i));
+    }
+
+    // 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.
+    SimpleMatrix combined = coefficients.mult(polynomialBases);
+
+    // Get x and y
+    final double x = combined.get(0, 0);
+    final double y = combined.get(1, 0);
+
+    double dx;
+    double dy;
+    double ddx;
+    double ddy;
+
+    if (t == 0) {
+      dx = coefficients.get(2, m_degree - 1);
+      dy = coefficients.get(3, m_degree - 1);
+      ddx = coefficients.get(4, m_degree - 2);
+      ddy = coefficients.get(5, m_degree - 2);
+    } else {
+      // Divide out t once for first derivative.
+      dx = combined.get(2, 0) / t;
+      dy = combined.get(3, 0) / t;
+
+      // Divide out t twice for second derivative.
+      ddx = combined.get(4, 0) / t / t;
+      ddy = combined.get(5, 0) / t / t;
+    }
+
+    // Find the curvature.
+    final double curvature = (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.hypot(dx, dy));
+
+    return new PoseWithCurvature(new Pose2d(x, y, new Rotation2d(dx, dy)), curvature);
+  }
+
+  /**
+   * Represents a control vector for a spline.
+   *
+   * <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;
+
+    /**
+     * Instantiates a control vector.
+     *
+     * @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);
+    }
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java

@@ -0,0 +1,274 @@
+// 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.spline;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import java.util.Arrays;
+import java.util.List;
+
+public final class SplineHelper {
+  /** Private constructor because this is a utility class. */
+  private SplineHelper() {}
+
+  /**
+   * Returns 2 cubic control vectors from a set of exterior waypoints and interior translations.
+   *
+   * @param start The starting pose.
+   * @param interiorWaypoints The interior waypoints.
+   * @param end The ending pose.
+   * @return 2 cubic control vectors.
+   */
+  public static Spline.ControlVector[] getCubicControlVectorsFromWaypoints(
+      Pose2d start, Translation2d[] interiorWaypoints, Pose2d end) {
+    // Generate control vectors from poses.
+    Spline.ControlVector initialCV;
+    Spline.ControlVector endCV;
+
+    // Chooses a magnitude automatically that makes the splines look better.
+    if (interiorWaypoints.length < 1) {
+      double scalar = start.getTranslation().getDistance(end.getTranslation()) * 1.2;
+      initialCV = getCubicControlVector(scalar, start);
+      endCV = getCubicControlVector(scalar, end);
+    } else {
+      double scalar = start.getTranslation().getDistance(interiorWaypoints[0]) * 1.2;
+      initialCV = getCubicControlVector(scalar, start);
+      scalar =
+          end.getTranslation().getDistance(interiorWaypoints[interiorWaypoints.length - 1]) * 1.2;
+      endCV = getCubicControlVector(scalar, end);
+    }
+    return new Spline.ControlVector[] {initialCV, endCV};
+  }
+
+  /**
+   * Returns quintic splines from a set of waypoints.
+   *
+   * @param waypoints The waypoints
+   * @return List of splines.
+   */
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  public static QuinticHermiteSpline[] getQuinticSplinesFromWaypoints(List<Pose2d> waypoints) {
+    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[waypoints.size() - 1];
+    for (int i = 0; i < waypoints.size() - 1; ++i) {
+      var p0 = waypoints.get(i);
+      var p1 = waypoints.get(i + 1);
+
+      // This just makes the splines look better.
+      final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation());
+
+      var controlVecA = getQuinticControlVector(scalar, p0);
+      var controlVecB = getQuinticControlVector(scalar, p1);
+
+      splines[i] =
+          new QuinticHermiteSpline(controlVecA.x, controlVecB.x, controlVecA.y, controlVecB.y);
+    }
+    return splines;
+  }
+
+  /**
+   * Returns a set of cubic splines corresponding to the provided control vectors. The user is free
+   * to set the direction of the start and end point. The directions for the middle waypoints are
+   * determined automatically to ensure continuous curvature throughout the path.
+   *
+   * @param start The starting control vector.
+   * @param waypoints The middle waypoints. This can be left blank if you only wish to create a path
+   *     with two waypoints.
+   * @param end The ending control vector.
+   * @return A vector of cubic hermite splines that interpolate through the provided waypoints and
+   *     control vectors.
+   */
+  @SuppressWarnings({
+    "LocalVariableName",
+    "PMD.ExcessiveMethodLength",
+    "PMD.AvoidInstantiatingObjectsInLoops"
+  })
+  public static CubicHermiteSpline[] getCubicSplinesFromControlVectors(
+      Spline.ControlVector start, Translation2d[] waypoints, Spline.ControlVector end) {
+
+    CubicHermiteSpline[] splines = new CubicHermiteSpline[waypoints.length + 1];
+
+    double[] xInitial = start.x;
+    double[] yInitial = start.y;
+    double[] xFinal = end.x;
+    double[] yFinal = end.y;
+
+    if (waypoints.length > 1) {
+      Translation2d[] newWaypts = new Translation2d[waypoints.length + 2];
+
+      // Create an array of all waypoints, including the start and end.
+      newWaypts[0] = new Translation2d(xInitial[0], yInitial[0]);
+      System.arraycopy(waypoints, 0, newWaypts, 1, waypoints.length);
+      newWaypts[newWaypts.length - 1] = new Translation2d(xFinal[0], yFinal[0]);
+
+      // Populate tridiagonal system for clamped cubic
+      /* See:
+      https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08
+      /undervisningsmateriale/chap7alecture.pdf
+      */
+      // Above-diagonal of tridiagonal matrix, zero-padded
+      final double[] a = new double[newWaypts.length - 2];
+
+      // Diagonal of tridiagonal matrix
+      final double[] b = new double[newWaypts.length - 2];
+      Arrays.fill(b, 4.0);
+
+      // Below-diagonal of tridiagonal matrix, zero-padded
+      final double[] c = new double[newWaypts.length - 2];
+
+      // rhs vectors
+      final double[] dx = new double[newWaypts.length - 2];
+      final double[] dy = new double[newWaypts.length - 2];
+
+      // solution vectors
+      final double[] fx = new double[newWaypts.length - 2];
+      final double[] fy = new double[newWaypts.length - 2];
+
+      // populate above-diagonal and below-diagonal vectors
+      a[0] = 0.0;
+      for (int i = 0; i < newWaypts.length - 3; i++) {
+        a[i + 1] = 1;
+        c[i] = 1;
+      }
+      c[c.length - 1] = 0.0;
+
+      // populate rhs vectors
+      dx[0] = 3 * (newWaypts[2].getX() - newWaypts[0].getX()) - xInitial[1];
+      dy[0] = 3 * (newWaypts[2].getY() - newWaypts[0].getY()) - yInitial[1];
+
+      if (newWaypts.length > 4) {
+        for (int i = 1; i <= newWaypts.length - 4; i++) {
+          // dx and dy represent the derivatives of the internal waypoints. The derivative
+          // of the second internal waypoint should involve the third and first internal waypoint,
+          // which have indices of 1 and 3 in the newWaypts list (which contains ALL waypoints).
+          dx[i] = 3 * (newWaypts[i + 2].getX() - newWaypts[i].getX());
+          dy[i] = 3 * (newWaypts[i + 2].getY() - newWaypts[i].getY());
+        }
+      }
+
+      dx[dx.length - 1] =
+          3 * (newWaypts[newWaypts.length - 1].getX() - newWaypts[newWaypts.length - 3].getX())
+              - xFinal[1];
+      dy[dy.length - 1] =
+          3 * (newWaypts[newWaypts.length - 1].getY() - newWaypts[newWaypts.length - 3].getY())
+              - yFinal[1];
+
+      // Compute solution to tridiagonal system
+      thomasAlgorithm(a, b, c, dx, fx);
+      thomasAlgorithm(a, b, c, dy, fy);
+
+      double[] newFx = new double[fx.length + 2];
+      double[] newFy = new double[fy.length + 2];
+
+      newFx[0] = xInitial[1];
+      newFy[0] = yInitial[1];
+      System.arraycopy(fx, 0, newFx, 1, fx.length);
+      System.arraycopy(fy, 0, newFy, 1, fy.length);
+      newFx[newFx.length - 1] = xFinal[1];
+      newFy[newFy.length - 1] = yFinal[1];
+
+      for (int i = 0; i < newFx.length - 1; i++) {
+        splines[i] =
+            new CubicHermiteSpline(
+                new double[] {newWaypts[i].getX(), newFx[i]},
+                new double[] {newWaypts[i + 1].getX(), newFx[i + 1]},
+                new double[] {newWaypts[i].getY(), newFy[i]},
+                new double[] {newWaypts[i + 1].getY(), newFy[i + 1]});
+      }
+    } else if (waypoints.length == 1) {
+      final var xDeriv = (3 * (xFinal[0] - xInitial[0]) - xFinal[1] - xInitial[1]) / 4.0;
+      final var yDeriv = (3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
+
+      double[] midXControlVector = {waypoints[0].getX(), xDeriv};
+      double[] midYControlVector = {waypoints[0].getY(), yDeriv};
+
+      splines[0] =
+          new CubicHermiteSpline(
+              xInitial, midXControlVector,
+              yInitial, midYControlVector);
+      splines[1] =
+          new CubicHermiteSpline(
+              midXControlVector, xFinal,
+              midYControlVector, yFinal);
+    } else {
+      splines[0] =
+          new CubicHermiteSpline(
+              xInitial, xFinal,
+              yInitial, yFinal);
+    }
+    return splines;
+  }
+
+  /**
+   * Returns a set of quintic splines corresponding to the provided control vectors. The user is
+   * free to set the direction of all control vectors. Continuous curvature is guaranteed throughout
+   * the path.
+   *
+   * @param controlVectors The control vectors.
+   * @return A vector of quintic hermite splines that interpolate through the provided waypoints.
+   */
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors(
+      Spline.ControlVector[] controlVectors) {
+    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length - 1];
+    for (int i = 0; i < controlVectors.length - 1; i++) {
+      var xInitial = controlVectors[i].x;
+      var xFinal = controlVectors[i + 1].x;
+      var yInitial = controlVectors[i].y;
+      var yFinal = controlVectors[i + 1].y;
+      splines[i] =
+          new QuinticHermiteSpline(
+              xInitial, xFinal,
+              yInitial, yFinal);
+    }
+    return splines;
+  }
+
+  /**
+   * Thomas algorithm for solving tridiagonal systems Af = d.
+   *
+   * @param a the values of A above the diagonal
+   * @param b the values of A on the diagonal
+   * @param c the values of A below the diagonal
+   * @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;
+
+    double[] cStar = new double[N];
+    double[] dStar = new double[N];
+
+    // This updates the coefficients in the first row
+    // Note that we should be checking for division by zero here
+    cStar[0] = c[0] / b[0];
+    dStar[0] = d[0] / b[0];
+
+    // Create the c_star and d_star coefficients in the forward sweep
+    for (int i = 1; i < N; i++) {
+      double m = 1.0 / (b[i] - a[i] * cStar[i - 1]);
+      cStar[i] = c[i] * m;
+      dStar[i] = (d[i] - a[i] * dStar[i - 1]) * m;
+    }
+    solutionVector[N - 1] = dStar[N - 1];
+    // This is the reverse sweep, used to update the solution vector f
+    for (int i = N - 2; i >= 0; i--) {
+      solutionVector[i] = dStar[i] - cStar[i] * solutionVector[i + 1];
+    }
+  }
+
+  private static Spline.ControlVector getCubicControlVector(double scalar, Pose2d point) {
+    return new Spline.ControlVector(
+        new double[] {point.getX(), scalar * point.getRotation().getCos()},
+        new double[] {point.getY(), scalar * point.getRotation().getSin()});
+  }
+
+  private static Spline.ControlVector getQuinticControlVector(double scalar, Pose2d point) {
+    return new Spline.ControlVector(
+        new double[] {point.getX(), scalar * point.getRotation().getCos(), 0.0},
+        new double[] {point.getY(), scalar * point.getRotation().getSin(), 0.0});
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java

@@ -0,0 +1,142 @@
+// 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.
+
+/*
+ * MIT License
+ *
+ * Copyright (c) 2018 Team 254
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package edu.wpi.first.math.spline;
+
+import java.util.ArrayDeque;
+import java.util.ArrayList;
+import java.util.List;
+
+/** Class used to parameterize a spline by its arc length. */
+public final class SplineParameterizer {
+  private static final double kMaxDx = 0.127;
+  private static final double kMaxDy = 0.00127;
+  private static final double kMaxDtheta = 0.0872;
+
+  /**
+   * A malformed spline does not actually explode the LIFO stack size. Instead, the stack size stays
+   * at a relatively small number (e.g. 30) and never decreases. Because of this, we must count
+   * iterations. Even long, complex paths don't usually go over 300 iterations, so hitting this
+   * maximum should definitely indicate something has gone wrong.
+   */
+  private static final int kMaxIterations = 5000;
+
+  @SuppressWarnings("MemberName")
+  private static class StackContents {
+    final double t1;
+    final double t0;
+
+    StackContents(double t0, double t1) {
+      this.t0 = t0;
+      this.t1 = t1;
+    }
+  }
+
+  @SuppressWarnings("serial")
+  public static class MalformedSplineException extends RuntimeException {
+    /**
+     * Create a new exception with the given message.
+     *
+     * @param message the message to pass with the exception
+     */
+    private MalformedSplineException(String message) {
+      super(message);
+    }
+  }
+
+  /** Private constructor because this is a utility class. */
+  private SplineParameterizer() {}
+
+  /**
+   * Parameterizes the spline. This method breaks up the spline into various arcs until their dx,
+   * dy, and dtheta are within specific tolerances.
+   *
+   * @param spline The spline to parameterize.
+   * @return A list of poses and curvatures that represents various points on the spline.
+   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+   *     with approximately opposing headings)
+   */
+  public static List<PoseWithCurvature> parameterize(Spline spline) {
+    return parameterize(spline, 0.0, 1.0);
+  }
+
+  /**
+   * Parameterizes the spline. This method breaks up the spline into various arcs until their dx,
+   * dy, and dtheta are within specific tolerances.
+   *
+   * @param spline The spline to parameterize.
+   * @param t0 Starting internal spline parameter. It is recommended to use 0.0.
+   * @param t1 Ending internal spline parameter. It is recommended to use 1.0.
+   * @return A list of poses and curvatures that represents various points on the spline.
+   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+   *     with approximately opposing headings)
+   */
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  public static List<PoseWithCurvature> parameterize(Spline spline, double t0, double t1) {
+    var splinePoints = new ArrayList<PoseWithCurvature>();
+
+    // The parameterization does not add the initial point. Let's add that.
+    splinePoints.add(spline.getPoint(t0));
+
+    // We use an "explicit stack" to simulate recursion, instead of a recursive function call
+    // This give us greater control, instead of a stack overflow
+    var stack = new ArrayDeque<StackContents>();
+    stack.push(new StackContents(t0, t1));
+
+    StackContents current;
+    PoseWithCurvature start;
+    PoseWithCurvature end;
+    int iterations = 0;
+
+    while (!stack.isEmpty()) {
+      current = stack.removeFirst();
+      start = spline.getPoint(current.t0);
+      end = spline.getPoint(current.t1);
+
+      final var twist = start.poseMeters.log(end.poseMeters);
+      if (Math.abs(twist.dy) > kMaxDy
+          || Math.abs(twist.dx) > kMaxDx
+          || Math.abs(twist.dtheta) > kMaxDtheta) {
+        stack.addFirst(new StackContents((current.t0 + current.t1) / 2, current.t1));
+        stack.addFirst(new StackContents(current.t0, (current.t0 + current.t1) / 2));
+      } else {
+        splinePoints.add(spline.getPoint(current.t1));
+      }
+
+      iterations++;
+      if (iterations >= kMaxIterations) {
+        throw new MalformedSplineException(
+            "Could not parameterize a malformed spline. This means that you probably had two or "
+                + " more adjacent waypoints that were very close together with headings in "
+                + "opposing directions.");
+      }
+    }
+
+    return splinePoints;
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java

@@ -0,0 +1,170 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.math.Discretization;
+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.
+   */
+  @SuppressWarnings("ParameterName")
+  public LinearSystem(
+      Matrix<States, States> a,
+      Matrix<States, Inputs> b,
+      Matrix<Outputs, States> c,
+      Matrix<Outputs, Inputs> d) {
+
+    this.m_A = a;
+    this.m_B = b;
+    this.m_C = c;
+    this.m_D = d;
+  }
+
+  /**
+   * Returns the system matrix A.
+   *
+   * @return the system matrix A.
+   */
+  public Matrix<States, States> getA() {
+    return m_A;
+  }
+
+  /**
+   * Returns an element of the system matrix A.
+   *
+   * @param row Row of A.
+   * @param col Column of A.
+   * @return the system matrix A at (i, j).
+   */
+  public double getA(int row, int col) {
+    return m_A.get(row, col);
+  }
+
+  /**
+   * Returns the input matrix B.
+   *
+   * @return the input matrix B.
+   */
+  public Matrix<States, Inputs> getB() {
+    return m_B;
+  }
+
+  /**
+   * Returns an element of the input matrix B.
+   *
+   * @param row Row of B.
+   * @param col Column of B.
+   * @return The value of the input matrix B at (i, j).
+   */
+  public double getB(int row, int col) {
+    return m_B.get(row, col);
+  }
+
+  /**
+   * Returns the output matrix C.
+   *
+   * @return Output matrix C.
+   */
+  public Matrix<Outputs, States> getC() {
+    return m_C;
+  }
+
+  /**
+   * Returns an element of the output matrix C.
+   *
+   * @param row Row of C.
+   * @param col Column of C.
+   * @return the double value of C at the given position.
+   */
+  public double getC(int row, int col) {
+    return m_C.get(row, col);
+  }
+
+  /**
+   * Returns the feedthrough matrix D.
+   *
+   * @return the feedthrough matrix D.
+   */
+  public Matrix<Outputs, Inputs> getD() {
+    return m_D;
+  }
+
+  /**
+   * Returns an element of the feedthrough matrix D.
+   *
+   * @param row Row of D.
+   * @param col Column of D.
+   * @return The feedthrough matrix D at (i, j).
+   */
+  public double getD(int row, int col) {
+    return m_D.get(row, col);
+  }
+
+  /**
+   * Computes the new x given the old x and the control input.
+   *
+   * <p>This is used by state observers directly to run updates based on state estimate.
+   *
+   * @param x The current state.
+   * @param clampedU The control input.
+   * @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);
+
+    return (discABpair.getFirst().times(x)).plus(discABpair.getSecond().times(clampedU));
+  }
+
+  /**
+   * Computes the new y given the control input.
+   *
+   * <p>This is used by state observers directly to run updates based on state estimate.
+   *
+   * @param x The current state.
+   * @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));
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "Linear System: A\n%s\n\nB:\n%s\n\nC:\n%s\n\nD:\n%s\n",
+        m_A.toString(), m_B.toString(), m_C.toString(), m_D.toString());
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java

@@ -0,0 +1,346 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.controller.LinearPlantInversionFeedforward;
+import edu.wpi.first.math.controller.LinearQuadraticRegulator;
+import edu.wpi.first.math.estimator.KalmanFilter;
+import edu.wpi.first.math.math.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import java.util.function.Function;
+import org.ejml.MatrixDimensionException;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * Combines a controller, feedforward, and observer for controlling a mechanism with full state
+ * feedback.
+ *
+ * <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
+ * 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;
+  private final KalmanFilter<States, Inputs, Outputs> m_observer;
+  private Matrix<States, N1> m_nextR;
+  private Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> m_clampFunction;
+
+  /**
+   * Constructs a state-space loop with the given plant, controller, and observer. By default, the
+   * initial reference is all zeros. Users should call reset with the initial system state before
+   * enabling the loop. This constructor assumes that the input(s) to this system are voltage.
+   *
+   * @param plant State-space plant.
+   * @param controller State-space controller.
+   * @param observer State-space observer.
+   * @param maxVoltageVolts The maximum voltage that can be applied. Commonly 12.
+   * @param dtSeconds The nominal timestep.
+   */
+  public LinearSystemLoop(
+      LinearSystem<States, Inputs, Outputs> plant,
+      LinearQuadraticRegulator<States, Inputs, Outputs> controller,
+      KalmanFilter<States, Inputs, Outputs> observer,
+      double maxVoltageVolts,
+      double dtSeconds) {
+    this(
+        controller,
+        new LinearPlantInversionFeedforward<>(plant, dtSeconds),
+        observer,
+        u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts));
+  }
+
+  /**
+   * Constructs a state-space loop with the given plant, controller, and observer. By default, the
+   * initial reference is all zeros. Users should call reset with the initial system state before
+   * enabling the loop.
+   *
+   * @param plant State-space plant.
+   * @param controller State-space controller.
+   * @param observer State-space observer.
+   * @param clampFunction The function used to clamp the input U.
+   * @param dtSeconds The nominal timestep.
+   */
+  public LinearSystemLoop(
+      LinearSystem<States, Inputs, Outputs> plant,
+      LinearQuadraticRegulator<States, Inputs, Outputs> controller,
+      KalmanFilter<States, Inputs, Outputs> observer,
+      Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction,
+      double dtSeconds) {
+    this(
+        controller,
+        new LinearPlantInversionFeedforward<>(plant, dtSeconds),
+        observer,
+        clampFunction);
+  }
+
+  /**
+   * Constructs a state-space loop with the given controller, feedforward and observer. By default,
+   * the initial reference is all zeros. Users should call reset with the initial system state
+   * before enabling the loop.
+   *
+   * @param controller State-space controller.
+   * @param feedforward Plant inversion feedforward.
+   * @param observer State-space observer.
+   * @param maxVoltageVolts The maximum voltage that can be applied. Assumes that the inputs are
+   *     voltages.
+   */
+  public LinearSystemLoop(
+      LinearQuadraticRegulator<States, Inputs, Outputs> controller,
+      LinearPlantInversionFeedforward<States, Inputs, Outputs> feedforward,
+      KalmanFilter<States, Inputs, Outputs> observer,
+      double maxVoltageVolts) {
+    this(
+        controller,
+        feedforward,
+        observer,
+        u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts));
+  }
+
+  /**
+   * Constructs a state-space loop with the given controller, feedforward, and observer. By default,
+   * the initial reference is all zeros. Users should call reset with the initial system state
+   * before enabling the loop.
+   *
+   * @param controller State-space controller.
+   * @param feedforward Plant inversion feedforward.
+   * @param observer State-space observer.
+   * @param clampFunction The function used to clamp the input U.
+   */
+  public LinearSystemLoop(
+      LinearQuadraticRegulator<States, Inputs, Outputs> controller,
+      LinearPlantInversionFeedforward<States, Inputs, Outputs> feedforward,
+      KalmanFilter<States, Inputs, Outputs> observer,
+      Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction) {
+    this.m_controller = controller;
+    this.m_feedforward = feedforward;
+    this.m_observer = observer;
+    this.m_clampFunction = clampFunction;
+
+    m_nextR = new Matrix<>(new SimpleMatrix(controller.getK().getNumCols(), 1));
+    reset(m_nextR);
+  }
+
+  /**
+   * Returns the observer's state estimate x-hat.
+   *
+   * @return the observer's state estimate x-hat.
+   */
+  public Matrix<States, N1> getXHat() {
+    return getObserver().getXhat();
+  }
+
+  /**
+   * Returns an element of the observer's state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @return the i-th element of the observer's state estimate x-hat.
+   */
+  public double getXHat(int row) {
+    return getObserver().getXhat(row);
+  }
+
+  /**
+   * Set the initial state estimate x-hat.
+   *
+   * @param xhat The initial state estimate x-hat.
+   */
+  public void setXHat(Matrix<States, N1> xhat) {
+    getObserver().setXhat(xhat);
+  }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  public void setXHat(int row, double value) {
+    getObserver().setXhat(row, value);
+  }
+
+  /**
+   * Returns an element of the controller's next reference r.
+   *
+   * @param row Row of r.
+   * @return the element i of the controller's next reference r.
+   */
+  public double getNextR(int row) {
+    return getNextR().get(row, 0);
+  }
+
+  /**
+   * Returns the controller's next reference r.
+   *
+   * @return the controller's next reference r.
+   */
+  public Matrix<States, N1> getNextR() {
+    return m_nextR;
+  }
+
+  /**
+   * Set the next reference r.
+   *
+   * @param nextR Next reference.
+   */
+  public void setNextR(Matrix<States, N1> nextR) {
+    m_nextR = nextR;
+  }
+
+  /**
+   * Set the next reference r.
+   *
+   * @param nextR Next reference.
+   */
+  public void setNextR(double... nextR) {
+    if (nextR.length != m_nextR.getNumRows()) {
+      throw new MatrixDimensionException(
+          String.format(
+              "The next reference does not have the "
+                  + "correct number of entries! Expected %s, but got %s.",
+              m_nextR.getNumRows(), nextR.length));
+    }
+    m_nextR = new Matrix<>(new SimpleMatrix(m_nextR.getNumRows(), 1, true, nextR));
+  }
+
+  /**
+   * Returns the controller's calculated control input u plus the calculated feedforward u_ff.
+   *
+   * @return the calculated control input u.
+   */
+  public Matrix<Inputs, N1> getU() {
+    return clampInput(m_controller.getU().plus(m_feedforward.getUff()));
+  }
+
+  /**
+   * Returns an element of the controller's calculated control input u.
+   *
+   * @param row Row of u.
+   * @return the calculated control input u at the row i.
+   */
+  public double getU(int row) {
+    return getU().get(row, 0);
+  }
+
+  /**
+   * Return the controller used internally.
+   *
+   * @return the controller used internally.
+   */
+  public LinearQuadraticRegulator<States, Inputs, Outputs> getController() {
+    return m_controller;
+  }
+
+  /**
+   * Return the feedforward used internally.
+   *
+   * @return the feedforward used internally.
+   */
+  public LinearPlantInversionFeedforward<States, Inputs, Outputs> getFeedforward() {
+    return m_feedforward;
+  }
+
+  /**
+   * Return the observer used internally.
+   *
+   * @return the observer used internally.
+   */
+  public KalmanFilter<States, Inputs, Outputs> getObserver() {
+    return m_observer;
+  }
+
+  /**
+   * Zeroes reference r and controller output u. The previous reference of the
+   * PlantInversionFeedforward and the initial state estimate of the KalmanFilter are set to the
+   * initial state provided.
+   *
+   * @param initialState The initial state.
+   */
+  public void reset(Matrix<States, N1> initialState) {
+    m_nextR.fill(0.0);
+    m_controller.reset();
+    m_feedforward.reset(initialState);
+    m_observer.setXhat(initialState);
+  }
+
+  /**
+   * Returns difference between reference r and current state x-hat.
+   *
+   * @return The state error matrix.
+   */
+  public Matrix<States, N1> getError() {
+    return getController().getR().minus(m_observer.getXhat());
+  }
+
+  /**
+   * Returns difference between reference r and current state x-hat.
+   *
+   * @param index The index of the error matrix to return.
+   * @return The error at that index.
+   */
+  public double getError(int index) {
+    return (getController().getR().minus(m_observer.getXhat())).get(index, 0);
+  }
+
+  /**
+   * Get the function used to clamp the input u.
+   *
+   * @return The clamping function.
+   */
+  public Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> getClampFunction() {
+    return m_clampFunction;
+  }
+
+  /** Set the clamping function used to clamp inputs. */
+  public void setClampFunction(Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction) {
+    this.m_clampFunction = clampFunction;
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param y Measurement vector.
+   */
+  @SuppressWarnings("ParameterName")
+  public void correct(Matrix<Outputs, N1> y) {
+    getObserver().correct(getU(), y);
+  }
+
+  /**
+   * Sets new controller output, projects model forward, and runs observer prediction.
+   *
+   * <p>After calling this, the user should send the elements of u to the actuators.
+   *
+   * @param dtSeconds Timestep for model update.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public void predict(double dtSeconds) {
+    var u =
+        clampInput(
+            m_controller
+                .calculate(getObserver().getXhat(), m_nextR)
+                .plus(m_feedforward.calculate(m_nextR)));
+    getObserver().predict(u, dtSeconds);
+  }
+
+  /**
+   * Clamp the input u to the min and max.
+   *
+   * @param unclampedU The input to clamp.
+   * @return The clamped input.
+   */
+  public Matrix<Inputs, N1> clampInput(Matrix<Inputs, N1> unclampedU) {
+    return m_clampFunction.apply(unclampedU);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java

@@ -0,0 +1,290 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.numbers.N6;
+import java.util.function.BiFunction;
+import java.util.function.DoubleFunction;
+import java.util.function.Function;
+
+public final class NumericalIntegration {
+  private NumericalIntegration() {
+    // utility Class
+  }
+
+  /**
+   * Performs Runge Kutta integration (4th order).
+   *
+   * @param f The function to integrate, which takes one argument x.
+   * @param x The initial value of x.
+   * @param dtSeconds The time over which to integrate.
+   * @return the integration of dx/dt = f(x) for dt.
+   */
+  @SuppressWarnings("ParameterName")
+  public static double rk4(DoubleFunction<Double> f, double x, double dtSeconds) {
+    final var halfDt = 0.5 * dtSeconds;
+    final var k1 = f.apply(x);
+    final var k2 = f.apply(x + k1 * halfDt);
+    final var k3 = f.apply(x + k2 * halfDt);
+    final var k4 = f.apply(x + k3 * dtSeconds);
+    return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+  }
+
+  /**
+   * Performs Runge Kutta integration (4th order).
+   *
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dtSeconds The time over which to integrate.
+   * @return The result of Runge Kutta integration (4th order).
+   */
+  @SuppressWarnings("ParameterName")
+  public static double rk4(
+      BiFunction<Double, Double, Double> f, double x, Double u, double dtSeconds) {
+    final var halfDt = 0.5 * dtSeconds;
+    final var k1 = f.apply(x, u);
+    final var k2 = f.apply(x + k1 * halfDt, u);
+    final var k3 = f.apply(x + k2 * halfDt, u);
+    final var k4 = f.apply(x + k3 * dtSeconds, u);
+    return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+  }
+
+  /**
+   * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
+   *
+   * @param <States> A Num representing the states of the system to integrate.
+   * @param <Inputs> A Num representing the inputs of the system to integrate.
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dtSeconds The time over which to integrate.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rk4(
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x,
+      Matrix<Inputs, N1> u,
+      double dtSeconds) {
+
+    final var halfDt = 0.5 * dtSeconds;
+    Matrix<States, N1> k1 = f.apply(x, u);
+    Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)), u);
+    Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)), u);
+    Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)), u);
+    return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
+  }
+
+  /**
+   * Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
+   *
+   * @param <States> A Num prepresenting the states of the system.
+   * @param f The function to integrate. It must take one argument x.
+   * @param x The initial value of x.
+   * @param dtSeconds The time over which to integrate.
+   * @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
+   */
+  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  public static <States extends Num> Matrix<States, N1> rk4(
+      Function<Matrix<States, N1>, Matrix<States, N1>> f, Matrix<States, N1> x, double dtSeconds) {
+
+    final var halfDt = 0.5 * dtSeconds;
+    Matrix<States, N1> k1 = f.apply(x);
+    Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)));
+    Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)));
+    Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)));
+    return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
+  }
+
+  /**
+   * Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described in
+   * https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method. By default, the max
+   * error is 1e-6.
+   *
+   * @param <States> A Num representing the states of the system to integrate.
+   * @param <Inputs> A Num representing the inputs of the system to integrate.
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dtSeconds The time over which to integrate.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkf45(
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x,
+      Matrix<Inputs, N1> u,
+      double dtSeconds) {
+    return rkf45(f, x, u, dtSeconds, 1e-6);
+  }
+
+  /**
+   * Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described in
+   * https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
+   *
+   * @param <States> A Num representing the states of the system to integrate.
+   * @param <Inputs> A Num representing the inputs of the system to integrate.
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dtSeconds The time over which to integrate.
+   * @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkf45(
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x,
+      Matrix<Inputs, N1> u,
+      double dtSeconds,
+      double maxError) {
+
+    double dtElapsed = 0;
+    double previousH = dtSeconds;
+    // Loop until we've gotten to our desired dt
+    while (dtElapsed < dtSeconds) {
+      // RKF45 will give us an updated x and a dt back.
+      // We use the new dt (h) as the initial dt for our next loop
+      var ret = rkf45Impl(f, x, u, previousH, maxError, dtSeconds - dtElapsed);
+      dtElapsed += ret.getSecond();
+      previousH = ret.getSecond();
+      x = ret.getFirst();
+    }
+    return x;
+  }
+
+  static final double[] ch = {47 / 450.0, 0, 12 / 25.0, 32 / 225.0, 1 / 30.0, 6 / 25.0};
+  static final double[] ct = {-1 / 150.0, 0, 3 / 100.0, -16 / 75.0, -1 / 20.0, 6 / 25.0};
+  static final Matrix<N6, N5> Bs =
+      Matrix.mat(Nat.N6(), Nat.N5())
+          .fill(
+              0,
+              0,
+              0,
+              0,
+              0,
+              2 / 9.0,
+              0,
+              0,
+              0,
+              0,
+              1 / 12.0,
+              1 / 4.0,
+              0,
+              0,
+              0,
+              69 / 128.0,
+              -243 / 128.0,
+              135 / 64.0,
+              0,
+              0,
+              -17 / 12.0,
+              27 / 4.0,
+              -27 / 5.0,
+              16 / 15.0,
+              0,
+              65 / 432.0,
+              -5 / 16.0,
+              13 / 16.0,
+              4 / 27.0,
+              5 / 144.0);
+
+  /**
+   * Implements one loop of RKF45. This takes an initial state, dt guess, and max truncation error,
+   * and returns a new x and the dt over which that x was updated. This should be called until there
+   * is no dt remaining.
+   *
+   * @param <States> Num representing the states of the system to integrate.
+   * @param <Inputs> A Num representing the inputs of the system to integrate.
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param initialH The initial dt guess. This is refined to clamp truncation error to the
+   *     specified max.
+   * @param maxTruncationError The max truncation error acceptable. Usually a small number like
+   *     1e-6.
+   * @param dtRemaining How much time is left to integrate over. Used to clamp h.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  private static <States extends Num, Inputs extends Num>
+      Pair<Matrix<States, N1>, Double> rkf45Impl(
+          BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+          Matrix<States, N1> x,
+          Matrix<Inputs, N1> u,
+          double initialH,
+          double maxTruncationError,
+          double dtRemaining) {
+
+    double truncationErr;
+    double h = initialH;
+    Matrix<States, N1> newX;
+
+    do {
+      // only allow us to advance up to the dt remaining
+      h = Math.min(h, dtRemaining);
+
+      // Notice how the derivative in the Wikipedia notation is dy/dx.
+      // That means their y is our x and their x is our t
+      Matrix<States, N1> k1 = f.apply(x, u).times(h);
+      Matrix<States, N1> k2 = f.apply(x.plus(k1.times(Bs.get(1, 0))), u).times(h);
+      Matrix<States, N1> k3 =
+          f.apply(x.plus(k1.times(Bs.get(2, 0))).plus(k2.times(Bs.get(2, 1))), u).times(h);
+      Matrix<States, N1> k4 =
+          f.apply(
+                  x.plus(k1.times(Bs.get(3, 0)))
+                      .plus(k2.times(Bs.get(3, 1)))
+                      .plus(k3.times(Bs.get(3, 2))),
+                  u)
+              .times(h);
+      Matrix<States, N1> k5 =
+          f.apply(
+                  x.plus(k1.times(Bs.get(4, 0)))
+                      .plus(k2.times(Bs.get(4, 1)))
+                      .plus(k3.times(Bs.get(4, 2)))
+                      .plus(k4.times(Bs.get(4, 3))),
+                  u)
+              .times(h);
+      Matrix<States, N1> k6 =
+          f.apply(
+                  x.plus(k1.times(Bs.get(5, 0)))
+                      .plus(k2.times(Bs.get(5, 1)))
+                      .plus(k3.times(Bs.get(5, 2)))
+                      .plus(k4.times(Bs.get(5, 3)))
+                      .plus(k5.times(Bs.get(5, 4))),
+                  u)
+              .times(h);
+
+      newX =
+          x.plus(k1.times(ch[0]))
+              .plus(k2.times(ch[1]))
+              .plus(k3.times(ch[2]))
+              .plus(k4.times(ch[3]))
+              .plus(k5.times(ch[4]))
+              .plus(k6.times(ch[5]));
+
+      truncationErr =
+          k1.times(ct[0])
+              .plus(k2.times(ct[1]))
+              .plus(k3.times(ct[2]))
+              .plus(k4.times(ct[3]))
+              .plus(k5.times(ct[4]))
+              .plus(k6.times(ct[5]))
+              .normF();
+
+      h = 0.9 * h * Math.pow(maxTruncationError / truncationErr, 1 / 5.0);
+    } while (truncationErr > maxTruncationError);
+
+    // Return the new x, and the timestep
+    return Pair.of(newX, h);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/system/NumericalJacobian.java

@@ -0,0 +1,104 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+import java.util.function.BiFunction;
+import java.util.function.Function;
+
+public final class NumericalJacobian {
+  private NumericalJacobian() {
+    // Utility Class.
+  }
+
+  private static final double kEpsilon = 1e-5;
+
+  /**
+   * Computes the numerical Jacobian with respect to x for f(x).
+   *
+   * @param <Rows> Number of rows in the result of f(x).
+   * @param <States> Num representing the number of rows in the output of f.
+   * @param <Cols> Number of columns in the result of f(x).
+   * @param rows Number of rows in the result of f(x).
+   * @param cols Number of columns in the result of f(x).
+   * @param f Vector-valued function from which to compute the Jacobian.
+   * @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,
+          Nat<Cols> cols,
+          Function<Matrix<Cols, N1>, Matrix<States, N1>> f,
+          Matrix<Cols, N1> x) {
+    var result = new Matrix<>(rows, cols);
+
+    for (int i = 0; i < cols.getNum(); i++) {
+      var dxPlus = x.copy();
+      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));
+    }
+
+    return result;
+  }
+
+  /**
+   * Returns numerical Jacobian with respect to x for f(x, u, ...).
+   *
+   * @param <Rows> Number of rows in the result of f(x, u).
+   * @param <States> Number of rows in x.
+   * @param <Inputs> Number of rows in the second input to f.
+   * @param <Outputs> Num representing the rows in the output of f.
+   * @param rows Number of rows in the result of f(x, u).
+   * @param states Number of rows in x.
+   * @param f Vector-valued function from which to compute Jacobian.
+   * @param x State vector.
+   * @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,
+          Nat<States> states,
+          BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> f,
+          Matrix<States, N1> x,
+          Matrix<Inputs, N1> u) {
+    return numericalJacobian(rows, states, _x -> f.apply(_x, u), x);
+  }
+
+  /**
+   * Returns the numerical Jacobian with respect to u for f(x, u).
+   *
+   * @param <States> The states of the system.
+   * @param <Inputs> The inputs to the system.
+   * @param <Rows> Number of rows in the result of f(x, u).
+   * @param rows Number of rows in the result of f(x, u).
+   * @param inputs Number of rows in u.
+   * @param f Vector-valued function from which to compute the Jacobian.
+   * @param x State vector.
+   * @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,
+          Nat<Inputs> inputs,
+          BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+          Matrix<States, N1> x,
+          Matrix<Inputs, N1> u) {
+    return numericalJacobian(rows, inputs, _u -> f.apply(x, _u), u);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java

@@ -0,0 +1,194 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system.plant;
+
+import edu.wpi.first.math.util.Units;
+
+/** 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;
+
+  /**
+   * Constructs a DC motor.
+   *
+   * @param nominalVoltageVolts Voltage at which the motor constants were measured.
+   * @param stallTorqueNewtonMeters Current draw when stalled.
+   * @param stallCurrentAmps Current draw when stalled.
+   * @param freeCurrentAmps Current draw under no load.
+   * @param freeSpeedRadPerSec Angular velocity under no load.
+   * @param numMotors Number of motors in a gearbox.
+   */
+  public DCMotor(
+      double nominalVoltageVolts,
+      double stallTorqueNewtonMeters,
+      double stallCurrentAmps,
+      double freeCurrentAmps,
+      double freeSpeedRadPerSec,
+      int numMotors) {
+    this.nominalVoltageVolts = nominalVoltageVolts;
+    this.stallTorqueNewtonMeters = stallTorqueNewtonMeters * numMotors;
+    this.stallCurrentAmps = stallCurrentAmps * numMotors;
+    this.freeCurrentAmps = freeCurrentAmps * numMotors;
+    this.freeSpeedRadPerSec = freeSpeedRadPerSec;
+
+    this.rOhms = nominalVoltageVolts / this.stallCurrentAmps;
+    this.KvRadPerSecPerVolt =
+        freeSpeedRadPerSec / (nominalVoltageVolts - rOhms * this.freeCurrentAmps);
+    this.KtNMPerAmp = this.stallTorqueNewtonMeters / this.stallCurrentAmps;
+  }
+
+  /**
+   * Estimate the current being drawn by this motor.
+   *
+   * @param speedRadiansPerSec The speed of the rotor.
+   * @param voltageInputVolts The input voltage.
+   */
+  public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) {
+    return -1.0 / KvRadPerSecPerVolt / rOhms * speedRadiansPerSec + 1.0 / rOhms * voltageInputVolts;
+  }
+
+  /**
+   * Return a gearbox of CIM motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getCIM(int numMotors) {
+    return new DCMotor(
+        12, 2.42, 133, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310), numMotors);
+  }
+
+  /**
+   * Return a gearbox of 775Pro motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getVex775Pro(int numMotors) {
+    return new DCMotor(
+        12, 0.71, 134, 0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730), numMotors);
+  }
+
+  /**
+   * Return a gearbox of NEO motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getNEO(int numMotors) {
+    return new DCMotor(
+        12, 2.6, 105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676), numMotors);
+  }
+
+  /**
+   * Return a gearbox of MiniCIM motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getMiniCIM(int numMotors) {
+    return new DCMotor(
+        12, 1.41, 89, 3, Units.rotationsPerMinuteToRadiansPerSecond(5840), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Bag motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getBag(int numMotors) {
+    return new DCMotor(
+        12, 0.43, 53, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(13180), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Andymark RS775-125 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getAndymarkRs775_125(int numMotors) {
+    return new DCMotor(
+        12, 0.28, 18, 1.6, Units.rotationsPerMinuteToRadiansPerSecond(5800.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Banebots RS775 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getBanebotsRs775(int numMotors) {
+    return new DCMotor(
+        12, 0.72, 97, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(13050.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Andymark 9015 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getAndymark9015(int numMotors) {
+    return new DCMotor(
+        12, 0.36, 71, 3.7, Units.rotationsPerMinuteToRadiansPerSecond(14270.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Banebots RS 550 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getBanebotsRs550(int numMotors) {
+    return new DCMotor(
+        12, 0.38, 84, 0.4, Units.rotationsPerMinuteToRadiansPerSecond(19000.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Neo 550 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getNeo550(int numMotors) {
+    return new DCMotor(
+        12, 0.97, 100, 1.4, Units.rotationsPerMinuteToRadiansPerSecond(11000.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Falcon 500 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getFalcon500(int numMotors) {
+    return new DCMotor(
+        12, 4.69, 257, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6380.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Romi/TI_RSLK MAX motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getRomiBuiltIn(int numMotors) {
+    // From https://www.pololu.com/product/1520/specs
+    return new DCMotor(
+        4.5, 0.1765, 1.25, 0.13, Units.rotationsPerMinuteToRadiansPerSecond(150.0), numMotors);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java

@@ -0,0 +1,211 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system.plant;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+
+public final class LinearSystemId {
+  private LinearSystemId() {
+    // Utility class
+  }
+
+  /**
+   * Create a state-space model of an elevator system. The states of the system are [position,
+   * velocity]^T, inputs are [voltage], and outputs are [position].
+   *
+   * @param motor The motor (or gearbox) attached to the arm.
+   * @param massKg The mass of the elevator carriage, in kilograms.
+   * @param radiusMeters The radius of thd driving drum of the elevator, 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.
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N2, N1, N1> createElevatorSystem(
+      DCMotor motor, double massKg, double radiusMeters, double G) {
+    return new LinearSystem<>(
+        Matrix.mat(Nat.N2(), Nat.N2())
+            .fill(
+                0,
+                1,
+                0,
+                -Math.pow(G, 2)
+                    * motor.KtNMPerAmp
+                    / (motor.rOhms
+                        * radiusMeters
+                        * radiusMeters
+                        * massKg
+                        * motor.KvRadPerSecPerVolt)),
+        VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)),
+        Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
+        new Matrix<>(Nat.N1(), Nat.N1()));
+  }
+
+  /**
+   * 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].
+   *
+   * @param motor The motor (or gearbox) attached to the arm.
+   * @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.
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N1, N1, N1> createFlywheelSystem(
+      DCMotor motor, double jKgMetersSquared, double G) {
+    return new LinearSystem<>(
+        VecBuilder.fill(
+            -G
+                * G
+                * motor.KtNMPerAmp
+                / (motor.KvRadPerSecPerVolt * motor.rOhms * jKgMetersSquared)),
+        VecBuilder.fill(G * motor.KtNMPerAmp / (motor.rOhms * jKgMetersSquared)),
+        Matrix.eye(Nat.N1()),
+        new Matrix<>(Nat.N1(), Nat.N1()));
+  }
+
+  /**
+   * Create a state-space model of a differential drive drivetrain. In this model, the states are
+   * [v_left, v_right]^T, inputs are [V_left, V_right]^T and outputs are [v_left, v_right]^T.
+   *
+   * @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.
+   * @return A LinearSystem representing a differential drivetrain.
+   */
+  @SuppressWarnings({"LocalVariableName", "ParameterName"})
+  public static LinearSystem<N2, N2, N2> createDrivetrainVelocitySystem(
+      DCMotor motor,
+      double massKg,
+      double rMeters,
+      double rbMeters,
+      double JKgMetersSquared,
+      double G) {
+    var C1 =
+        -(G * G) * motor.KtNMPerAmp / (motor.KvRadPerSecPerVolt * motor.rOhms * rMeters * rMeters);
+    var C2 = G * motor.KtNMPerAmp / (motor.rOhms * rMeters);
+
+    final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared;
+    final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared;
+    var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(C3 * C1, C4 * C1, C4 * C1, C3 * C1);
+    var B = Matrix.mat(Nat.N2(), Nat.N2()).fill(C3 * C2, C4 * C2, C4 * C2, C3 * C2);
+    var C = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0);
+    var D = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 0.0, 0.0, 0.0);
+
+    return new LinearSystem<>(A, B, C, D);
+  }
+
+  /**
+   * 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].
+   *
+   * @param motor The motor (or gearbox) attached to 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) {
+    return new LinearSystem<>(
+        Matrix.mat(Nat.N2(), Nat.N2())
+            .fill(
+                0,
+                1,
+                0,
+                -Math.pow(G, 2)
+                    * motor.KtNMPerAmp
+                    / (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 frc-characterization. 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)
+   * @return A LinearSystem representing the given characterized constants.
+   * @see <a href="https://github.com/wpilibsuite/frc-characterization">
+   *     https://github.com/wpilibsuite/frc-characterization</a>
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N1, N1, N1> identifyVelocitySystem(double kV, double kA) {
+    return new LinearSystem<>(
+        VecBuilder.fill(-kV / kA),
+        VecBuilder.fill(1.0 / kA),
+        VecBuilder.fill(1.0),
+        VecBuilder.fill(0.0));
+  }
+
+  /**
+   * Identify a position system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). These
+   * constants cam be found using frc-characterization. The states of the system are [position,
+   * velocity]^T, 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)
+   * @return A LinearSystem representing the given characterized constants.
+   * @see <a href="https://github.com/wpilibsuite/frc-characterization">
+   *     https://github.com/wpilibsuite/frc-characterization</a>
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N2, N1, N1> identifyPositionSystem(double kV, double kA) {
+    return new LinearSystem<>(
+        Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -kV / kA),
+        VecBuilder.fill(0.0, 1.0 / kA),
+        Matrix.mat(Nat.N1(), Nat.N2()).fill(1.0, 0.0),
+        VecBuilder.fill(0.0));
+  }
+
+  /**
+   * 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 frc-characterization. The states of the
+   * system are [left velocity, right velocity]^T, inputs are [left voltage, right voltage]^T, and
+   * outputs are [left velocity, right velocity]^T.
+   *
+   * @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).
+   * @return A LinearSystem representing the given characterized constants.
+   * @see <a href="https://github.com/wpilibsuite/frc-characterization">
+   *     https://github.com/wpilibsuite/frc-characterization</a>
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
+      double kVLinear, double kALinear, double kVAngular, double kAAngular) {
+
+    final double c = 0.5 / (kALinear * kAAngular);
+    final double A1 = c * (-kALinear * kVAngular - kVLinear * kAAngular);
+    final double A2 = c * (kALinear * kVAngular - kVLinear * kAAngular);
+    final double B1 = c * (kALinear + kAAngular);
+    final double B2 = c * (kAAngular - kALinear);
+
+    return new LinearSystem<>(
+        Matrix.mat(Nat.N2(), Nat.N2()).fill(A1, A2, A2, A1),
+        Matrix.mat(Nat.N2(), Nat.N2()).fill(B1, B2, B2, B1),
+        Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1),
+        Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0));
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java

@@ -0,0 +1,411 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Objects;
+import java.util.stream.Collectors;
+
+/**
+ * Represents a time-parameterized trajectory. The trajectory contains of various States that
+ * represent the pose, curvature, time elapsed, velocity, and acceleration at that point.
+ */
+public class Trajectory {
+  private final double m_totalTimeSeconds;
+  private final List<State> m_states;
+
+  /** Constructs an empty trajectory. */
+  public Trajectory() {
+    m_states = new ArrayList<>();
+    m_totalTimeSeconds = 0.0;
+  }
+
+  /**
+   * Constructs a trajectory from a vector of states.
+   *
+   * @param states A vector of states.
+   */
+  public Trajectory(final List<State> states) {
+    m_states = states;
+    m_totalTimeSeconds = m_states.get(m_states.size() - 1).timeSeconds;
+  }
+
+  /**
+   * Linearly interpolates between two values.
+   *
+   * @param startValue The start value.
+   * @param endValue The end value.
+   * @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;
+  }
+
+  /**
+   * Linearly interpolates between two poses.
+   *
+   * @param startValue The start pose.
+   * @param endValue The end pose.
+   * @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));
+  }
+
+  /**
+   * Returns the initial pose of the trajectory.
+   *
+   * @return The initial pose of the trajectory.
+   */
+  public Pose2d getInitialPose() {
+    return sample(0).poseMeters;
+  }
+
+  /**
+   * Returns the overall duration of the trajectory.
+   *
+   * @return The duration of the trajectory.
+   */
+  public double getTotalTimeSeconds() {
+    return m_totalTimeSeconds;
+  }
+
+  /**
+   * Return the states of the trajectory.
+   *
+   * @return The states of the trajectory.
+   */
+  public List<State> getStates() {
+    return m_states;
+  }
+
+  /**
+   * Sample the trajectory at a point in time.
+   *
+   * @param timeSeconds The point in time since the beginning of the trajectory to sample.
+   * @return The state at that point in time.
+   */
+  public State sample(double timeSeconds) {
+    if (timeSeconds <= m_states.get(0).timeSeconds) {
+      return m_states.get(0);
+    }
+    if (timeSeconds >= m_totalTimeSeconds) {
+      return m_states.get(m_states.size() - 1);
+    }
+
+    // To get the element that we want, we will use a binary search algorithm
+    // instead of iterating over a for-loop. A binary search is O(std::log(n))
+    // whereas searching using a loop is O(n).
+
+    // This starts at 1 because we use the previous state later on for
+    // interpolation.
+    int low = 1;
+    int high = m_states.size() - 1;
+
+    while (low != high) {
+      int mid = (low + high) / 2;
+      if (m_states.get(mid).timeSeconds < timeSeconds) {
+        // This index and everything under it are less than the requested
+        // timestamp. Therefore, we can discard them.
+        low = mid + 1;
+      } else {
+        // t is at least as large as the element at this index. This means that
+        // anything after it cannot be what we are looking for.
+        high = mid;
+      }
+    }
+
+    // High and Low should be the same.
+
+    // The sample's timestamp is now greater than or equal to the requested
+    // timestamp. If it is greater, we need to interpolate between the
+    // previous state and the current state to get the exact state that we
+    // want.
+    final State sample = m_states.get(low);
+    final State prevSample = m_states.get(low - 1);
+
+    // If the difference in states is negligible, then we are spot on!
+    if (Math.abs(sample.timeSeconds - prevSample.timeSeconds) < 1E-9) {
+      return sample;
+    }
+    // Interpolate between the two states for the state that we want.
+    return prevSample.interpolate(
+        sample,
+        (timeSeconds - prevSample.timeSeconds) / (sample.timeSeconds - prevSample.timeSeconds));
+  }
+
+  /**
+   * Transforms all poses in the trajectory by the given transform. This is useful for converting a
+   * robot-relative trajectory into a field-relative trajectory. This works with respect to the
+   * first pose in the trajectory.
+   *
+   * @param transform The transform to transform the trajectory by.
+   * @return The transformed trajectory.
+   */
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  public Trajectory transformBy(Transform2d transform) {
+    var firstState = m_states.get(0);
+    var firstPose = firstState.poseMeters;
+
+    // Calculate the transformed first pose.
+    var newFirstPose = firstPose.plus(transform);
+    List<State> newStates = new ArrayList<>();
+
+    newStates.add(
+        new State(
+            firstState.timeSeconds,
+            firstState.velocityMetersPerSecond,
+            firstState.accelerationMetersPerSecondSq,
+            newFirstPose,
+            firstState.curvatureRadPerMeter));
+
+    for (int i = 1; i < m_states.size(); i++) {
+      var state = m_states.get(i);
+      // We are transforming relative to the coordinate frame of the new initial pose.
+      newStates.add(
+          new State(
+              state.timeSeconds,
+              state.velocityMetersPerSecond,
+              state.accelerationMetersPerSecondSq,
+              newFirstPose.plus(state.poseMeters.minus(firstPose)),
+              state.curvatureRadPerMeter));
+    }
+
+    return new Trajectory(newStates);
+  }
+
+  /**
+   * Transforms all poses in the trajectory so that they are relative to the given pose. This is
+   * useful for converting a field-relative trajectory into a robot-relative trajectory.
+   *
+   * @param pose The pose that is the origin of the coordinate frame that the current trajectory
+   *     will be transformed into.
+   * @return The transformed trajectory.
+   */
+  public Trajectory relativeTo(Pose2d pose) {
+    return new Trajectory(
+        m_states.stream()
+            .map(
+                state ->
+                    new State(
+                        state.timeSeconds,
+                        state.velocityMetersPerSecond,
+                        state.accelerationMetersPerSecondSq,
+                        state.poseMeters.relativeTo(pose),
+                        state.curvatureRadPerMeter))
+            .collect(Collectors.toList()));
+  }
+
+  /**
+   * Concatenates another trajectory to the current trajectory. The user is responsible for making
+   * sure that the end pose of this trajectory and the start pose of the other trajectory match (if
+   * that is the desired behavior).
+   *
+   * @param other The trajectory to concatenate.
+   * @return The concatenated trajectory.
+   */
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  public Trajectory concatenate(Trajectory other) {
+    // If this is a default constructed trajectory with no states, then we can
+    // simply return the rhs trajectory.
+    if (m_states.isEmpty()) {
+      return other;
+    }
+
+    // Deep copy the current states.
+    List<State> states =
+        m_states.stream()
+            .map(
+                state ->
+                    new State(
+                        state.timeSeconds,
+                        state.velocityMetersPerSecond,
+                        state.accelerationMetersPerSecondSq,
+                        state.poseMeters,
+                        state.curvatureRadPerMeter))
+            .collect(Collectors.toList());
+
+    // Here we omit the first state of the other trajectory because we don't want
+    // two time points with different states. Sample() will automatically
+    // interpolate between the end of this trajectory and the second state of the
+    // other trajectory.
+    for (int i = 1; i < other.getStates().size(); ++i) {
+      var s = other.getStates().get(i);
+      states.add(
+          new State(
+              s.timeSeconds + m_totalTimeSeconds,
+              s.velocityMetersPerSecond,
+              s.accelerationMetersPerSecondSq,
+              s.poseMeters,
+              s.curvatureRadPerMeter));
+    }
+    return new Trajectory(states);
+  }
+
+  /**
+   * 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")
+    public double timeSeconds;
+
+    // The speed at that point of the trajectory.
+    @JsonProperty("velocity")
+    public double velocityMetersPerSecond;
+
+    // The acceleration at that point of the trajectory.
+    @JsonProperty("acceleration")
+    public double accelerationMetersPerSecondSq;
+
+    // The pose at that point of the trajectory.
+    @JsonProperty("pose")
+    public Pose2d poseMeters;
+
+    // The curvature at that point of the trajectory.
+    @JsonProperty("curvature")
+    public double curvatureRadPerMeter;
+
+    public State() {
+      poseMeters = new Pose2d();
+    }
+
+    /**
+     * Constructs a State with the specified parameters.
+     *
+     * @param timeSeconds The time elapsed since the beginning of the trajectory.
+     * @param velocityMetersPerSecond The speed at that point of the trajectory.
+     * @param accelerationMetersPerSecondSq The acceleration at that point of the trajectory.
+     * @param poseMeters The pose at that point of the trajectory.
+     * @param curvatureRadPerMeter The curvature at that point of the trajectory.
+     */
+    public State(
+        double timeSeconds,
+        double velocityMetersPerSecond,
+        double accelerationMetersPerSecondSq,
+        Pose2d poseMeters,
+        double curvatureRadPerMeter) {
+      this.timeSeconds = timeSeconds;
+      this.velocityMetersPerSecond = velocityMetersPerSecond;
+      this.accelerationMetersPerSecondSq = accelerationMetersPerSecondSq;
+      this.poseMeters = poseMeters;
+      this.curvatureRadPerMeter = curvatureRadPerMeter;
+    }
+
+    /**
+     * Interpolates between two States.
+     *
+     * @param endValue The end value for the interpolation.
+     * @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);
+
+      // Find the delta time between the current state and the interpolated state.
+      final double deltaT = newT - timeSeconds;
+
+      // If delta time is negative, flip the order of interpolation.
+      if (deltaT < 0) {
+        return endValue.interpolate(this, 1 - i);
+      }
+
+      // Check whether the robot is reversing at this stage.
+      final boolean reversing =
+          velocityMetersPerSecond < 0
+              || Math.abs(velocityMetersPerSecond) < 1E-9 && accelerationMetersPerSecondSq < 0;
+
+      // Calculate the new velocity
+      // v_f = v_0 + at
+      final double newV = velocityMetersPerSecond + (accelerationMetersPerSecondSq * deltaT);
+
+      // Calculate the change in position.
+      // delta_s = v_0 t + 0.5 at^2
+      final double newS =
+          (velocityMetersPerSecond * deltaT
+                  + 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2))
+              * (reversing ? -1.0 : 1.0);
+
+      // Return the new state. To find the new position for the new state, we need
+      // to interpolate between the two endpoint poses. The fraction for
+      // interpolation is the change in position (delta s) divided by the total
+      // distance between the two endpoints.
+      final double interpolationFrac =
+          newS / endValue.poseMeters.getTranslation().getDistance(poseMeters.getTranslation());
+
+      return new State(
+          newT,
+          newV,
+          accelerationMetersPerSecondSq,
+          lerp(poseMeters, endValue.poseMeters, interpolationFrac),
+          lerp(curvatureRadPerMeter, endValue.curvatureRadPerMeter, interpolationFrac));
+    }
+
+    @Override
+    public String toString() {
+      return String.format(
+          "State(Sec: %.2f, Vel m/s: %.2f, Accel m/s/s: %.2f, Pose: %s, Curvature: %.2f)",
+          timeSeconds,
+          velocityMetersPerSecond,
+          accelerationMetersPerSecondSq,
+          poseMeters,
+          curvatureRadPerMeter);
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+      if (this == obj) {
+        return true;
+      }
+      if (!(obj instanceof State)) {
+        return false;
+      }
+      State state = (State) obj;
+      return Double.compare(state.timeSeconds, timeSeconds) == 0
+          && Double.compare(state.velocityMetersPerSecond, velocityMetersPerSecond) == 0
+          && Double.compare(state.accelerationMetersPerSecondSq, accelerationMetersPerSecondSq) == 0
+          && Double.compare(state.curvatureRadPerMeter, curvatureRadPerMeter) == 0
+          && Objects.equals(poseMeters, state.poseMeters);
+    }
+
+    @Override
+    public int hashCode() {
+      return Objects.hash(
+          timeSeconds,
+          velocityMetersPerSecond,
+          accelerationMetersPerSecondSq,
+          poseMeters,
+          curvatureRadPerMeter);
+    }
+  }
+
+  @Override
+  public String toString() {
+    String stateList = m_states.stream().map(State::toString).collect(Collectors.joining(", \n"));
+    return String.format("Trajectory - Seconds: %.2f, States:\n%s", m_totalTimeSeconds, stateList);
+  }
+
+  @Override
+  public int hashCode() {
+    return m_states.hashCode();
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    return obj instanceof Trajectory && m_states.equals(((Trajectory) obj).getStates());
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java

@@ -0,0 +1,190 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+import edu.wpi.first.math.trajectory.constraint.MecanumDriveKinematicsConstraint;
+import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint;
+import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Represents the configuration for generating a trajectory. This class stores the start velocity,
+ * end velocity, max velocity, max acceleration, custom constraints, and the reversed flag.
+ *
+ * <p>The class must be constructed with a max velocity and max acceleration. The other parameters
+ * (start velocity, end velocity, constraints, reversed) have been defaulted to reasonable values
+ * (0, 0, {}, false). These values can be changed via the setXXX methods.
+ */
+public class TrajectoryConfig {
+  private final double m_maxVelocity;
+  private final double m_maxAcceleration;
+  private final List<TrajectoryConstraint> m_constraints;
+  private double m_startVelocity;
+  private double m_endVelocity;
+  private boolean m_reversed;
+
+  /**
+   * Constructs the trajectory configuration class.
+   *
+   * @param maxVelocityMetersPerSecond The max velocity for the trajectory.
+   * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
+   */
+  public TrajectoryConfig(
+      double maxVelocityMetersPerSecond, double maxAccelerationMetersPerSecondSq) {
+    m_maxVelocity = maxVelocityMetersPerSecond;
+    m_maxAcceleration = maxAccelerationMetersPerSecondSq;
+    m_constraints = new ArrayList<>();
+  }
+
+  /**
+   * Adds a user-defined constraint to the trajectory.
+   *
+   * @param constraint The user-defined constraint.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig addConstraint(TrajectoryConstraint constraint) {
+    m_constraints.add(constraint);
+    return this;
+  }
+
+  /**
+   * Adds all user-defined constraints from a list to the trajectory.
+   *
+   * @param constraints List of user-defined constraints.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig addConstraints(List<? extends TrajectoryConstraint> constraints) {
+    m_constraints.addAll(constraints);
+    return this;
+  }
+
+  /**
+   * Adds a differential drive kinematics constraint to ensure that no wheel velocity of a
+   * differential drive goes above the max velocity.
+   *
+   * @param kinematics The differential drive kinematics.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setKinematics(DifferentialDriveKinematics kinematics) {
+    addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
+    return this;
+  }
+
+  /**
+   * Adds a mecanum drive kinematics constraint to ensure that no wheel velocity of a mecanum drive
+   * goes above the max velocity.
+   *
+   * @param kinematics The mecanum drive kinematics.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setKinematics(MecanumDriveKinematics kinematics) {
+    addConstraint(new MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
+    return this;
+  }
+
+  /**
+   * Adds a swerve drive kinematics constraint to ensure that no wheel velocity of a swerve drive
+   * goes above the max velocity.
+   *
+   * @param kinematics The swerve drive kinematics.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setKinematics(SwerveDriveKinematics kinematics) {
+    addConstraint(new SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
+    return this;
+  }
+
+  /**
+   * Returns the starting velocity of the trajectory.
+   *
+   * @return The starting velocity of the trajectory.
+   */
+  public double getStartVelocity() {
+    return m_startVelocity;
+  }
+
+  /**
+   * Sets the start velocity of the trajectory.
+   *
+   * @param startVelocityMetersPerSecond The start velocity of the trajectory.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setStartVelocity(double startVelocityMetersPerSecond) {
+    m_startVelocity = startVelocityMetersPerSecond;
+    return this;
+  }
+
+  /**
+   * Returns the starting velocity of the trajectory.
+   *
+   * @return The starting velocity of the trajectory.
+   */
+  public double getEndVelocity() {
+    return m_endVelocity;
+  }
+
+  /**
+   * Sets the end velocity of the trajectory.
+   *
+   * @param endVelocityMetersPerSecond The end velocity of the trajectory.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setEndVelocity(double endVelocityMetersPerSecond) {
+    m_endVelocity = endVelocityMetersPerSecond;
+    return this;
+  }
+
+  /**
+   * Returns the maximum velocity of the trajectory.
+   *
+   * @return The maximum velocity of the trajectory.
+   */
+  public double getMaxVelocity() {
+    return m_maxVelocity;
+  }
+
+  /**
+   * Returns the maximum acceleration of the trajectory.
+   *
+   * @return The maximum acceleration of the trajectory.
+   */
+  public double getMaxAcceleration() {
+    return m_maxAcceleration;
+  }
+
+  /**
+   * Returns the user-defined constraints of the trajectory.
+   *
+   * @return The user-defined constraints of the trajectory.
+   */
+  public List<TrajectoryConstraint> getConstraints() {
+    return m_constraints;
+  }
+
+  /**
+   * Returns whether the trajectory is reversed or not.
+   *
+   * @return whether the trajectory is reversed or not.
+   */
+  public boolean isReversed() {
+    return m_reversed;
+  }
+
+  /**
+   * Sets the reversed flag of the trajectory.
+   *
+   * @param reversed Whether the trajectory should be reversed or not.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setReversed(boolean reversed) {
+    m_reversed = reversed;
+    return this;
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java

@@ -0,0 +1,281 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import edu.wpi.first.math.MathSharedStore;
+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.spline.PoseWithCurvature;
+import edu.wpi.first.math.spline.Spline;
+import edu.wpi.first.math.spline.SplineHelper;
+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()));
+  private static BiConsumer<String, StackTraceElement[]> errorFunc;
+
+  /** Private constructor because this is a utility class. */
+  private TrajectoryGenerator() {}
+
+  private static void reportError(String error, StackTraceElement[] stackTrace) {
+    if (errorFunc != null) {
+      errorFunc.accept(error, stackTrace);
+    } else {
+      MathSharedStore.reportError(error, stackTrace);
+    }
+  }
+
+  /**
+   * Set error reporting function. By default, DriverStation.reportError() is used.
+   *
+   * @param func Error reporting function, arguments are error and stackTrace.
+   */
+  public static void setErrorHandler(BiConsumer<String, StackTraceElement[]> func) {
+    errorFunc = func;
+  }
+
+  /**
+   * Generates a trajectory from the given control vectors and config. This method uses clamped
+   * cubic splines -- a method in which the exterior control vectors and interior waypoints are
+   * provided. The headings are automatically determined at the interior points to ensure continuous
+   * curvature.
+   *
+   * @param initial The initial control vector.
+   * @param interiorWaypoints The interior waypoints.
+   * @param end The ending control vector.
+   * @param config The configuration for the trajectory.
+   * @return The generated trajectory.
+   */
+  public static Trajectory generateTrajectory(
+      Spline.ControlVector initial,
+      List<Translation2d> interiorWaypoints,
+      Spline.ControlVector end,
+      TrajectoryConfig config) {
+    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
+
+    // Clone the control vectors.
+    var newInitial = new Spline.ControlVector(initial.x, initial.y);
+    var newEnd = new Spline.ControlVector(end.x, end.y);
+
+    // Change the orientation if reversed.
+    if (config.isReversed()) {
+      newInitial.x[1] *= -1;
+      newInitial.y[1] *= -1;
+      newEnd.x[1] *= -1;
+      newEnd.y[1] *= -1;
+    }
+
+    // Get the spline points
+    List<PoseWithCurvature> points;
+    try {
+      points =
+          splinePointsFromSplines(
+              SplineHelper.getCubicSplinesFromControlVectors(
+                  newInitial, interiorWaypoints.toArray(new Translation2d[0]), newEnd));
+    } catch (MalformedSplineException ex) {
+      reportError(ex.getMessage(), ex.getStackTrace());
+      return kDoNothingTrajectory;
+    }
+
+    // Change the points back to their original orientation.
+    if (config.isReversed()) {
+      for (var point : points) {
+        point.poseMeters = point.poseMeters.plus(flip);
+        point.curvatureRadPerMeter *= -1;
+      }
+    }
+
+    // Generate and return trajectory.
+    return TrajectoryParameterizer.timeParameterizeTrajectory(
+        points,
+        config.getConstraints(),
+        config.getStartVelocity(),
+        config.getEndVelocity(),
+        config.getMaxVelocity(),
+        config.getMaxAcceleration(),
+        config.isReversed());
+  }
+
+  /**
+   * Generates a trajectory from the given waypoints and config. This method uses clamped cubic
+   * splines -- a method in which the initial pose, final pose, and interior waypoints are provided.
+   * The headings are automatically determined at the interior points to ensure continuous
+   * curvature.
+   *
+   * @param start The starting pose.
+   * @param interiorWaypoints The interior waypoints.
+   * @param end The ending pose.
+   * @param config The configuration for the trajectory.
+   * @return The generated trajectory.
+   */
+  public static Trajectory generateTrajectory(
+      Pose2d start, List<Translation2d> interiorWaypoints, Pose2d end, TrajectoryConfig config) {
+    var controlVectors =
+        SplineHelper.getCubicControlVectorsFromWaypoints(
+            start, interiorWaypoints.toArray(new Translation2d[0]), end);
+
+    // Return the generated trajectory.
+    return generateTrajectory(controlVectors[0], interiorWaypoints, controlVectors[1], config);
+  }
+
+  /**
+   * Generates a trajectory from the given quintic control vectors and config. This method uses
+   * quintic hermite splines -- therefore, all points must be represented by control vectors.
+   * Continuous curvature is guaranteed in this method.
+   *
+   * @param controlVectors List of quintic control vectors.
+   * @param config The configuration for the trajectory.
+   * @return The generated trajectory.
+   */
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  public static Trajectory generateTrajectory(
+      ControlVectorList controlVectors, TrajectoryConfig config) {
+    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
+    final var newControlVectors = new ArrayList<Spline.ControlVector>(controlVectors.size());
+
+    // Create a new control vector list, flipping the orientation if reversed.
+    for (final var vector : controlVectors) {
+      var newVector = new Spline.ControlVector(vector.x, vector.y);
+      if (config.isReversed()) {
+        newVector.x[1] *= -1;
+        newVector.y[1] *= -1;
+      }
+      newControlVectors.add(newVector);
+    }
+
+    // Get the spline points
+    List<PoseWithCurvature> points;
+    try {
+      points =
+          splinePointsFromSplines(
+              SplineHelper.getQuinticSplinesFromControlVectors(
+                  newControlVectors.toArray(new Spline.ControlVector[] {})));
+    } catch (MalformedSplineException ex) {
+      reportError(ex.getMessage(), ex.getStackTrace());
+      return kDoNothingTrajectory;
+    }
+
+    // Change the points back to their original orientation.
+    if (config.isReversed()) {
+      for (var point : points) {
+        point.poseMeters = point.poseMeters.plus(flip);
+        point.curvatureRadPerMeter *= -1;
+      }
+    }
+
+    // Generate and return trajectory.
+    return TrajectoryParameterizer.timeParameterizeTrajectory(
+        points,
+        config.getConstraints(),
+        config.getStartVelocity(),
+        config.getEndVelocity(),
+        config.getMaxVelocity(),
+        config.getMaxAcceleration(),
+        config.isReversed());
+  }
+
+  /**
+   * Generates a trajectory from the given waypoints and config. This method uses quintic hermite
+   * splines -- therefore, all points must be represented by Pose2d objects. Continuous curvature is
+   * guaranteed in this method.
+   *
+   * @param waypoints List of waypoints..
+   * @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));
+
+    List<Pose2d> newWaypoints = new ArrayList<>();
+    if (config.isReversed()) {
+      for (Pose2d originalWaypoint : waypoints) {
+        newWaypoints.add(originalWaypoint.plus(flip));
+      }
+    } else {
+      newWaypoints.addAll(waypoints);
+    }
+
+    // Get the spline points
+    List<PoseWithCurvature> points;
+    try {
+      points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromWaypoints(newWaypoints));
+    } catch (MalformedSplineException ex) {
+      reportError(ex.getMessage(), ex.getStackTrace());
+      return kDoNothingTrajectory;
+    }
+
+    // Change the points back to their original orientation.
+    if (config.isReversed()) {
+      for (var point : points) {
+        point.poseMeters = point.poseMeters.plus(flip);
+        point.curvatureRadPerMeter *= -1;
+      }
+    }
+
+    // Generate and return trajectory.
+    return TrajectoryParameterizer.timeParameterizeTrajectory(
+        points,
+        config.getConstraints(),
+        config.getStartVelocity(),
+        config.getEndVelocity(),
+        config.getMaxVelocity(),
+        config.getMaxAcceleration(),
+        config.isReversed());
+  }
+
+  /**
+   * Generate spline points from a vector of splines by parameterizing the splines.
+   *
+   * @param splines The splines to parameterize.
+   * @return The spline points for use in time parameterization of a trajectory.
+   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+   *     with approximately opposing headings)
+   */
+  public static List<PoseWithCurvature> splinePointsFromSplines(Spline[] splines) {
+    // Create the vector of spline points.
+    var splinePoints = new ArrayList<PoseWithCurvature>();
+
+    // Add the first point to the vector.
+    splinePoints.add(splines[0].getPoint(0.0));
+
+    // Iterate through the vector and parameterize each spline, adding the
+    // parameterized points to the final vector.
+    for (final var spline : splines) {
+      var points = SplineParameterizer.parameterize(spline);
+
+      // Append the array of poses to the vector. We are removing the first
+      // point because it's a duplicate of the last point from the previous
+      // spline.
+      splinePoints.addAll(points.subList(1, points.size()));
+    }
+    return splinePoints;
+  }
+
+  // Work around type erasure signatures
+  @SuppressWarnings("serial")
+  public static class ControlVectorList extends ArrayList<Spline.ControlVector> {
+    public ControlVectorList(int initialCapacity) {
+      super(initialCapacity);
+    }
+
+    public ControlVectorList() {
+      super();
+    }
+
+    public ControlVectorList(Collection<? extends Spline.ControlVector> collection) {
+      super(collection);
+    }
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java

@@ -0,0 +1,336 @@
+// 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.
+
+/*
+ * MIT License
+ *
+ * Copyright (c) 2018 Team 254
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package edu.wpi.first.math.trajectory;
+
+import edu.wpi.first.math.spline.PoseWithCurvature;
+import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint;
+import java.util.ArrayList;
+import java.util.List;
+
+/** Class used to parameterize a trajectory by time. */
+public final class TrajectoryParameterizer {
+  /** Private constructor because this is a utility class. */
+  private TrajectoryParameterizer() {}
+
+  /**
+   * Parameterize the trajectory by time. This is where the velocity profile is generated.
+   *
+   * <p>The derivation of the algorithm used can be found <a
+   * href="http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf">here</a>.
+   *
+   * @param points Reference to the spline points.
+   * @param constraints A vector of various velocity and acceleration. constraints.
+   * @param startVelocityMetersPerSecond The start velocity for the trajectory.
+   * @param endVelocityMetersPerSecond The end velocity for the trajectory.
+   * @param maxVelocityMetersPerSecond The max velocity for the trajectory.
+   * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
+   * @param reversed Whether the robot should move backwards. Note that the robot will still move
+   *     from a -&gt; b -&gt; ... -&gt; z as defined in the waypoints.
+   * @return The trajectory.
+   */
+  @SuppressWarnings({
+    "PMD.ExcessiveMethodLength",
+    "PMD.CyclomaticComplexity",
+    "PMD.NPathComplexity",
+    "PMD.AvoidInstantiatingObjectsInLoops"
+  })
+  public static Trajectory timeParameterizeTrajectory(
+      List<PoseWithCurvature> points,
+      List<TrajectoryConstraint> constraints,
+      double startVelocityMetersPerSecond,
+      double endVelocityMetersPerSecond,
+      double maxVelocityMetersPerSecond,
+      double maxAccelerationMetersPerSecondSq,
+      boolean reversed) {
+    var constrainedStates = new ArrayList<ConstrainedState>(points.size());
+    var predecessor =
+        new ConstrainedState(
+            points.get(0),
+            0,
+            startVelocityMetersPerSecond,
+            -maxAccelerationMetersPerSecondSq,
+            maxAccelerationMetersPerSecondSq);
+
+    // Forward pass
+    for (int i = 0; i < points.size(); i++) {
+      constrainedStates.add(new ConstrainedState());
+      var constrainedState = constrainedStates.get(i);
+      constrainedState.pose = points.get(i);
+
+      // Begin constraining based on predecessor.
+      double ds =
+          constrainedState
+              .pose
+              .poseMeters
+              .getTranslation()
+              .getDistance(predecessor.pose.poseMeters.getTranslation());
+      constrainedState.distanceMeters = predecessor.distanceMeters + ds;
+
+      // We may need to iterate to find the maximum end velocity and common
+      // 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).
+        constrainedState.maxVelocityMetersPerSecond =
+            Math.min(
+                maxVelocityMetersPerSecond,
+                Math.sqrt(
+                    predecessor.maxVelocityMetersPerSecond * predecessor.maxVelocityMetersPerSecond
+                        + predecessor.maxAccelerationMetersPerSecondSq * ds * 2.0));
+
+        constrainedState.minAccelerationMetersPerSecondSq = -maxAccelerationMetersPerSecondSq;
+        constrainedState.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
+
+        // At this point, the constrained state is fully constructed apart from
+        // all the custom-defined user constraints.
+        for (final var constraint : constraints) {
+          constrainedState.maxVelocityMetersPerSecond =
+              Math.min(
+                  constrainedState.maxVelocityMetersPerSecond,
+                  constraint.getMaxVelocityMetersPerSecond(
+                      constrainedState.pose.poseMeters,
+                      constrainedState.pose.curvatureRadPerMeter,
+                      constrainedState.maxVelocityMetersPerSecond));
+        }
+
+        // Now enforce all acceleration limits.
+        enforceAccelerationLimits(reversed, constraints, constrainedState);
+
+        if (ds < 1E-6) {
+          break;
+        }
+
+        // If the actual acceleration for this state is higher than the max
+        // acceleration that we applied, then we need to reduce the max
+        // acceleration of the predecessor and try again.
+        double actualAcceleration =
+            (constrainedState.maxVelocityMetersPerSecond
+                        * constrainedState.maxVelocityMetersPerSecond
+                    - predecessor.maxVelocityMetersPerSecond
+                        * predecessor.maxVelocityMetersPerSecond)
+                / (ds * 2.0);
+
+        // If we violate the max acceleration constraint, let's modify the
+        // predecessor.
+        if (constrainedState.maxAccelerationMetersPerSecondSq < actualAcceleration - 1E-6) {
+          predecessor.maxAccelerationMetersPerSecondSq =
+              constrainedState.maxAccelerationMetersPerSecondSq;
+        } else {
+          // Constrain the predecessor's max acceleration to the current
+          // acceleration.
+          if (actualAcceleration > predecessor.minAccelerationMetersPerSecondSq) {
+            predecessor.maxAccelerationMetersPerSecondSq = actualAcceleration;
+          }
+          // If the actual acceleration is less than the predecessor's min
+          // acceleration, it will be repaired in the backward pass.
+          break;
+        }
+      }
+      predecessor = constrainedState;
+    }
+
+    var successor =
+        new ConstrainedState(
+            points.get(points.size() - 1),
+            constrainedStates.get(constrainedStates.size() - 1).distanceMeters,
+            endVelocityMetersPerSecond,
+            -maxAccelerationMetersPerSecondSq,
+            maxAccelerationMetersPerSecondSq);
+
+    // Backward pass
+    for (int i = points.size() - 1; i >= 0; i--) {
+      var constrainedState = constrainedStates.get(i);
+      double ds = constrainedState.distanceMeters - successor.distanceMeters; // negative
+
+      while (true) {
+        // Enforce max velocity limit (reverse)
+        // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
+        double newMaxVelocity =
+            Math.sqrt(
+                successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond
+                    + successor.minAccelerationMetersPerSecondSq * ds * 2.0);
+
+        // No more limits to impose! This state can be finalized.
+        if (newMaxVelocity >= constrainedState.maxVelocityMetersPerSecond) {
+          break;
+        }
+
+        constrainedState.maxVelocityMetersPerSecond = newMaxVelocity;
+
+        // Check all acceleration constraints with the new max velocity.
+        enforceAccelerationLimits(reversed, constraints, constrainedState);
+
+        if (ds > -1E-6) {
+          break;
+        }
+
+        // If the actual acceleration for this state is lower than the min
+        // acceleration, then we need to lower the min acceleration of the
+        // successor and try again.
+        double actualAcceleration =
+            (constrainedState.maxVelocityMetersPerSecond
+                        * constrainedState.maxVelocityMetersPerSecond
+                    - successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond)
+                / (ds * 2.0);
+
+        if (constrainedState.minAccelerationMetersPerSecondSq > actualAcceleration + 1E-6) {
+          successor.minAccelerationMetersPerSecondSq =
+              constrainedState.minAccelerationMetersPerSecondSq;
+        } else {
+          successor.minAccelerationMetersPerSecondSq = actualAcceleration;
+          break;
+        }
+      }
+      successor = constrainedState;
+    }
+
+    // Now we can integrate the constrained states forward in time to obtain our
+    // trajectory states.
+    var states = new ArrayList<Trajectory.State>(points.size());
+    double timeSeconds = 0.0;
+    double distanceMeters = 0.0;
+    double velocityMetersPerSecond = 0.0;
+
+    for (int i = 0; i < constrainedStates.size(); i++) {
+      final var state = constrainedStates.get(i);
+
+      // Calculate the change in position between the current state and the previous
+      // state.
+      double ds = state.distanceMeters - distanceMeters;
+
+      // Calculate the acceleration between the current state and the previous
+      // state.
+      double accel =
+          (state.maxVelocityMetersPerSecond * state.maxVelocityMetersPerSecond
+                  - velocityMetersPerSecond * velocityMetersPerSecond)
+              / (ds * 2);
+
+      // Calculate dt
+      double dt = 0.0;
+      if (i > 0) {
+        states.get(i - 1).accelerationMetersPerSecondSq = reversed ? -accel : accel;
+        if (Math.abs(accel) > 1E-6) {
+          // v_f = v_0 + a * t
+          dt = (state.maxVelocityMetersPerSecond - velocityMetersPerSecond) / accel;
+        } else if (Math.abs(velocityMetersPerSecond) > 1E-6) {
+          // delta_x = v * t
+          dt = ds / velocityMetersPerSecond;
+        } else {
+          throw new TrajectoryGenerationException(
+              "Something went wrong at iteration " + i + " of time parameterization.");
+        }
+      }
+
+      velocityMetersPerSecond = state.maxVelocityMetersPerSecond;
+      distanceMeters = state.distanceMeters;
+
+      timeSeconds += dt;
+
+      states.add(
+          new Trajectory.State(
+              timeSeconds,
+              reversed ? -velocityMetersPerSecond : velocityMetersPerSecond,
+              reversed ? -accel : accel,
+              state.pose.poseMeters,
+              state.pose.curvatureRadPerMeter));
+    }
+
+    return new Trajectory(states);
+  }
+
+  private static void enforceAccelerationLimits(
+      boolean reverse, List<TrajectoryConstraint> constraints, ConstrainedState state) {
+
+    for (final var constraint : constraints) {
+      double factor = reverse ? -1.0 : 1.0;
+      final var minMaxAccel =
+          constraint.getMinMaxAccelerationMetersPerSecondSq(
+              state.pose.poseMeters,
+              state.pose.curvatureRadPerMeter,
+              state.maxVelocityMetersPerSecond * factor);
+
+      if (minMaxAccel.minAccelerationMetersPerSecondSq
+          > minMaxAccel.maxAccelerationMetersPerSecondSq) {
+        throw new TrajectoryGenerationException(
+            "The constraint's min acceleration "
+                + "was greater than its max acceleration.\n Offending Constraint: "
+                + constraint.getClass().getName()
+                + "\n If the offending constraint was packaged with WPILib, please file a bug"
+                + " report.");
+      }
+
+      state.minAccelerationMetersPerSecondSq =
+          Math.max(
+              state.minAccelerationMetersPerSecondSq,
+              reverse
+                  ? -minMaxAccel.maxAccelerationMetersPerSecondSq
+                  : minMaxAccel.minAccelerationMetersPerSecondSq);
+
+      state.maxAccelerationMetersPerSecondSq =
+          Math.min(
+              state.maxAccelerationMetersPerSecondSq,
+              reverse
+                  ? -minMaxAccel.minAccelerationMetersPerSecondSq
+                  : minMaxAccel.maxAccelerationMetersPerSecondSq);
+    }
+  }
+
+  @SuppressWarnings("MemberName")
+  private static class ConstrainedState {
+    PoseWithCurvature pose;
+    double distanceMeters;
+    double maxVelocityMetersPerSecond;
+    double minAccelerationMetersPerSecondSq;
+    double maxAccelerationMetersPerSecondSq;
+
+    ConstrainedState(
+        PoseWithCurvature pose,
+        double distanceMeters,
+        double maxVelocityMetersPerSecond,
+        double minAccelerationMetersPerSecondSq,
+        double maxAccelerationMetersPerSecondSq) {
+      this.pose = pose;
+      this.distanceMeters = distanceMeters;
+      this.maxVelocityMetersPerSecond = maxVelocityMetersPerSecond;
+      this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
+      this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
+    }
+
+    ConstrainedState() {
+      pose = new PoseWithCurvature();
+    }
+  }
+
+  @SuppressWarnings("serial")
+  public static class TrajectoryGenerationException extends RuntimeException {
+    public TrajectoryGenerationException(String message) {
+      super(message);
+    }
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java

@@ -0,0 +1,120 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import edu.wpi.first.math.WPIMathJNI;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import java.io.IOException;
+import java.nio.file.Path;
+import java.util.ArrayList;
+import java.util.List;
+
+public final class TrajectoryUtil {
+  private TrajectoryUtil() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Creates a trajectory from a double[] of elements.
+   *
+   * @param elements A double[] containing the raw elements of the trajectory.
+   * @return A trajectory created from the elements.
+   */
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  private static Trajectory createTrajectoryFromElements(double[] elements) {
+    // Make sure that the elements have the correct length.
+    if (elements.length % 7 != 0) {
+      throw new TrajectorySerializationException(
+          "An error occurred when converting trajectory elements into a trajectory.");
+    }
+
+    // Create a list of states from the elements.
+    List<Trajectory.State> states = new ArrayList<>();
+    for (int i = 0; i < elements.length; i += 7) {
+      states.add(
+          new Trajectory.State(
+              elements[i],
+              elements[i + 1],
+              elements[i + 2],
+              new Pose2d(elements[i + 3], elements[i + 4], new Rotation2d(elements[i + 5])),
+              elements[i + 6]));
+    }
+    return new Trajectory(states);
+  }
+
+  /**
+   * Returns a double[] of elements from the given trajectory.
+   *
+   * @param trajectory The trajectory to retrieve raw elements from.
+   * @return A double[] of elements from the given trajectory.
+   */
+  private static double[] getElementsFromTrajectory(Trajectory trajectory) {
+    // Create a double[] of elements and fill it from the trajectory states.
+    double[] elements = new double[trajectory.getStates().size() * 7];
+
+    for (int i = 0; i < trajectory.getStates().size() * 7; i += 7) {
+      var state = trajectory.getStates().get(i / 7);
+      elements[i] = state.timeSeconds;
+      elements[i + 1] = state.velocityMetersPerSecond;
+      elements[i + 2] = state.accelerationMetersPerSecondSq;
+      elements[i + 3] = state.poseMeters.getX();
+      elements[i + 4] = state.poseMeters.getY();
+      elements[i + 5] = state.poseMeters.getRotation().getRadians();
+      elements[i + 6] = state.curvatureRadPerMeter;
+    }
+    return elements;
+  }
+
+  /**
+   * Imports a Trajectory from a PathWeaver-style JSON file.
+   *
+   * @param path The path of the json file to import from
+   * @return The trajectory represented by the file.
+   * @throws IOException if reading from the file fails.
+   */
+  public static Trajectory fromPathweaverJson(Path path) throws IOException {
+    return createTrajectoryFromElements(WPIMathJNI.fromPathweaverJson(path.toString()));
+  }
+
+  /**
+   * Exports a Trajectory to a PathWeaver-style JSON file.
+   *
+   * @param trajectory The trajectory to export
+   * @param path The path of the file to export to
+   * @throws IOException if writing to the file fails.
+   */
+  public static void toPathweaverJson(Trajectory trajectory, Path path) throws IOException {
+    WPIMathJNI.toPathweaverJson(getElementsFromTrajectory(trajectory), path.toString());
+  }
+
+  /**
+   * Deserializes a Trajectory from PathWeaver-style JSON.
+   *
+   * @param json The string containing the serialized JSON
+   * @return the trajectory represented by the JSON
+   * @throws TrajectorySerializationException if deserialization of the string fails.
+   */
+  public static Trajectory deserializeTrajectory(String json) {
+    return createTrajectoryFromElements(WPIMathJNI.deserializeTrajectory(json));
+  }
+
+  /**
+   * Serializes a Trajectory to PathWeaver-style JSON.
+   *
+   * @param trajectory The trajectory to export
+   * @return The string containing the serialized JSON
+   * @throws TrajectorySerializationException if serialization of the trajectory fails.
+   */
+  public static String serializeTrajectory(Trajectory trajectory) {
+    return WPIMathJNI.serializeTrajectory(getElementsFromTrajectory(trajectory));
+  }
+
+  public static class TrajectorySerializationException extends RuntimeException {
+    public TrajectorySerializationException(String message) {
+      super(message);
+    }
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java

@@ -0,0 +1,302 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import java.util.Objects;
+
+/**
+ * A trapezoid-shaped velocity profile.
+ *
+ * <p>While this class can be used for a profiled movement from start to finish, the intended usage
+ * is to filter a reference's dynamics based on trapezoidal velocity constraints. To compute the
+ * reference obeying this constraint, do the following.
+ *
+ * <p>Initialization:
+ *
+ * <pre><code>
+ * TrapezoidProfile.Constraints constraints =
+ *   new TrapezoidProfile.Constraints(kMaxV, kMaxA);
+ * TrapezoidProfile.State previousProfiledReference =
+ *   new TrapezoidProfile.State(initialReference, 0.0);
+ * </code></pre>
+ *
+ * <p>Run on update:
+ *
+ * <pre><code>
+ * TrapezoidProfile profile =
+ *   new TrapezoidProfile(constraints, unprofiledReference, previousProfiledReference);
+ * previousProfiledReference = profile.calculate(timeSincePreviousUpdate);
+ * </code></pre>
+ *
+ * <p>where `unprofiledReference` is free to change between calls. Note that when the unprofiled
+ * reference is within the constraints, `calculate()` returns the unprofiled reference unchanged.
+ *
+ * <p>Otherwise, a timer can be started to provide monotonic values for `calculate()` and to
+ * determine when the profile has completed via `isFinished()`.
+ */
+public class TrapezoidProfile {
+  // The direction of the profile, either 1 for forwards or -1 for inverted
+  private int m_direction;
+
+  private Constraints m_constraints;
+  private State m_initial;
+  private State m_goal;
+
+  private double m_endAccel;
+  private double m_endFullSpeed;
+  private double m_endDeccel;
+
+  public static class Constraints {
+    @SuppressWarnings("MemberName")
+    public double maxVelocity;
+
+    @SuppressWarnings("MemberName")
+    public double maxAcceleration;
+
+    public Constraints() {
+      MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1);
+    }
+
+    /**
+     * Construct constraints for a TrapezoidProfile.
+     *
+     * @param maxVelocity maximum velocity
+     * @param maxAcceleration maximum acceleration
+     */
+    public Constraints(double maxVelocity, double maxAcceleration) {
+      this.maxVelocity = maxVelocity;
+      this.maxAcceleration = maxAcceleration;
+      MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1);
+    }
+  }
+
+  public static class State {
+    @SuppressWarnings("MemberName")
+    public double position;
+
+    @SuppressWarnings("MemberName")
+    public double velocity;
+
+    public State() {}
+
+    public State(double position, double velocity) {
+      this.position = position;
+      this.velocity = velocity;
+    }
+
+    @Override
+    public boolean equals(Object other) {
+      if (other instanceof State) {
+        State rhs = (State) other;
+        return this.position == rhs.position && this.velocity == rhs.velocity;
+      } else {
+        return false;
+      }
+    }
+
+    @Override
+    public int hashCode() {
+      return Objects.hash(position, velocity);
+    }
+  }
+
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
+   * @param goal The desired state when the profile is complete.
+   * @param initial The initial state (usually the current state).
+   */
+  public TrapezoidProfile(Constraints constraints, State goal, State initial) {
+    m_direction = shouldFlipAcceleration(initial, goal) ? -1 : 1;
+    m_constraints = constraints;
+    m_initial = direct(initial);
+    m_goal = direct(goal);
+
+    if (m_initial.velocity > m_constraints.maxVelocity) {
+      m_initial.velocity = m_constraints.maxVelocity;
+    }
+
+    // Deal with a possibly truncated motion profile (with nonzero initial or
+    // final velocity) by calculating the parameters as if the profile began and
+    // ended at zero velocity
+    double cutoffBegin = m_initial.velocity / m_constraints.maxAcceleration;
+    double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+    double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
+    double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
+
+    // Now we can calculate the parameters as if it was a full trapezoid instead
+    // of a truncated one
+
+    double fullTrapezoidDist =
+        cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
+    double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+    double fullSpeedDist =
+        fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration;
+
+    // Handle the case where the profile never reaches full speed
+    if (fullSpeedDist < 0) {
+      accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+      fullSpeedDist = 0;
+    }
+
+    m_endAccel = accelerationTime - cutoffBegin;
+    m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+    m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+  }
+
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
+   * @param goal The desired state when the profile is complete.
+   */
+  public TrapezoidProfile(Constraints constraints, State goal) {
+    this(constraints, goal, new State(0, 0));
+  }
+
+  /**
+   * Calculate the correct position and velocity for the profile at a time t where the beginning of
+   * the profile was at time t = 0.
+   *
+   * @param t The time since the beginning of the profile.
+   */
+  public State calculate(double t) {
+    State result = new State(m_initial.position, m_initial.velocity);
+
+    if (t < m_endAccel) {
+      result.velocity += t * m_constraints.maxAcceleration;
+      result.position += (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+    } else if (t < m_endFullSpeed) {
+      result.velocity = m_constraints.maxVelocity;
+      result.position +=
+          (m_initial.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
+              + m_constraints.maxVelocity * (t - m_endAccel);
+    } else if (t <= m_endDeccel) {
+      result.velocity = m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
+      double timeLeft = m_endDeccel - t;
+      result.position =
+          m_goal.position
+              - (m_goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft;
+    } else {
+      result = m_goal;
+    }
+
+    return direct(result);
+  }
+
+  /**
+   * Returns the time left until a target distance in the profile is reached.
+   *
+   * @param target The target distance.
+   */
+  public double timeLeftUntil(double target) {
+    double position = m_initial.position * m_direction;
+    double velocity = m_initial.velocity * m_direction;
+
+    double endAccel = m_endAccel * m_direction;
+    double endFullSpeed = m_endFullSpeed * m_direction - endAccel;
+
+    if (target < position) {
+      endAccel = -endAccel;
+      endFullSpeed = -endFullSpeed;
+      velocity = -velocity;
+    }
+
+    endAccel = Math.max(endAccel, 0);
+    endFullSpeed = Math.max(endFullSpeed, 0);
+    double endDeccel = m_endDeccel - endAccel - endFullSpeed;
+    endDeccel = Math.max(endDeccel, 0);
+
+    final double acceleration = m_constraints.maxAcceleration;
+    final double decceleration = -m_constraints.maxAcceleration;
+
+    double distToTarget = Math.abs(target - position);
+    if (distToTarget < 1e-6) {
+      return 0;
+    }
+
+    double accelDist = velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
+
+    double deccelVelocity;
+    if (endAccel > 0) {
+      deccelVelocity = Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist));
+    } else {
+      deccelVelocity = velocity;
+    }
+
+    double deccelDist = deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
+
+    deccelDist = Math.max(deccelDist, 0);
+
+    double fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
+
+    if (accelDist > distToTarget) {
+      accelDist = distToTarget;
+      fullSpeedDist = 0;
+      deccelDist = 0;
+    } else if (accelDist + fullSpeedDist > distToTarget) {
+      fullSpeedDist = distToTarget - accelDist;
+      deccelDist = 0;
+    } else {
+      deccelDist = distToTarget - fullSpeedDist - accelDist;
+    }
+
+    double accelTime =
+        (-velocity + Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist)))
+            / acceleration;
+
+    double deccelTime =
+        (-deccelVelocity
+                + Math.sqrt(
+                    Math.abs(deccelVelocity * deccelVelocity + 2 * decceleration * deccelDist)))
+            / decceleration;
+
+    double fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
+
+    return accelTime + fullSpeedTime + deccelTime;
+  }
+
+  /** Returns the total time the profile takes to reach the goal. */
+  public double totalTime() {
+    return m_endDeccel;
+  }
+
+  /**
+   * Returns true if the profile has reached the goal.
+   *
+   * <p>The profile has reached the goal if the time since the profile started has exceeded the
+   * profile's total time.
+   *
+   * @param t The time since the beginning of the profile.
+   */
+  public boolean isFinished(double t) {
+    return t >= totalTime();
+  }
+
+  /**
+   * Returns true if the profile inverted.
+   *
+   * <p>The profile is inverted if goal position is less than the initial position.
+   *
+   * @param initial The initial state (usually the current state).
+   * @param goal The desired state when the profile is complete.
+   */
+  private static boolean shouldFlipAcceleration(State initial, State goal) {
+    return initial.position > goal.position;
+  }
+
+  // Flip the sign of the velocity and position if the profile is inverted
+  private State direct(State in) {
+    State result = new State(in.position, in.velocity);
+    result.position = result.position * m_direction;
+    result.velocity = result.velocity * m_direction;
+    return result;
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java

@@ -0,0 +1,68 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory.constraint;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+/**
+ * A constraint on the maximum absolute centripetal acceleration allowed when traversing a
+ * trajectory. The centripetal acceleration of a robot is defined as the velocity squared divided by
+ * the radius of curvature.
+ *
+ * <p>Effectively, limiting the maximum centripetal acceleration will cause the robot to slow down
+ * around tight turns, making it easier to track trajectories with sharp turns.
+ */
+public class CentripetalAccelerationConstraint implements TrajectoryConstraint {
+  private final double m_maxCentripetalAccelerationMetersPerSecondSq;
+
+  /**
+   * Constructs a centripetal acceleration constraint.
+   *
+   * @param maxCentripetalAccelerationMetersPerSecondSq The max centripetal acceleration.
+   */
+  public CentripetalAccelerationConstraint(double maxCentripetalAccelerationMetersPerSecondSq) {
+    m_maxCentripetalAccelerationMetersPerSecondSq = maxCentripetalAccelerationMetersPerSecondSq;
+  }
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *     constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  @Override
+  public double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    // ac = v^2 / r
+    // k (curvature) = 1 / r
+
+    // therefore, ac = v^2 * k
+    // ac / k = v^2
+    // v = std::sqrt(ac / k)
+
+    return Math.sqrt(
+        m_maxCentripetalAccelerationMetersPerSecondSq / Math.abs(curvatureRadPerMeter));
+  }
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory given pose,
+   * curvature, and speed.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    // The acceleration of the robot has no impact on the centripetal acceleration
+    // of the robot.
+    return new MinMax();
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java

@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory.constraint;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+
+/**
+ * A class that enforces constraints on the differential drive kinematics. This can be used to
+ * ensure that the trajectory is constructed so that the commanded velocities for both sides of the
+ * drivetrain stay below a certain limit.
+ */
+public class DifferentialDriveKinematicsConstraint implements TrajectoryConstraint {
+  private final double m_maxSpeedMetersPerSecond;
+  private final DifferentialDriveKinematics m_kinematics;
+
+  /**
+   * Constructs a differential drive dynamics constraint.
+   *
+   * @param kinematics A kinematics component describing the drive geometry.
+   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
+   */
+  public DifferentialDriveKinematicsConstraint(
+      final DifferentialDriveKinematics kinematics, double maxSpeedMetersPerSecond) {
+    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
+    m_kinematics = kinematics;
+  }
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *     constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  @Override
+  public double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    // Create an object to represent the current chassis speeds.
+    var chassisSpeeds =
+        new ChassisSpeeds(
+            velocityMetersPerSecond, 0, velocityMetersPerSecond * curvatureRadPerMeter);
+
+    // Get the wheel speeds and normalize them to within the max velocity.
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+    wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
+
+    // Return the new linear chassis speed.
+    return m_kinematics.toChassisSpeeds(wheelSpeeds).vxMetersPerSecond;
+  }
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory given pose,
+   * curvature, and speed.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    return new MinMax();
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveVoltageConstraint.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.trajectory.constraint;
+
+import static edu.wpi.first.wpiutil.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+
+/**
+ * A class that enforces constraints on differential drive voltage expenditure based on the motor
+ * dynamics and the drive kinematics. Ensures that the acceleration of any wheel of the robot while
+ * following the trajectory is never higher than what can be achieved with the given maximum
+ * voltage.
+ */
+public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint {
+  private final SimpleMotorFeedforward m_feedforward;
+  private final DifferentialDriveKinematics m_kinematics;
+  private final double m_maxVoltage;
+
+  /**
+   * Creates a new DifferentialDriveVoltageConstraint.
+   *
+   * @param feedforward A feedforward component describing the behavior of the drive.
+   * @param kinematics A kinematics component describing the drive geometry.
+   * @param maxVoltage The maximum voltage available to the motors while following the path. Should
+   *     be somewhat less than the nominal battery voltage (12V) to account for "voltage sag" due to
+   *     current draw.
+   */
+  public DifferentialDriveVoltageConstraint(
+      SimpleMotorFeedforward feedforward,
+      DifferentialDriveKinematics kinematics,
+      double maxVoltage) {
+    m_feedforward =
+        requireNonNullParam(feedforward, "feedforward", "DifferentialDriveVoltageConstraint");
+    m_kinematics =
+        requireNonNullParam(kinematics, "kinematics", "DifferentialDriveVoltageConstraint");
+    m_maxVoltage = maxVoltage;
+  }
+
+  @Override
+  public double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    return Double.POSITIVE_INFINITY;
+  }
+
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+
+    var wheelSpeeds =
+        m_kinematics.toWheelSpeeds(
+            new ChassisSpeeds(
+                velocityMetersPerSecond, 0, velocityMetersPerSecond * curvatureRadPerMeter));
+
+    double maxWheelSpeed =
+        Math.max(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond);
+    double minWheelSpeed =
+        Math.min(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond);
+
+    // Calculate maximum/minimum possible accelerations from motor dynamics
+    // and max/min wheel speeds
+    double maxWheelAcceleration =
+        m_feedforward.maxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
+    double minWheelAcceleration =
+        m_feedforward.minAchievableAcceleration(m_maxVoltage, minWheelSpeed);
+
+    // Robot chassis turning on radius = 1/|curvature|.  Outer wheel has radius
+    // increased by half of the trackwidth T.  Inner wheel has radius decreased
+    // by half of the trackwidth.  Achassis / radius = Aouter / (radius + T/2), so
+    // Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 + |curvature|T/2).
+    // Inner wheel is similar.
+
+    // sgn(speed) term added to correctly account for which wheel is on
+    // outside of turn:
+    // If moving forward, max acceleration constraint corresponds to wheel on outside of turn
+    // If moving backward, max acceleration constraint corresponds to wheel on inside of turn
+
+    // When velocity is zero, then wheel velocities are uniformly zero (robot cannot be
+    // turning on its center) - we have to treat this as a special case, as it breaks
+    // the signum function.  Both max and min acceleration are *reduced in magnitude*
+    // in this case.
+
+    double maxChassisAcceleration;
+    double minChassisAcceleration;
+
+    if (velocityMetersPerSecond == 0) {
+      maxChassisAcceleration =
+          maxWheelAcceleration
+              / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2);
+      minChassisAcceleration =
+          minWheelAcceleration
+              / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2);
+    } else {
+      maxChassisAcceleration =
+          maxWheelAcceleration
+              / (1
+                  + m_kinematics.trackWidthMeters
+                      * Math.abs(curvatureRadPerMeter)
+                      * Math.signum(velocityMetersPerSecond)
+                      / 2);
+      minChassisAcceleration =
+          minWheelAcceleration
+              / (1
+                  - m_kinematics.trackWidthMeters
+                      * Math.abs(curvatureRadPerMeter)
+                      * Math.signum(velocityMetersPerSecond)
+                      / 2);
+    }
+
+    // When turning about a point inside of 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.
+
+    if ((m_kinematics.trackWidthMeters / 2) > (1 / Math.abs(curvatureRadPerMeter))) {
+      if (velocityMetersPerSecond > 0) {
+        minChassisAcceleration = -minChassisAcceleration;
+      } else if (velocityMetersPerSecond < 0) {
+        maxChassisAcceleration = -maxChassisAcceleration;
+      }
+    }
+
+    return new MinMax(minChassisAcceleration, maxChassisAcceleration);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java

@@ -0,0 +1,77 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory.constraint;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+/** Enforces a particular constraint only within an elliptical region. */
+public class EllipticalRegionConstraint implements TrajectoryConstraint {
+  private final Translation2d m_center;
+  private final Translation2d m_radii;
+  private final TrajectoryConstraint m_constraint;
+
+  /**
+   * Constructs a new EllipticalRegionConstraint.
+   *
+   * @param center The center of the ellipse in which to enforce the constraint.
+   * @param xWidth The width of the ellipse in which to enforce the constraint.
+   * @param yWidth The height of the ellipse in which to enforce the constraint.
+   * @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,
+      double yWidth,
+      Rotation2d rotation,
+      TrajectoryConstraint constraint) {
+    m_center = center;
+    m_radii = new Translation2d(xWidth / 2.0, yWidth / 2.0).rotateBy(rotation);
+    m_constraint = constraint;
+  }
+
+  @Override
+  public double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    if (isPoseInRegion(poseMeters)) {
+      return m_constraint.getMaxVelocityMetersPerSecond(
+          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
+    } else {
+      return Double.POSITIVE_INFINITY;
+    }
+  }
+
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    if (isPoseInRegion(poseMeters)) {
+      return m_constraint.getMinMaxAccelerationMetersPerSecondSq(
+          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
+    } else {
+      return new MinMax();
+    }
+  }
+
+  /**
+   * Returns whether the specified robot pose is within the region that the constraint is enforced
+   * in.
+   *
+   * @param robotPose The robot pose.
+   * @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
+    // 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);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MaxVelocityConstraint.java

@@ -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.
+
+package edu.wpi.first.math.trajectory.constraint;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+/**
+ * Represents a constraint that enforces a max velocity. This can be composed with the {@link
+ * EllipticalRegionConstraint} or {@link RectangularRegionConstraint} to enforce a max velocity in a
+ * region.
+ */
+public class MaxVelocityConstraint implements TrajectoryConstraint {
+  private final double m_maxVelocity;
+
+  /**
+   * Constructs a new MaxVelocityConstraint.
+   *
+   * @param maxVelocityMetersPerSecond The max velocity.
+   */
+  public MaxVelocityConstraint(double maxVelocityMetersPerSecond) {
+    m_maxVelocity = maxVelocityMetersPerSecond;
+  }
+
+  @Override
+  public double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    return m_maxVelocity;
+  }
+
+  @Override
+  public TrajectoryConstraint.MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    return new MinMax();
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java

@@ -0,0 +1,78 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory.constraint;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+
+/**
+ * A class that enforces constraints on the mecanum drive kinematics. This can be used to ensure
+ * that the trajectory is constructed so that the commanded velocities for all 4 wheels of the
+ * drivetrain stay below a certain limit.
+ */
+public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
+  private final double m_maxSpeedMetersPerSecond;
+  private final MecanumDriveKinematics m_kinematics;
+
+  /**
+   * Constructs a mecanum drive dynamics constraint.
+   *
+   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
+   */
+  public MecanumDriveKinematicsConstraint(
+      final MecanumDriveKinematics kinematics, double maxSpeedMetersPerSecond) {
+    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
+    m_kinematics = kinematics;
+  }
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *     constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  @Override
+  public double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    // Represents the velocity of the chassis in the x direction
+    var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
+
+    // Represents the velocity of the chassis in the y direction
+    var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
+
+    // Create an object to represent the current chassis speeds.
+    var chassisSpeeds =
+        new ChassisSpeeds(xdVelocity, ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
+
+    // Get the wheel speeds and normalize them to within the max velocity.
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+    wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
+
+    // Convert normalized wheel speeds back to chassis speeds
+    var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    // Return the new linear chassis speed.
+    return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
+  }
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory given pose,
+   * curvature, and speed.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    return new MinMax();
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/RectangularRegionConstraint.java

@@ -0,0 +1,67 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory.constraint;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+/** Enforces a particular constraint only within a rectangular region. */
+public class RectangularRegionConstraint implements TrajectoryConstraint {
+  private final Translation2d m_bottomLeftPoint;
+  private final Translation2d m_topRightPoint;
+  private final TrajectoryConstraint m_constraint;
+
+  /**
+   * Constructs a new RectangularRegionConstraint.
+   *
+   * @param bottomLeftPoint The bottom left point of the rectangular region in which to enforce the
+   *     constraint.
+   * @param topRightPoint The top right point of the rectangular region in which to enforce the
+   *     constraint.
+   * @param constraint The constraint to enforce when the robot is within the region.
+   */
+  public RectangularRegionConstraint(
+      Translation2d bottomLeftPoint, Translation2d topRightPoint, TrajectoryConstraint constraint) {
+    m_bottomLeftPoint = bottomLeftPoint;
+    m_topRightPoint = topRightPoint;
+    m_constraint = constraint;
+  }
+
+  @Override
+  public double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    if (isPoseInRegion(poseMeters)) {
+      return m_constraint.getMaxVelocityMetersPerSecond(
+          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
+    } else {
+      return Double.POSITIVE_INFINITY;
+    }
+  }
+
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    if (isPoseInRegion(poseMeters)) {
+      return m_constraint.getMinMaxAccelerationMetersPerSecondSq(
+          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
+    } else {
+      return new MinMax();
+    }
+  }
+
+  /**
+   * Returns whether the specified robot pose is within the region that the constraint is enforced
+   * in.
+   *
+   * @param robotPose The robot pose.
+   * @return Whether the robot pose is within the constraint region.
+   */
+  public boolean isPoseInRegion(Pose2d robotPose) {
+    return robotPose.getX() >= m_bottomLeftPoint.getX()
+        && robotPose.getX() <= m_topRightPoint.getX()
+        && robotPose.getY() >= m_bottomLeftPoint.getY()
+        && robotPose.getY() <= m_topRightPoint.getY();
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java

@@ -0,0 +1,78 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory.constraint;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+
+/**
+ * A class that enforces constraints on the swerve drive kinematics. This can be used to ensure that
+ * the trajectory is constructed so that the commanded velocities for all 4 wheels of the drivetrain
+ * stay below a certain limit.
+ */
+public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
+  private final double m_maxSpeedMetersPerSecond;
+  private final SwerveDriveKinematics m_kinematics;
+
+  /**
+   * Constructs a swerve drive dynamics constraint.
+   *
+   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
+   */
+  public SwerveDriveKinematicsConstraint(
+      final SwerveDriveKinematics kinematics, double maxSpeedMetersPerSecond) {
+    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
+    m_kinematics = kinematics;
+  }
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *     constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  @Override
+  public double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    // Represents the velocity of the chassis in the x direction
+    var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
+
+    // Represents the velocity of the chassis in the y direction
+    var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
+
+    // Create an object to represent the current chassis speeds.
+    var chassisSpeeds =
+        new ChassisSpeeds(xdVelocity, ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
+
+    // Get the wheel speeds and normalize them to within the max velocity.
+    var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds);
+    SwerveDriveKinematics.normalizeWheelSpeeds(wheelSpeeds, m_maxSpeedMetersPerSecond);
+
+    // Convert normalized wheel speeds back to chassis speeds
+    var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    // Return the new linear chassis speed.
+    return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
+  }
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory given pose,
+   * curvature, and speed.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    return new MinMax();
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java

@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory.constraint;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+/**
+ * An interface for defining user-defined velocity and acceleration constraints while generating
+ * trajectories.
+ */
+public interface TrajectoryConstraint {
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *     constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond);
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory given pose,
+   * curvature, and speed.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  MinMax getMinMaxAccelerationMetersPerSecondSq(
+      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;
+
+    /**
+     * Constructs a MinMax.
+     *
+     * @param minAccelerationMetersPerSecondSq The minimum acceleration.
+     * @param maxAccelerationMetersPerSecondSq The maximum acceleration.
+     */
+    public MinMax(
+        double minAccelerationMetersPerSecondSq, double maxAccelerationMetersPerSecondSq) {
+      this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
+      this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
+    }
+
+    /** Constructs a MinMax with default values. */
+    public MinMax() {}
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/util/Units.java

@@ -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.
+
+package edu.wpi.first.math.util;
+
+/** Utility class that converts between commonly used units in FRC. */
+public final class Units {
+  private static final double kInchesPerFoot = 12.0;
+  private static final double kMetersPerInch = 0.0254;
+  private static final double kSecondsPerMinute = 60;
+  private static final double kKilogramsPerLb = 0.453592;
+
+  /** Utility class, so constructor is private. */
+  private Units() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Converts given meters to feet.
+   *
+   * @param meters The meters to convert to feet.
+   * @return Feet converted from meters.
+   */
+  public static double metersToFeet(double meters) {
+    return metersToInches(meters) / kInchesPerFoot;
+  }
+
+  /**
+   * Converts given feet to meters.
+   *
+   * @param feet The feet to convert to meters.
+   * @return Meters converted from feet.
+   */
+  public static double feetToMeters(double feet) {
+    return inchesToMeters(feet * kInchesPerFoot);
+  }
+
+  /**
+   * Converts given meters to inches.
+   *
+   * @param meters The meters to convert to inches.
+   * @return Inches converted from meters.
+   */
+  public static double metersToInches(double meters) {
+    return meters / kMetersPerInch;
+  }
+
+  /**
+   * Converts given inches to meters.
+   *
+   * @param inches The inches to convert to meters.
+   * @return Meters converted from inches.
+   */
+  public static double inchesToMeters(double inches) {
+    return inches * kMetersPerInch;
+  }
+
+  /**
+   * Converts given degrees to radians.
+   *
+   * @param degrees The degrees to convert to radians.
+   * @return Radians converted from degrees.
+   */
+  public static double degreesToRadians(double degrees) {
+    return Math.toRadians(degrees);
+  }
+
+  /**
+   * Converts given radians to degrees.
+   *
+   * @param radians The radians to convert to degrees.
+   * @return Degrees converted from radians.
+   */
+  public static double radiansToDegrees(double radians) {
+    return Math.toDegrees(radians);
+  }
+
+  /**
+   * Converts rotations per minute to radians per second.
+   *
+   * @param rpm The rotations per minute to convert to radians per second.
+   * @return Radians per second converted from rotations per minute.
+   */
+  public static double rotationsPerMinuteToRadiansPerSecond(double rpm) {
+    return rpm * Math.PI / (kSecondsPerMinute / 2);
+  }
+
+  /**
+   * Converts radians per second to rotations per minute.
+   *
+   * @param radiansPerSecond The radians per second to convert to from rotations per minute.
+   * @return Rotations per minute converted from radians per second.
+   */
+  public static double radiansPerSecondToRotationsPerMinute(double radiansPerSecond) {
+    return radiansPerSecond * (kSecondsPerMinute / 2) / Math.PI;
+  }
+
+  /**
+   * Converts kilograms into lbs (pound-mass).
+   *
+   * @param kilograms The kilograms to convert to lbs (pound-mass).
+   * @return Lbs (pound-mass) converted from kilograms.
+   */
+  public static double kilogramsToLbs(double kilograms) {
+    return kilograms / kKilogramsPerLb;
+  }
+
+  /**
+   * Converts lbs (pound-mass) into kilograms.
+   *
+   * @param lbs The lbs (pound-mass) to convert to kilograms.
+   * @return Kilograms converted from lbs (pound-mass).
+   */
+  public static double lbsToKilograms(double lbs) {
+    return lbs * kKilogramsPerLb;
+  }
+}