SCRIPT Move java files

wpimath/src/main/java/org/wpilib/math/controller/ArmFeedforward.java

@@ -0,0 +1,276 @@
+// 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.controller.proto.ArmFeedforwardProto;
+import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct;
+import edu.wpi.first.math.jni.ArmFeedforwardJNI;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+
+/**
+ * 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).
+ */
+public class ArmFeedforward implements ProtobufSerializable, StructSerializable {
+  /** The static gain, in volts. */
+  private double ks;
+
+  /** The gravity gain, in volts. */
+  private double kg;
+
+  /** The velocity gain, in V/(rad/s). */
+  private double kv;
+
+  /** The acceleration gain, in V/(rad/s²). */
+  private double ka;
+
+  /** The period, in seconds. */
+  private final double m_dt;
+
+  /**
+   * Creates a new ArmFeedforward with the specified gains and period.
+   *
+   * @param ks The static gain in volts.
+   * @param kg The gravity gain in volts.
+   * @param kv The velocity gain in V/(rad/s).
+   * @param ka The acceleration gain in V/(rad/s²).
+   * @param dt The period in seconds.
+   * @throws IllegalArgumentException for kv < zero.
+   * @throws IllegalArgumentException for ka < zero.
+   * @throws IllegalArgumentException for period ≤ zero.
+   */
+  public ArmFeedforward(double ks, double kg, double kv, double ka, double dt) {
+    this.ks = ks;
+    this.kg = kg;
+    this.kv = kv;
+    this.ka = ka;
+    if (kv < 0.0) {
+      throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!");
+    }
+    if (ka < 0.0) {
+      throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
+    }
+    if (dt <= 0.0) {
+      throw new IllegalArgumentException("period must be a positive number, got " + dt + "!");
+    }
+    m_dt = dt;
+  }
+
+  /**
+   * Creates a new ArmFeedforward with the specified gains. The period is defaulted to 20 ms.
+   *
+   * @param ks The static gain in volts.
+   * @param kg The gravity gain in volts.
+   * @param kv The velocity gain in V/(rad/s).
+   * @param ka The acceleration gain in V/(rad/s²).
+   * @throws IllegalArgumentException for kv &lt; zero.
+   * @throws IllegalArgumentException for ka &lt; zero.
+   */
+  public ArmFeedforward(double ks, double kg, double kv, double ka) {
+    this(ks, kg, kv, ka, 0.020);
+  }
+
+  /**
+   * Creates a new ArmFeedforward with the specified gains. The period is defaulted to 20 ms.
+   *
+   * @param ks The static gain in volts.
+   * @param kg The gravity gain in volts.
+   * @param kv The velocity gain in V/(rad/s).
+   * @throws IllegalArgumentException for kv &lt; zero.
+   */
+  public ArmFeedforward(double ks, double kg, double kv) {
+    this(ks, kg, kv, 0);
+  }
+
+  /**
+   * Sets the static gain.
+   *
+   * @param ks The static gain in volts.
+   */
+  public void setKs(double ks) {
+    this.ks = ks;
+  }
+
+  /**
+   * Sets the gravity gain.
+   *
+   * @param kg The gravity gain in volts.
+   */
+  public void setKg(double kg) {
+    this.kg = kg;
+  }
+
+  /**
+   * Sets the velocity gain.
+   *
+   * @param kv The velocity gain in V/(rad/s).
+   */
+  public void setKv(double kv) {
+    this.kv = kv;
+  }
+
+  /**
+   * Sets the acceleration gain.
+   *
+   * @param ka The acceleration gain in V/(rad/s²).
+   */
+  public void setKa(double ka) {
+    this.ka = ka;
+  }
+
+  /**
+   * Returns the static gain in volts.
+   *
+   * @return The static gain in volts.
+   */
+  public double getKs() {
+    return ks;
+  }
+
+  /**
+   * Returns the gravity gain in volts.
+   *
+   * @return The gravity gain in volts.
+   */
+  public double getKg() {
+    return kg;
+  }
+
+  /**
+   * Returns the velocity gain in V/(rad/s).
+   *
+   * @return The velocity gain.
+   */
+  public double getKv() {
+    return kv;
+  }
+
+  /**
+   * Returns the acceleration gain in V/(rad/s²).
+   *
+   * @return The acceleration gain.
+   */
+  public double getKa() {
+    return ka;
+  }
+
+  /**
+   * Returns the period in seconds.
+   *
+   * @return The period in seconds.
+   */
+  public double getDt() {
+    return m_dt;
+  }
+
+  /**
+   * Calculates the feedforward from the gains and velocity setpoint assuming continuous control
+   * (acceleration is assumed to be zero).
+   *
+   * @param position The position setpoint in radians. This angle should be measured from the
+   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
+   *     your encoder does not follow this convention, an offset should be added.
+   * @param velocity The velocity setpoint in radians per second.
+   * @return The computed feedforward.
+   */
+  public double calculate(double position, double velocity) {
+    return ks * Math.signum(velocity) + kg * Math.cos(position) + kv * velocity;
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints assuming continuous control.
+   *
+   * @param currentAngle The current angle in radians. This angle should be measured from the
+   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel to the floor). If
+   *     your encoder does not follow this convention, an offset should be added.
+   * @param currentVelocity The current velocity setpoint in radians per second.
+   * @param nextVelocity The next velocity setpoint in radians per second.
+   * @return The computed feedforward in volts.
+   */
+  public double calculate(double currentAngle, double currentVelocity, double nextVelocity) {
+    return ArmFeedforwardJNI.calculate(
+        ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, m_dt);
+  }
+
+  // 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, in radians. This angle should be measured from the
+   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
+   *     your encoder does not follow this convention, an offset should be added.
+   * @param acceleration The acceleration of the arm, in (rad/s²).
+   * @return The maximum possible velocity in (rad/s) 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) * kg - 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, in volts.
+   * @param angle The angle of the arm, in radians. This angle should be measured from the
+   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
+   *     your encoder does not follow this convention, an offset should be added.
+   * @param acceleration The acceleration of the arm, in (rad/s²).
+   * @return The minimum possible velocity in (rad/s) 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) * kg - 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, in volts.
+   * @param angle The angle of the arm, in radians. This angle should be measured from the
+   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
+   *     your encoder does not follow this convention, an offset should be added.
+   * @param velocity The velocity of the elevator, in (rad/s)
+   * @return The maximum possible acceleration in (rad/s²) at the given velocity.
+   */
+  public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) {
+    return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kg - 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, in volts.
+   * @param angle The angle of the arm, in radians. This angle should be measured from the
+   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
+   *     your encoder does not follow this convention, an offset should be added.
+   * @param velocity The velocity of the elevator, in (rad/s)
+   * @return The maximum possible acceleration in (rad/s²) at the given velocity.
+   */
+  public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) {
+    return maxAchievableAcceleration(-maxVoltage, angle, velocity);
+  }
+
+  /** Arm feedforward struct for serialization. */
+  public static final ArmFeedforwardStruct struct = new ArmFeedforwardStruct();
+
+  /** Arm feedforward protobuf for serialization. */
+  public static final ArmFeedforwardProto proto = new ArmFeedforwardProto();
+}

wpimath/src/main/java/org/wpilib/math/controller/BangBangController.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.controller;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Implements a bang-bang controller, which outputs either 0 or 1 depending on whether the
+ * measurement is less than the setpoint. This maximally-aggressive control approach works very well
+ * for velocity control of high-inertia mechanisms, and poorly on most other things.
+ *
+ * <p>Note that this is an *asymmetric* bang-bang controller - it will not exert any control effort
+ * in the reverse direction (e.g. it won't try to slow down an over-speeding shooter wheel). This
+ * asymmetry is *extremely important.* Bang-bang control is extremely simple, but also potentially
+ * hazardous. Always ensure that your motor controllers are set to "coast" before attempting to
+ * control them with a bang-bang controller.
+ */
+public class BangBangController implements Sendable {
+  private static int instances;
+
+  private double m_tolerance;
+
+  private double m_setpoint;
+  private double m_measurement;
+
+  /**
+   * Creates a new bang-bang controller.
+   *
+   * <p>Always ensure that your motor controllers are set to "coast" before attempting to control
+   * them with a bang-bang controller.
+   *
+   * @param tolerance Tolerance for {@link #atSetpoint() atSetpoint}.
+   */
+  @SuppressWarnings("this-escape")
+  public BangBangController(double tolerance) {
+    instances++;
+
+    setTolerance(tolerance);
+
+    SendableRegistry.add(this, "BangBangController", instances);
+
+    MathSharedStore.reportUsage("BangBangController", String.valueOf(instances));
+  }
+
+  /**
+   * Creates a new bang-bang controller.
+   *
+   * <p>Always ensure that your motor controllers are set to "coast" before attempting to control
+   * them with a bang-bang controller.
+   */
+  public BangBangController() {
+    this(Double.POSITIVE_INFINITY);
+  }
+
+  /**
+   * Sets the setpoint for the bang-bang controller.
+   *
+   * @param setpoint The desired setpoint.
+   */
+  public void setSetpoint(double setpoint) {
+    m_setpoint = setpoint;
+  }
+
+  /**
+   * Returns the current setpoint of the bang-bang controller.
+   *
+   * @return The current setpoint.
+   */
+  public double getSetpoint() {
+    return m_setpoint;
+  }
+
+  /**
+   * Returns true if the error is within the tolerance of the setpoint.
+   *
+   * @return Whether the error is within the acceptable bounds.
+   */
+  public boolean atSetpoint() {
+    return Math.abs(m_setpoint - m_measurement) < m_tolerance;
+  }
+
+  /**
+   * Sets the error within which atSetpoint will return true.
+   *
+   * @param tolerance Position error which is tolerable.
+   */
+  public final void setTolerance(double tolerance) {
+    m_tolerance = tolerance;
+  }
+
+  /**
+   * Returns the current tolerance of the controller.
+   *
+   * @return The current tolerance.
+   */
+  public double getTolerance() {
+    return m_tolerance;
+  }
+
+  /**
+   * Returns the current measurement of the process variable.
+   *
+   * @return The current measurement of the process variable.
+   */
+  public double getMeasurement() {
+    return m_measurement;
+  }
+
+  /**
+   * Returns the current error.
+   *
+   * @return The current error.
+   */
+  public double getError() {
+    return m_setpoint - m_measurement;
+  }
+
+  /**
+   * Returns the calculated control output.
+   *
+   * <p>Always ensure that your motor controllers are set to "coast" before attempting to control
+   * them with a bang-bang controller.
+   *
+   * @param measurement The most recent measurement of the process variable.
+   * @param setpoint The setpoint for the process variable.
+   * @return The calculated motor output (0 or 1).
+   */
+  public double calculate(double measurement, double setpoint) {
+    m_measurement = measurement;
+    m_setpoint = setpoint;
+
+    return measurement < setpoint ? 1 : 0;
+  }
+
+  /**
+   * Returns the calculated control output.
+   *
+   * @param measurement The most recent measurement of the process variable.
+   * @return The calculated motor output (0 or 1).
+   */
+  public double calculate(double measurement) {
+    return calculate(measurement, m_setpoint);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("BangBangController");
+    builder.addDoubleProperty("tolerance", this::getTolerance, this::setTolerance);
+    builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
+    builder.addDoubleProperty("measurement", this::getMeasurement, null);
+    builder.addDoubleProperty("error", this::getError, null);
+    builder.addBooleanProperty("atSetpoint", this::atSetpoint, null);
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/ControlAffinePlantInversionFeedforward.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.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 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 <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ */
+public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs extends Num> {
+  /** The current reference state. */
+  private Matrix<States, N1> m_r;
+
+  /** The computed feedforward. */
+  private Matrix<Inputs, N1> m_uff;
+
+  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 dt The timestep between calls of calculate() in seconds.
+   */
+  public ControlAffinePlantInversionFeedforward(
+      Nat<States> states,
+      Nat<Inputs> inputs,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      double dt) {
+    this.m_dt = dt;
+    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();
+  }
+
+  /**
+   * 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 dt The timestep between calls of calculate() in seconds.
+   */
+  public ControlAffinePlantInversionFeedforward(
+      Nat<States> states,
+      Nat<Inputs> inputs,
+      Function<Matrix<States, N1>, Matrix<States, N1>> f,
+      Matrix<States, Inputs> B,
+      double dt) {
+    this.m_dt = dt;
+    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();
+  }
+
+  /**
+   * 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 final void reset(Matrix<States, N1> initialState) {
+    m_r = initialState;
+    m_uff.fill(0.0);
+  }
+
+  /** Resets the feedforward with a zero initial state vector. */
+  public final 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.
+   */
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
+    var rDot = nextR.minus(r).div(m_dt);
+
+    // ṙ = f(r) + Bu
+    // Bu = ṙ − f(r)
+    // u = B⁺(ṙ − f(r))
+    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/org/wpilib/math/controller/DifferentialDriveAccelerationLimiter.java

@@ -0,0 +1,131 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+
+/**
+ * Filters the provided voltages to limit a differential drive's linear and angular acceleration.
+ *
+ * <p>The differential drive model can be created via the functions in {@link
+ * edu.wpi.first.math.system.plant.LinearSystemId}.
+ */
+public class DifferentialDriveAccelerationLimiter {
+  private final LinearSystem<N2, N2, N2> m_system;
+  private final double m_trackwidth;
+  private final double m_minLinearAccel;
+  private final double m_maxLinearAccel;
+  private final double m_maxAngularAccel;
+
+  /**
+   * Constructs a DifferentialDriveAccelerationLimiter.
+   *
+   * @param system The differential drive dynamics.
+   * @param trackwidth The distance between the differential drive's left and right wheels in
+   *     meters.
+   * @param maxLinearAccel The maximum linear acceleration in meters per second squared.
+   * @param maxAngularAccel The maximum angular acceleration in radians per second squared.
+   */
+  public DifferentialDriveAccelerationLimiter(
+      LinearSystem<N2, N2, N2> system,
+      double trackwidth,
+      double maxLinearAccel,
+      double maxAngularAccel) {
+    this(system, trackwidth, -maxLinearAccel, maxLinearAccel, maxAngularAccel);
+  }
+
+  /**
+   * Constructs a DifferentialDriveAccelerationLimiter.
+   *
+   * @param system The differential drive dynamics.
+   * @param trackwidth The distance between the differential drive's left and right wheels in
+   *     meters.
+   * @param minLinearAccel The minimum (most negative) linear acceleration in meters per second
+   *     squared.
+   * @param maxLinearAccel The maximum (most positive) linear acceleration in meters per second
+   *     squared.
+   * @param maxAngularAccel The maximum angular acceleration in radians per second squared.
+   * @throws IllegalArgumentException if minimum linear acceleration is greater than maximum linear
+   *     acceleration
+   */
+  public DifferentialDriveAccelerationLimiter(
+      LinearSystem<N2, N2, N2> system,
+      double trackwidth,
+      double minLinearAccel,
+      double maxLinearAccel,
+      double maxAngularAccel) {
+    if (minLinearAccel > maxLinearAccel) {
+      throw new IllegalArgumentException("maxLinearAccel must be greater than minLinearAccel");
+    }
+    m_system = system;
+    m_trackwidth = trackwidth;
+    m_minLinearAccel = minLinearAccel;
+    m_maxLinearAccel = maxLinearAccel;
+    m_maxAngularAccel = maxAngularAccel;
+  }
+
+  /**
+   * Returns the next voltage pair subject to acceleration constraints.
+   *
+   * @param leftVelocity The left wheel velocity in meters per second.
+   * @param rightVelocity The right wheel velocity in meters per second.
+   * @param leftVoltage The unconstrained left motor voltage.
+   * @param rightVoltage The unconstrained right motor voltage.
+   * @return The constrained wheel voltages.
+   */
+  public DifferentialDriveWheelVoltages calculate(
+      double leftVelocity, double rightVelocity, double leftVoltage, double rightVoltage) {
+    Matrix<N2, N1> u = VecBuilder.fill(leftVoltage, rightVoltage);
+
+    // Find unconstrained wheel accelerations
+    var x = VecBuilder.fill(leftVelocity, rightVelocity);
+    var dxdt = m_system.getA().times(x).plus(m_system.getB().times(u));
+
+    // Convert from wheel accelerations to linear and angular accelerations
+    //
+    // a = (dxdt(0) + dx/dt(1)) / 2
+    //   = 0.5 dxdt(0) + 0.5 dxdt(1)
+    //
+    // α = (dxdt(1) - dxdt(0)) / trackwidth
+    //   = -1/trackwidth dxdt(0) + 1/trackwidth dxdt(1)
+    //
+    // [a] = [          0.5           0.5][dxdt(0)]
+    // [α]   [-1/trackwidth  1/trackwidth][dxdt(1)]
+    //
+    // accels = M dxdt where M = [0.5, 0.5; -1/trackwidth, 1/trackwidth]
+    var M = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.5, 0.5, -1.0 / m_trackwidth, 1.0 / m_trackwidth);
+    var accels = M.times(dxdt);
+
+    // Constrain the linear and angular accelerations
+    if (accels.get(0, 0) > m_maxLinearAccel) {
+      accels.set(0, 0, m_maxLinearAccel);
+    } else if (accels.get(0, 0) < m_minLinearAccel) {
+      accels.set(0, 0, m_minLinearAccel);
+    }
+    if (accels.get(1, 0) > m_maxAngularAccel) {
+      accels.set(1, 0, m_maxAngularAccel);
+    } else if (accels.get(1, 0) < -m_maxAngularAccel) {
+      accels.set(1, 0, -m_maxAngularAccel);
+    }
+
+    // Convert the constrained linear and angular accelerations back to wheel
+    // accelerations
+    dxdt = M.solve(accels);
+
+    // Find voltages for the given wheel accelerations
+    //
+    // dx/dt = Ax + Bu
+    // u = B⁻¹(dx/dt - Ax)
+    u = m_system.getB().solve(dxdt.minus(m_system.getA().times(x)));
+
+    return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0));
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/DifferentialDriveFeedforward.java

@@ -0,0 +1,97 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.proto.DifferentialDriveFeedforwardProto;
+import edu.wpi.first.math.controller.struct.DifferentialDriveFeedforwardStruct;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+
+/** A helper class which computes the feedforward outputs for a differential drive drivetrain. */
+public class DifferentialDriveFeedforward implements ProtobufSerializable, StructSerializable {
+  private final LinearSystem<N2, N2, N2> m_plant;
+
+  /** The linear velocity gain in volts per (meters per second). */
+  public final double m_kVLinear;
+
+  /** The linear acceleration gain in volts per (meters per second squared). */
+  public final double m_kALinear;
+
+  /** The angular velocity gain in volts per (radians per second). */
+  public final double m_kVAngular;
+
+  /** The angular acceleration gain in volts per (radians per second squared). */
+  public final double m_kAAngular;
+
+  /**
+   * Creates a new DifferentialDriveFeedforward with the specified parameters.
+   *
+   * @param kVLinear The linear velocity gain in volts per (meters per second).
+   * @param kALinear The linear acceleration gain in volts per (meters per second squared).
+   * @param kVAngular The angular velocity gain in volts per (radians per second).
+   * @param kAAngular The angular acceleration gain in volts per (radians per second squared).
+   * @param trackwidth The distance between the differential drive's left and right wheels, in
+   *     meters.
+   */
+  public DifferentialDriveFeedforward(
+      double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
+    // See LinearSystemId.identifyDrivetrainSystem(double, double, double, double, double)
+    this(kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth);
+  }
+
+  /**
+   * Creates a new DifferentialDriveFeedforward with the specified parameters.
+   *
+   * @param kVLinear The linear velocity gain in volts per (meters per second).
+   * @param kALinear The linear acceleration gain in volts per (meters per second squared).
+   * @param kVAngular The angular velocity gain in volts per (meters per second).
+   * @param kAAngular The angular acceleration gain in volts per (meters per second squared).
+   */
+  public DifferentialDriveFeedforward(
+      double kVLinear, double kALinear, double kVAngular, double kAAngular) {
+    m_plant = LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular);
+    m_kVLinear = kVLinear;
+    m_kALinear = kALinear;
+    m_kVAngular = kVAngular;
+    m_kAAngular = kAAngular;
+  }
+
+  /**
+   * Calculates the differential drive feedforward inputs given velocity setpoints.
+   *
+   * @param currentLeftVelocity The current left velocity of the differential drive in
+   *     meters/second.
+   * @param nextLeftVelocity The next left velocity of the differential drive in meters/second.
+   * @param currentRightVelocity The current right velocity of the differential drive in
+   *     meters/second.
+   * @param nextRightVelocity The next right velocity of the differential drive in meters/second.
+   * @param dt Discretization timestep in seconds.
+   * @return A DifferentialDriveWheelVoltages object containing the computed feedforward voltages.
+   */
+  public DifferentialDriveWheelVoltages calculate(
+      double currentLeftVelocity,
+      double nextLeftVelocity,
+      double currentRightVelocity,
+      double nextRightVelocity,
+      double dt) {
+    var feedforward = new LinearPlantInversionFeedforward<>(m_plant, dt);
+    var r = VecBuilder.fill(currentLeftVelocity, currentRightVelocity);
+    var nextR = VecBuilder.fill(nextLeftVelocity, nextRightVelocity);
+    var u = feedforward.calculate(r, nextR);
+    return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0));
+  }
+
+  /** DifferentialDriveFeedforward struct for serialization. */
+  public static final DifferentialDriveFeedforwardStruct struct =
+      new DifferentialDriveFeedforwardStruct();
+
+  /** DifferentialDriveFeedforward protobuf for serialization. */
+  public static final DifferentialDriveFeedforwardProto proto =
+      new DifferentialDriveFeedforwardProto();
+}

wpimath/src/main/java/org/wpilib/math/controller/DifferentialDriveWheelVoltages.java

@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.controller.proto.DifferentialDriveWheelVoltagesProto;
+import edu.wpi.first.math.controller.struct.DifferentialDriveWheelVoltagesStruct;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+
+/** Motor voltages for a differential drive. */
+public class DifferentialDriveWheelVoltages implements ProtobufSerializable, StructSerializable {
+  /** Left wheel voltage. */
+  public double left;
+
+  /** Right wheel voltage. */
+  public double right;
+
+  /** DifferentialDriveWheelVoltages protobuf for serialization. */
+  public static final DifferentialDriveWheelVoltagesProto proto =
+      new DifferentialDriveWheelVoltagesProto();
+
+  /** DifferentialDriveWheelVoltages struct for serialization. */
+  public static final DifferentialDriveWheelVoltagesStruct struct =
+      new DifferentialDriveWheelVoltagesStruct();
+
+  /** Default constructor. */
+  public DifferentialDriveWheelVoltages() {}
+
+  /**
+   * Constructs a DifferentialDriveWheelVoltages.
+   *
+   * @param left Left wheel voltage.
+   * @param right Right wheel voltage.
+   */
+  public DifferentialDriveWheelVoltages(double left, double right) {
+    this.left = left;
+    this.right = right;
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/ElevatorFeedforward.java

@@ -0,0 +1,270 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.controller.proto.ElevatorFeedforwardProto;
+import edu.wpi.first.math.controller.struct.ElevatorFeedforwardStruct;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+
+/**
+ * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting
+ * against the force of gravity).
+ */
+public class ElevatorFeedforward implements ProtobufSerializable, StructSerializable {
+  /** The static gain, in volts. */
+  private double ks;
+
+  /** The gravity gain, in volts. */
+  private double kg;
+
+  /** The velocity gain, in V/(m/s). */
+  private double kv;
+
+  /** The acceleration gain, in V/(m/s²). */
+  private double ka;
+
+  /** The period, in seconds. */
+  private final double m_dt;
+
+  /**
+   * Creates a new ElevatorFeedforward with the specified gains and period.
+   *
+   * @param ks The static gain in volts.
+   * @param kg The gravity gain in volts.
+   * @param kv The velocity gain in V/(m/s).
+   * @param ka The acceleration gain in V/(m/s²).
+   * @param dt The period in seconds.
+   * @throws IllegalArgumentException for kv < zero.
+   * @throws IllegalArgumentException for ka < zero.
+   * @throws IllegalArgumentException for period ≤ zero.
+   */
+  public ElevatorFeedforward(double ks, double kg, double kv, double ka, double dt) {
+    this.ks = ks;
+    this.kg = kg;
+    this.kv = kv;
+    this.ka = ka;
+    if (kv < 0.0) {
+      throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!");
+    }
+    if (ka < 0.0) {
+      throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
+    }
+    if (dt <= 0.0) {
+      throw new IllegalArgumentException("period must be a positive number, got " + dt + "!");
+    }
+    m_dt = dt;
+  }
+
+  /**
+   * Creates a new ElevatorFeedforward with the specified gains. The period is defaulted to 20 ms.
+   *
+   * @param ks The static gain in volts.
+   * @param kg The gravity gain in volts.
+   * @param kv The velocity gain in V/(m/s).
+   * @param ka The acceleration gain in V/(m/s²).
+   * @throws IllegalArgumentException for kv &lt; zero.
+   * @throws IllegalArgumentException for ka &lt; zero.
+   */
+  public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
+    this(ks, kg, kv, ka, 0.020);
+  }
+
+  /**
+   * Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is defaulted to
+   * zero. The period is defaulted to 20 ms.
+   *
+   * @param ks The static gain in volts.
+   * @param kg The gravity gain in volts.
+   * @param kv The velocity gain in V/(m/s).
+   * @throws IllegalArgumentException for kv &lt; zero.
+   */
+  public ElevatorFeedforward(double ks, double kg, double kv) {
+    this(ks, kg, kv, 0);
+  }
+
+  /**
+   * Sets the static gain.
+   *
+   * @param ks The static gain in volts.
+   */
+  public void setKs(double ks) {
+    this.ks = ks;
+  }
+
+  /**
+   * Sets the gravity gain.
+   *
+   * @param kg The gravity gain in volts.
+   */
+  public void setKg(double kg) {
+    this.kg = kg;
+  }
+
+  /**
+   * Sets the velocity gain.
+   *
+   * @param kv The velocity gain in V/(m/s).
+   */
+  public void setKv(double kv) {
+    this.kv = kv;
+  }
+
+  /**
+   * Sets the acceleration gain.
+   *
+   * @param ka The acceleration gain in V/(m/s²).
+   */
+  public void setKa(double ka) {
+    this.ka = ka;
+  }
+
+  /**
+   * Returns the static gain in volts.
+   *
+   * @return The static gain in volts.
+   */
+  public double getKs() {
+    return ks;
+  }
+
+  /**
+   * Returns the gravity gain in volts.
+   *
+   * @return The gravity gain in volts.
+   */
+  public double getKg() {
+    return kg;
+  }
+
+  /**
+   * Returns the velocity gain in V/(m/s).
+   *
+   * @return The velocity gain.
+   */
+  public double getKv() {
+    return kv;
+  }
+
+  /**
+   * Returns the acceleration gain in V/(m/s²).
+   *
+   * @return The acceleration gain.
+   */
+  public double getKa() {
+    return ka;
+  }
+
+  /**
+   * Returns the period in seconds.
+   *
+   * @return The period in seconds.
+   */
+  public double getDt() {
+    return m_dt;
+  }
+
+  /**
+   * Calculates the feedforward from the gains and velocity setpoint assuming continuous control
+   * (acceleration is assumed to be zero).
+   *
+   * @param velocity The velocity setpoint in meters per second.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity) {
+    return calculate(velocity, velocity);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints assuming discrete control.
+   *
+   * <p>Note this method is inaccurate when the velocity crosses 0.
+   *
+   * @param currentVelocity The current velocity setpoint in meters per second.
+   * @param nextVelocity The next velocity setpoint in meters per second.
+   * @return The computed feedforward.
+   */
+  public double calculate(double currentVelocity, double nextVelocity) {
+    // See wpimath/algorithms.md#Elevator_feedforward for derivation
+    if (ka < 1e-9) {
+      return ks * Math.signum(nextVelocity) + kg + kv * nextVelocity;
+    } else {
+      double A = -kv / ka;
+      double B = 1.0 / ka;
+      double A_d = Math.exp(A * m_dt);
+      double B_d = A > -1e-9 ? B * m_dt : 1.0 / A * (A_d - 1.0) * B;
+      return kg
+          + ks * Math.signum(currentVelocity)
+          + 1.0 / B_d * (nextVelocity - A_d * currentVelocity);
+    }
+  }
+
+  // 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, in volts.
+   * @param acceleration The acceleration of the elevator, in (m/s²).
+   * @return The maximum possible velocity in (m/s) 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, in volts.
+   * @param acceleration The acceleration of the elevator, in (m/s²).
+   * @return The maximum possible velocity in (m/s) 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, in volts.
+   * @param velocity The velocity of the elevator, in (m/s)
+   * @return The maximum possible acceleration in (m/s²) 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, in volts.
+   * @param velocity The velocity of the elevator, in (m/s)
+   * @return The maximum possible acceleration in (m/s²) at the given velocity.
+   */
+  public double minAchievableAcceleration(double maxVoltage, double velocity) {
+    return maxAchievableAcceleration(-maxVoltage, velocity);
+  }
+
+  /** ElevatorFeedforward struct for serialization. */
+  public static final ElevatorFeedforwardStruct struct = new ElevatorFeedforwardStruct();
+
+  /** ElevatorFeedforward protobuf for serialization. */
+  public static final ElevatorFeedforwardProto proto = new ElevatorFeedforwardProto();
+}

wpimath/src/main/java/org/wpilib/math/controller/ImplicitModelFollower.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;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.LinearSystem;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * Contains the controller coefficients and logic for an implicit model follower.
+ *
+ * <p>Implicit model following lets us design a feedback controller that erases the dynamics of our
+ * system and makes it behave like some other system. This can be used to make a drivetrain more
+ * controllable during teleop driving by making it behave like a slower or more benign drivetrain.
+ *
+ * <p>For more on the underlying math, read appendix B.3 in <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
+ */
+public class ImplicitModelFollower<States extends Num, Inputs extends Num, Outputs extends Num> {
+  // Computed controller output
+  private Matrix<Inputs, N1> m_u;
+
+  // State space conversion gain
+  private Matrix<Inputs, States> m_A;
+
+  // Input space conversion gain
+  private Matrix<Inputs, Inputs> m_B;
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param plant The plant being controlled.
+   * @param plantRef The plant whose dynamics should be followed.
+   */
+  public ImplicitModelFollower(
+      LinearSystem<States, Inputs, Outputs> plant, LinearSystem<States, Inputs, Outputs> plantRef) {
+    this(plant.getA(), plant.getB(), plantRef.getA(), plantRef.getB());
+  }
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param A Continuous system matrix of the plant being controlled.
+   * @param B Continuous input matrix of the plant being controlled.
+   * @param Aref Continuous system matrix whose dynamics should be followed.
+   * @param Bref Continuous input matrix whose dynamics should be followed.
+   */
+  public ImplicitModelFollower(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Aref,
+      Matrix<States, Inputs> Bref) {
+    m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
+
+    // Find u_imf that makes real model match reference model.
+    //
+    // dx/dt = Ax + Bu_imf
+    // dz/dt = A_ref z + B_ref u
+    //
+    // Let x = z.
+    //
+    // dx/dt = dz/dt
+    // Ax + Bu_imf = Aref x + B_ref u
+    // Bu_imf = A_ref x - Ax + B_ref u
+    // Bu_imf = (A_ref - A)x + B_ref u
+    // u_imf = B⁻¹((A_ref - A)x + Bref u)
+    // u_imf = -B⁻¹(A - A_ref)x + B⁻¹B_ref u
+
+    // The first term makes the open-loop poles that of the reference
+    // system, and the second term makes the input behave like that of the
+    // reference system.
+    m_A = B.solve(A.minus(Aref)).times(-1.0);
+    m_B = B.solve(Bref);
+
+    reset();
+  }
+
+  /**
+   * Returns the control input vector u.
+   *
+   * @return The control input.
+   */
+  public Matrix<Inputs, N1> getU() {
+    return m_u;
+  }
+
+  /**
+   * Returns an element of the control input vector u.
+   *
+   * @param i Row of u.
+   * @return The row of the control input vector.
+   */
+  public double getU(int i) {
+    return m_u.get(i, 0);
+  }
+
+  /** Resets the controller. */
+  public final void reset() {
+    m_u.fill(0.0);
+  }
+
+  /**
+   * Returns the next output of the controller.
+   *
+   * @param x The current state x.
+   * @param u The current input for the original model.
+   * @return The next controller output.
+   */
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> x, Matrix<Inputs, N1> u) {
+    m_u = m_A.times(x).plus(m_B.times(u));
+    return m_u;
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/LTVDifferentialDriveController.java

@@ -0,0 +1,249 @@
+// 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.DARE;
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.system.Discretization;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.trajectory.Trajectory;
+
+/**
+ * The linear time-varying differential drive controller has a similar form to the LQR, but the
+ * model used to compute the controller gain is the nonlinear differential drive model linearized
+ * around the drivetrain's current state. We precompute gains for important places in our
+ * state-space, then interpolate between them with a lookup table to save computational resources.
+ *
+ * <p>This controller has a flat hierarchy with pose and wheel velocity references and voltage
+ * outputs. This is different from a unicycle controller's nested hierarchy where the top-level
+ * controller has a pose reference and chassis velocity command outputs, and the low-level
+ * controller has wheel velocity references and voltage outputs. Flat hierarchies are easier to tune
+ * in one shot.
+ *
+ * <p>See section 8.7 in Controls Engineering in FRC for a derivation of the control law we used
+ * shown in theorem 8.7.4.
+ */
+public class LTVDifferentialDriveController {
+  private final double m_trackwidth;
+
+  // Continuous velocity dynamics
+  private final Matrix<N2, N2> m_A;
+  private final Matrix<N2, N2> m_B;
+
+  // LQR cost matrices
+  private final Matrix<N5, N5> m_Q;
+  private final Matrix<N2, N2> m_R;
+
+  private final double m_dt;
+
+  private Matrix<N5, N1> m_error = new Matrix<>(Nat.N5(), Nat.N1());
+  private Matrix<N5, N1> m_tolerance = new Matrix<>(Nat.N5(), Nat.N1());
+
+  /**
+   * Constructs a linear time-varying differential drive controller.
+   *
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning</a>
+   * for how to select the tolerances.
+   *
+   * @param plant The differential drive velocity plant.
+   * @param trackwidth The distance between the differential drive's left and right wheels in
+   *     meters.
+   * @param qelems The maximum desired error tolerance for each state.
+   * @param relems The maximum desired control effort for each input.
+   * @param dt Discretization timestep in seconds.
+   */
+  public LTVDifferentialDriveController(
+      LinearSystem<N2, N2, N2> plant,
+      double trackwidth,
+      Vector<N5> qelems,
+      Vector<N2> relems,
+      double dt) {
+    m_trackwidth = trackwidth;
+    m_A = plant.getA();
+    m_B = plant.getB();
+    m_Q = StateSpaceUtil.makeCostMatrix(qelems);
+    m_R = StateSpaceUtil.makeCostMatrix(relems);
+    m_dt = dt;
+  }
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   *
+   * @return True if the pose error is within tolerance of the reference.
+   */
+  public boolean atReference() {
+    return Math.abs(m_error.get(0, 0)) < m_tolerance.get(0, 0)
+        && Math.abs(m_error.get(1, 0)) < m_tolerance.get(1, 0)
+        && Math.abs(m_error.get(2, 0)) < m_tolerance.get(2, 0)
+        && Math.abs(m_error.get(3, 0)) < m_tolerance.get(3, 0)
+        && Math.abs(m_error.get(4, 0)) < m_tolerance.get(4, 0);
+  }
+
+  /**
+   * Sets the pose error which is considered tolerable for use with atReference().
+   *
+   * @param poseTolerance Pose error which is tolerable.
+   * @param leftVelocityTolerance Left velocity error which is tolerable in meters per second.
+   * @param rightVelocityTolerance Right velocity error which is tolerable in meters per second.
+   */
+  public void setTolerance(
+      Pose2d poseTolerance, double leftVelocityTolerance, double rightVelocityTolerance) {
+    m_tolerance =
+        VecBuilder.fill(
+            poseTolerance.getX(),
+            poseTolerance.getY(),
+            poseTolerance.getRotation().getRadians(),
+            leftVelocityTolerance,
+            rightVelocityTolerance);
+  }
+
+  /**
+   * Returns the left and right output voltages of the LTV controller.
+   *
+   * <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
+   * trajectory.
+   *
+   * @param currentPose The current pose.
+   * @param leftVelocity The current left velocity in meters per second.
+   * @param rightVelocity The current right velocity in meters per second.
+   * @param poseRef The desired pose.
+   * @param leftVelocityRef The desired left velocity in meters per second.
+   * @param rightVelocityRef The desired right velocity in meters per second.
+   * @return Left and right output voltages of the LTV controller.
+   */
+  public DifferentialDriveWheelVoltages calculate(
+      Pose2d currentPose,
+      double leftVelocity,
+      double rightVelocity,
+      Pose2d poseRef,
+      double leftVelocityRef,
+      double rightVelocityRef) {
+    // This implements the linear time-varying differential drive controller in
+    // theorem 8.7.4 of https://controls-in-frc.link/
+    //
+    //     [x ]
+    //     [y ]       [Vₗ]
+    // x = [θ ]   u = [Vᵣ]
+    //     [vₗ]
+    //     [vᵣ]
+
+    double velocity = (leftVelocity + rightVelocity) / 2.0;
+
+    // The DARE is ill-conditioned if the velocity is close to zero, so don't
+    // let the system stop.
+    if (Math.abs(velocity) < 1e-4) {
+      velocity = 1e-4;
+    }
+
+    var r =
+        VecBuilder.fill(
+            poseRef.getX(),
+            poseRef.getY(),
+            poseRef.getRotation().getRadians(),
+            leftVelocityRef,
+            rightVelocityRef);
+    var x =
+        VecBuilder.fill(
+            currentPose.getX(),
+            currentPose.getY(),
+            currentPose.getRotation().getRadians(),
+            leftVelocity,
+            rightVelocity);
+
+    m_error = r.minus(x);
+    m_error.set(2, 0, MathUtil.angleModulus(m_error.get(2, 0)));
+
+    // spotless:off
+    var A = MatBuilder.fill(Nat.N5(), Nat.N5(),
+        0.0, 0.0, 0.0, 0.5, 0.5,
+        0.0, 0.0, velocity, 0.0, 0.0,
+        0.0, 0.0, 0.0, -1.0 / m_trackwidth, 1.0 / m_trackwidth,
+        0.0, 0.0, 0.0, m_A.get(0, 0), m_A.get(0, 1),
+        0.0, 0.0, 0.0, m_A.get(1, 0), m_A.get(1, 1));
+    var B = MatBuilder.fill(Nat.N5(), Nat.N2(),
+        0.0, 0.0,
+        0.0, 0.0,
+        0.0, 0.0,
+        m_B.get(0, 0), m_B.get(0, 1),
+        m_B.get(1, 0), m_B.get(1, 1));
+    // spotless:on
+
+    var discABPair = Discretization.discretizeAB(A, B, m_dt);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    var S = DARE.dareNoPrecond(discA, discB, m_Q, m_R);
+
+    // K = (BᵀSB + R)⁻¹BᵀSA
+    var K =
+        discB
+            .transpose()
+            .times(S)
+            .times(discB)
+            .plus(m_R)
+            .solve(discB.transpose().times(S).times(discA));
+
+    // spotless:off
+    var inRobotFrame = MatBuilder.fill(Nat.N5(), Nat.N5(),
+        Math.cos(x.get(2, 0)), Math.sin(x.get(2, 0)), 0.0, 0.0, 0.0,
+        -Math.sin(x.get(2, 0)), Math.cos(x.get(2, 0)), 0.0, 0.0, 0.0,
+        0.0, 0.0, 1.0, 0.0, 0.0,
+        0.0, 0.0, 0.0, 1.0, 0.0,
+        0.0, 0.0, 0.0, 0.0, 1.0);
+    // spotless:on
+
+    var u = K.times(inRobotFrame).times(m_error);
+
+    return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0));
+  }
+
+  /**
+   * Returns the left and right output voltages of the LTV controller.
+   *
+   * <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
+   * trajectory.
+   *
+   * @param currentPose The current pose.
+   * @param leftVelocity The left velocity in meters per second.
+   * @param rightVelocity The right velocity in meters per second.
+   * @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory.
+   * @return The left and right output voltages of the LTV controller.
+   */
+  public DifferentialDriveWheelVoltages calculate(
+      Pose2d currentPose,
+      double leftVelocity,
+      double rightVelocity,
+      Trajectory.State desiredState) {
+    // v = (v_r + v_l) / 2     (1)
+    // w = (v_r - v_l) / (2r)  (2)
+    // k = w / v               (3)
+    //
+    // v_l = v - wr
+    // v_l = v - (vk)r
+    // v_l = v(1 - kr)
+    //
+    // v_r = v + wr
+    // v_r = v + (vk)r
+    // v_r = v(1 + kr)
+    return calculate(
+        currentPose,
+        leftVelocity,
+        rightVelocity,
+        desiredState.pose,
+        desiredState.velocity * (1 - (desiredState.curvature * m_trackwidth / 2.0)),
+        desiredState.velocity * (1 + (desiredState.curvature * m_trackwidth / 2.0)));
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/LTVUnicycleController.java

@@ -0,0 +1,221 @@
+// 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.DARE;
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.system.Discretization;
+import edu.wpi.first.math.trajectory.Trajectory;
+
+/**
+ * The linear time-varying unicycle controller has a similar form to the LQR, but the model used to
+ * compute the controller gain is the nonlinear unicycle model linearized around the drivetrain's
+ * current state.
+ *
+ * <p>See section 8.9 in Controls Engineering in FRC for a derivation of the control law we used
+ * shown in theorem 8.9.1.
+ */
+public class LTVUnicycleController {
+  // LQR cost matrices
+  private Matrix<N3, N3> m_Q;
+  private Matrix<N2, N2> m_R;
+
+  private final double m_dt;
+
+  private Pose2d m_poseError;
+  private Pose2d m_poseTolerance;
+  private boolean m_enabled = true;
+
+  /**
+   * Constructs a linear time-varying unicycle controller with default maximum desired error
+   * tolerances of (x = 0.0625 m, y = 0.125 m, heading = 2 rad), default maximum desired control
+   * effort of (linear velocity = 1 m/s, angular velocity = 2 rad/s), and default maximum Velocity
+   * of 9 m/s.
+   *
+   * @param dt Discretization timestep in seconds.
+   */
+  public LTVUnicycleController(double dt) {
+    this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt);
+  }
+
+  /**
+   * Constructs a linear time-varying unicycle controller.
+   *
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning</a>
+   * for how to select the tolerances.
+   *
+   * <p>The default maximum Velocity is 9 m/s.
+   *
+   * @param qelems The maximum desired error tolerance for each state (x, y, heading).
+   * @param relems The maximum desired control effort for each input (linear velocity, angular
+   *     velocity).
+   * @param dt Discretization timestep in seconds.
+   */
+  public LTVUnicycleController(Vector<N3> qelems, Vector<N2> relems, double dt) {
+    m_Q = StateSpaceUtil.makeCostMatrix(qelems);
+    m_R = StateSpaceUtil.makeCostMatrix(relems);
+    m_dt = dt;
+  }
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   *
+   * @return True if the pose error is within tolerance of the reference.
+   */
+  public boolean atReference() {
+    final var eTranslate = m_poseError.getTranslation();
+    final var eRotate = m_poseError.getRotation();
+    final var tolTranslate = m_poseTolerance.getTranslation();
+    final var tolRotate = m_poseTolerance.getRotation();
+    return Math.abs(eTranslate.getX()) < tolTranslate.getX()
+        && Math.abs(eTranslate.getY()) < tolTranslate.getY()
+        && Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
+  }
+
+  /**
+   * Sets the pose error which is considered tolerable for use with atReference().
+   *
+   * @param poseTolerance Pose error which is tolerable.
+   */
+  public void setTolerance(Pose2d poseTolerance) {
+    m_poseTolerance = poseTolerance;
+  }
+
+  /**
+   * Returns the linear and angular velocity outputs of the LTV controller.
+   *
+   * <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
+   * trajectory.
+   *
+   * @param currentPose The current pose.
+   * @param poseRef The desired pose.
+   * @param linearVelocityRef The desired linear velocity in meters per second.
+   * @param angularVelocityRef The desired angular velocity in radians per second.
+   * @return The linear and angular velocity outputs of the LTV controller.
+   */
+  public ChassisSpeeds calculate(
+      Pose2d currentPose, Pose2d poseRef, double linearVelocityRef, double angularVelocityRef) {
+    // The change in global pose for a unicycle is defined by the following
+    // three equations.
+    //
+    // ẋ = v cosθ
+    // ẏ = v sinθ
+    // θ̇ = ω
+    //
+    // Here's the model as a vector function where x = [x  y  θ]ᵀ and
+    // u = [v  ω]ᵀ.
+    //
+    //           [v cosθ]
+    // f(x, u) = [v sinθ]
+    //           [  ω   ]
+    //
+    // To create an LQR, we need to linearize this.
+    //
+    //               [0  0  −v sinθ]                  [cosθ  0]
+    // ∂f(x, u)/∂x = [0  0   v cosθ]    ∂f(x, u)/∂u = [sinθ  0]
+    //               [0  0     0   ]                  [ 0    1]
+    //
+    // We're going to make a cross-track error controller, so we'll apply a
+    // clockwise rotation matrix to the global tracking error to transform it
+    // into the robot's coordinate frame. Since the cross-track error is always
+    // measured from the robot's coordinate frame, the model used to compute the
+    // LQR should be linearized around θ = 0 at all times.
+    //
+    //     [0  0  −v sin0]        [cos0  0]
+    // A = [0  0   v cos0]    B = [sin0  0]
+    //     [0  0     0   ]        [ 0    1]
+    //
+    //     [0  0  0]              [1  0]
+    // A = [0  0  v]          B = [0  0]
+    //     [0  0  0]              [0  1]
+
+    if (!m_enabled) {
+      return new ChassisSpeeds(linearVelocityRef, 0.0, angularVelocityRef);
+    }
+
+    // The DARE is ill-conditioned if the velocity is close to zero, so don't
+    // let the system stop.
+    if (Math.abs(linearVelocityRef) < 1e-4) {
+      linearVelocityRef = 1e-4;
+    }
+
+    m_poseError = poseRef.relativeTo(currentPose);
+
+    // spotless:off
+    var A = MatBuilder.fill(Nat.N3(), Nat.N3(),
+        0.0, 0.0, 0.0,
+        0.0, 0.0, linearVelocityRef,
+        0.0, 0.0, 0.0);
+    var B = MatBuilder.fill(Nat.N3(), Nat.N2(),
+        1.0, 0.0,
+        0.0, 0.0,
+        0.0, 1.0);
+    // spotless:on
+
+    var discABPair = Discretization.discretizeAB(A, B, m_dt);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    var S = DARE.dareNoPrecond(discA, discB, m_Q, m_R);
+
+    // K = (BᵀSB + R)⁻¹BᵀSA
+    var K =
+        discB
+            .transpose()
+            .times(S)
+            .times(discB)
+            .plus(m_R)
+            .solve(discB.transpose().times(S).times(discA));
+
+    var e =
+        MatBuilder.fill(
+            Nat.N3(),
+            Nat.N1(),
+            m_poseError.getX(),
+            m_poseError.getY(),
+            m_poseError.getRotation().getRadians());
+    var u = K.times(e);
+
+    return new ChassisSpeeds(
+        linearVelocityRef + u.get(0, 0), 0.0, angularVelocityRef + u.get(1, 0));
+  }
+
+  /**
+   * Returns the next output of the LTV controller.
+   *
+   * <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
+   * trajectory.
+   *
+   * @param currentPose The current pose.
+   * @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory.
+   * @return The linear and angular velocity outputs of the LTV controller.
+   */
+  public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
+    return calculate(
+        currentPose,
+        desiredState.pose,
+        desiredState.velocity,
+        desiredState.velocity * desiredState.curvature);
+  }
+
+  /**
+   * 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;
+  }
+}

wpimath/src/main/java/org/wpilib/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.numbers.N1;
+import edu.wpi.first.math.system.Discretization;
+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 <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
+ */
+public class LinearPlantInversionFeedforward<
+    States extends Num, Inputs extends Num, Outputs extends Num> {
+  /** The current reference state. */
+  private Matrix<States, N1> m_r;
+
+  /** The computed feedforward. */
+  private Matrix<Inputs, N1> m_uff;
+
+  private final Matrix<States, Inputs> m_B;
+
+  private final Matrix<States, States> m_A;
+
+  /**
+   * Constructs a feedforward with the given plant.
+   *
+   * @param plant The plant being controlled.
+   * @param dt Discretization timestep in seconds.
+   */
+  public LinearPlantInversionFeedforward(LinearSystem<States, Inputs, Outputs> plant, double dt) {
+    this(plant.getA(), plant.getB(), dt);
+  }
+
+  /**
+   * 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 dt Discretization timestep in seconds.
+   */
+  public LinearPlantInversionFeedforward(
+      Matrix<States, States> A, Matrix<States, Inputs> B, double dt) {
+    var discABPair = Discretization.discretizeAB(A, B, dt);
+    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();
+  }
+
+  /**
+   * 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 final void reset(Matrix<States, N1> initialState) {
+    m_r = initialState;
+    m_uff.fill(0.0);
+  }
+
+  /** Resets the feedforward with a zero initial state vector. */
+  public final 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.
+   */
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
+    // rₖ₊₁ = Arₖ + Buₖ
+    // Buₖ = rₖ₊₁ − Arₖ
+    // uₖ = B⁺(rₖ₊₁ − Arₖ)
+    m_uff = new Matrix<>(m_B.solve(nextR.minus(m_A.times(r))));
+
+    m_r = nextR;
+    return m_uff;
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/LinearQuadraticRegulator.java

@@ -0,0 +1,262 @@
+// 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.DARE;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.Discretization;
+import edu.wpi.first.math.system.LinearSystem;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * 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 <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
+ */
+public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Outputs extends Num> {
+  /** The current reference state. */
+  private Matrix<States, N1> m_r;
+
+  /** The computed and capped controller output. */
+  private Matrix<Inputs, N1> m_u;
+
+  // Controller gain.
+  private Matrix<Inputs, States> m_K;
+
+  /**
+   * Constructs a controller with the given coefficients and plant. Rho is defaulted to 1.
+   *
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning</a>
+   * for how to select the tolerances.
+   *
+   * @param plant The plant being controlled.
+   * @param qelms The maximum desired error tolerance for each state.
+   * @param relms The maximum desired control effort for each input.
+   * @param dt Discretization timestep in seconds.
+   * @throws IllegalArgumentException If the system is unstabilizable.
+   */
+  public LinearQuadraticRegulator(
+      LinearSystem<States, Inputs, Outputs> plant,
+      Vector<States> qelms,
+      Vector<Inputs> relms,
+      double dt) {
+    this(
+        plant.getA(),
+        plant.getB(),
+        StateSpaceUtil.makeCostMatrix(qelms),
+        StateSpaceUtil.makeCostMatrix(relms),
+        dt);
+  }
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning</a>
+   * for how to select the tolerances.
+   *
+   * @param A Continuous system matrix of the plant being controlled.
+   * @param B Continuous input matrix of the plant being controlled.
+   * @param qelms The maximum desired error tolerance for each state.
+   * @param relms The maximum desired control effort for each input.
+   * @param dt Discretization timestep in seconds.
+   * @throws IllegalArgumentException If the system is unstabilizable.
+   */
+  public LinearQuadraticRegulator(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Vector<States> qelms,
+      Vector<Inputs> relms,
+      double dt) {
+    this(A, B, StateSpaceUtil.makeCostMatrix(qelms), StateSpaceUtil.makeCostMatrix(relms), dt);
+  }
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param A Continuous system matrix of the plant being controlled.
+   * @param B Continuous input matrix of the plant being controlled.
+   * @param Q The state cost matrix.
+   * @param R The input cost matrix.
+   * @param dt Discretization timestep in seconds.
+   * @throws IllegalArgumentException If the system is unstabilizable.
+   */
+  public LinearQuadraticRegulator(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R,
+      double dt) {
+    var discABPair = Discretization.discretizeAB(A, B, dt);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    var S = DARE.dare(discA, discB, Q, R);
+
+    // K = (BᵀSB + R)⁻¹BᵀSA
+    m_K =
+        discB
+            .transpose()
+            .times(S)
+            .times(discB)
+            .plus(R)
+            .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 A Continuous system matrix of the plant being controlled.
+   * @param B Continuous input matrix of the plant being controlled.
+   * @param Q The state cost matrix.
+   * @param R The input cost matrix.
+   * @param N The state-input cross-term cost matrix.
+   * @param dt Discretization timestep in seconds.
+   * @throws IllegalArgumentException If the system is unstabilizable.
+   */
+  public LinearQuadraticRegulator(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R,
+      Matrix<States, Inputs> N,
+      double dt) {
+    var discABPair = Discretization.discretizeAB(A, B, dt);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    var S = DARE.dare(discA, discB, Q, R, N);
+
+    // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
+    m_K =
+        discB
+            .transpose()
+            .times(S)
+            .times(discB)
+            .plus(R)
+            .solve(discB.transpose().times(S).times(discA).plus(N.transpose()));
+
+    m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
+    m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
+
+    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 final 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.
+   * @return The next controller output.
+   */
+  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.
+   * @return The next controller output.
+   */
+  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 <a
+   * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
+   * appendix C.4 for a derivation.
+   *
+   * @param plant The plant being controlled.
+   * @param dt Discretization timestep in seconds.
+   * @param inputDelay Input time delay in seconds.
+   */
+  public void latencyCompensate(
+      LinearSystem<States, Inputs, Outputs> plant, double dt, double inputDelay) {
+    var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dt);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    m_K = m_K.times((discA.minus(discB.times(m_K))).pow(inputDelay / dt));
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/PIDController.java

@@ -0,0 +1,499 @@
+// 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.MathSharedStore;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/** Implements a PID control loop. */
+public class PIDController implements Sendable, AutoCloseable {
+  private static int instances;
+
+  // Factor for "proportional" control
+  private double m_kp;
+
+  // Factor for "integral" control
+  private double m_ki;
+
+  // Factor for "derivative" control
+  private double m_kd;
+
+  // The error range where "integral" control applies
+  private double m_iZone = Double.POSITIVE_INFINITY;
+
+  // The period (in seconds) of the loop that calls the controller
+  private final double m_period;
+
+  private double m_maximumIntegral = 1.0;
+
+  private double m_minimumIntegral = -1.0;
+
+  private double m_maximumInput;
+
+  private double m_minimumInput;
+
+  // Do the endpoints wrap around? e.g. Absolute encoder
+  private boolean m_continuous;
+
+  // The error at the time of the most recent call to calculate()
+  private double m_error;
+  private double m_errorDerivative;
+
+  // The error at the time of the second-most-recent call to calculate() (used to compute velocity)
+  private double m_prevError;
+
+  // The sum of the errors for use in the integral calc
+  private double m_totalError;
+
+  // The error that is considered at setpoint.
+  private double m_errorTolerance = 0.05;
+  private double m_errorDerivativeTolerance = Double.POSITIVE_INFINITY;
+
+  private double m_setpoint;
+  private double m_measurement;
+
+  private boolean m_haveMeasurement;
+  private boolean m_haveSetpoint;
+
+  /**
+   * Allocates a PIDController with the given constants for kp, ki, and kd and a default period of
+   * 0.02 seconds.
+   *
+   * @param kp The proportional coefficient.
+   * @param ki The integral coefficient.
+   * @param kd The derivative coefficient.
+   * @throws IllegalArgumentException if kp < 0
+   * @throws IllegalArgumentException if ki < 0
+   * @throws IllegalArgumentException if kd < 0
+   */
+  public PIDController(double kp, double ki, double kd) {
+    this(kp, ki, kd, 0.02);
+  }
+
+  /**
+   * Allocates a PIDController with the given constants for kp, ki, and kd.
+   *
+   * @param kp The proportional coefficient.
+   * @param ki The integral coefficient.
+   * @param kd The derivative coefficient.
+   * @param period The period between controller updates in seconds.
+   * @throws IllegalArgumentException if kp < 0
+   * @throws IllegalArgumentException if ki < 0
+   * @throws IllegalArgumentException if kd < 0
+   * @throws IllegalArgumentException if period <= 0
+   */
+  @SuppressWarnings("this-escape")
+  public PIDController(double kp, double ki, double kd, double period) {
+    m_kp = kp;
+    m_ki = ki;
+    m_kd = kd;
+
+    if (kp < 0.0) {
+      throw new IllegalArgumentException("Kp must be a non-negative number!");
+    }
+    if (ki < 0.0) {
+      throw new IllegalArgumentException("Ki must be a non-negative number!");
+    }
+    if (kd < 0.0) {
+      throw new IllegalArgumentException("Kd must be a non-negative number!");
+    }
+    if (period <= 0.0) {
+      throw new IllegalArgumentException("Controller period must be a positive number!");
+    }
+    m_period = period;
+
+    instances++;
+    SendableRegistry.add(this, "PIDController", instances);
+
+    MathSharedStore.reportUsage("PIDController", String.valueOf(instances));
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * Sets the PID Controller gain parameters.
+   *
+   * <p>Set the proportional, integral, and differential coefficients.
+   *
+   * @param kp The proportional coefficient.
+   * @param ki The integral coefficient.
+   * @param kd The derivative coefficient.
+   */
+  public void setPID(double kp, double ki, double kd) {
+    m_kp = kp;
+    m_ki = ki;
+    m_kd = kd;
+  }
+
+  /**
+   * Sets the Proportional coefficient of the PID controller gain.
+   *
+   * @param kp The proportional coefficient. Must be &gt;= 0.
+   */
+  public void setP(double kp) {
+    m_kp = kp;
+  }
+
+  /**
+   * Sets the Integral coefficient of the PID controller gain.
+   *
+   * @param ki The integral coefficient. Must be &gt;= 0.
+   */
+  public void setI(double ki) {
+    m_ki = ki;
+  }
+
+  /**
+   * Sets the Differential coefficient of the PID controller gain.
+   *
+   * @param kd The differential coefficient. Must be &gt;= 0.
+   */
+  public void setD(double kd) {
+    m_kd = kd;
+  }
+
+  /**
+   * Sets the IZone range. When the absolute value of the position error is greater than IZone, the
+   * total accumulated error will reset to zero, disabling integral gain until the absolute value of
+   * the position error is less than IZone. This is used to prevent integral windup. Must be
+   * non-negative. Passing a value of zero will effectively disable integral gain. Passing a value
+   * of {@link Double#POSITIVE_INFINITY} disables IZone functionality.
+   *
+   * @param iZone Maximum magnitude of error to allow integral control.
+   * @throws IllegalArgumentException if iZone &lt; 0
+   */
+  public void setIZone(double iZone) {
+    if (iZone < 0) {
+      throw new IllegalArgumentException("IZone must be a non-negative number!");
+    }
+    m_iZone = iZone;
+  }
+
+  /**
+   * Get the Proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  public double getP() {
+    return m_kp;
+  }
+
+  /**
+   * Get the Integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  public double getI() {
+    return m_ki;
+  }
+
+  /**
+   * Get the Differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  public double getD() {
+    return m_kd;
+  }
+
+  /**
+   * Get the IZone range.
+   *
+   * @return Maximum magnitude of error to allow integral control.
+   */
+  public double getIZone() {
+    return m_iZone;
+  }
+
+  /**
+   * Returns the period of this controller.
+   *
+   * @return the period of the controller.
+   */
+  public double getPeriod() {
+    return m_period;
+  }
+
+  /**
+   * Returns the position tolerance of this controller.
+   *
+   * @return the position tolerance of the controller.
+   * @deprecated Use getErrorTolerance() instead.
+   */
+  @Deprecated(forRemoval = true, since = "2025")
+  public double getPositionTolerance() {
+    return m_errorTolerance;
+  }
+
+  /**
+   * Returns the velocity tolerance of this controller.
+   *
+   * @return the velocity tolerance of the controller.
+   * @deprecated Use getErrorDerivativeTolerance() instead.
+   */
+  @Deprecated(forRemoval = true, since = "2025")
+  public double getVelocityTolerance() {
+    return m_errorDerivativeTolerance;
+  }
+
+  /**
+   * Returns the error tolerance of this controller. Defaults to 0.05.
+   *
+   * @return the error tolerance of the controller.
+   */
+  public double getErrorTolerance() {
+    return m_errorTolerance;
+  }
+
+  /**
+   * Returns the error derivative tolerance of this controller. Defaults to ∞.
+   *
+   * @return the error derivative tolerance of the controller.
+   */
+  public double getErrorDerivativeTolerance() {
+    return m_errorDerivativeTolerance;
+  }
+
+  /**
+   * Returns the accumulated error used in the integral calculation of this controller.
+   *
+   * @return The accumulated error of this controller.
+   */
+  public double getAccumulatedError() {
+    return m_totalError;
+  }
+
+  /**
+   * Sets the setpoint for the PIDController.
+   *
+   * @param setpoint The desired setpoint.
+   */
+  public void setSetpoint(double setpoint) {
+    m_setpoint = setpoint;
+    m_haveSetpoint = true;
+
+    if (m_continuous) {
+      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+      m_error = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+    } else {
+      m_error = m_setpoint - m_measurement;
+    }
+
+    m_errorDerivative = (m_error - m_prevError) / m_period;
+  }
+
+  /**
+   * Returns the current setpoint of the PIDController.
+   *
+   * @return The current setpoint.
+   */
+  public double getSetpoint() {
+    return m_setpoint;
+  }
+
+  /**
+   * Returns true if the error is within the tolerance of the setpoint. The error tolerance defaults
+   * to 0.05, and the error derivative tolerance defaults to ∞.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   *
+   * @return Whether the error is within the acceptable bounds.
+   */
+  public boolean atSetpoint() {
+    return m_haveMeasurement
+        && m_haveSetpoint
+        && Math.abs(m_error) < m_errorTolerance
+        && Math.abs(m_errorDerivative) < m_errorDerivativeTolerance;
+  }
+
+  /**
+   * Enables continuous input.
+   *
+   * <p>Rather then using the max and min input range as constraints, it considers them to be the
+   * same point and automatically calculates the shortest route to the setpoint.
+   *
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  public void enableContinuousInput(double minimumInput, double maximumInput) {
+    m_continuous = true;
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
+  }
+
+  /** Disables continuous input. */
+  public void disableContinuousInput() {
+    m_continuous = false;
+  }
+
+  /**
+   * Returns true if continuous input is enabled.
+   *
+   * @return True if continuous input is enabled.
+   */
+  public boolean isContinuousInputEnabled() {
+    return m_continuous;
+  }
+
+  /**
+   * Sets the minimum and maximum contributions of the integral term.
+   *
+   * <p>The internal integrator is clamped so that the integral term's contribution to the output
+   * stays between minimumIntegral and maximumIntegral. This prevents integral windup.
+   *
+   * @param minimumIntegral The minimum contribution of the integral term.
+   * @param maximumIntegral The maximum contribution of the integral term.
+   */
+  public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
+    m_minimumIntegral = minimumIntegral;
+    m_maximumIntegral = maximumIntegral;
+  }
+
+  /**
+   * Sets the error which is considered tolerable for use with atSetpoint().
+   *
+   * @param errorTolerance Error which is tolerable.
+   */
+  public void setTolerance(double errorTolerance) {
+    setTolerance(errorTolerance, Double.POSITIVE_INFINITY);
+  }
+
+  /**
+   * Sets the error which is considered tolerable for use with atSetpoint().
+   *
+   * @param errorTolerance Error which is tolerable.
+   * @param errorDerivativeTolerance Error derivative which is tolerable.
+   */
+  public void setTolerance(double errorTolerance, double errorDerivativeTolerance) {
+    m_errorTolerance = errorTolerance;
+    m_errorDerivativeTolerance = errorDerivativeTolerance;
+  }
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   *
+   * @return The error.
+   * @deprecated Use getError() instead.
+   */
+  @Deprecated(forRemoval = true, since = "2025")
+  public double getPositionError() {
+    return m_error;
+  }
+
+  /**
+   * Returns the velocity error.
+   *
+   * @return The velocity error.
+   * @deprecated Use getErrorDerivative() instead.
+   */
+  @Deprecated(forRemoval = true, since = "2025")
+  public double getVelocityError() {
+    return m_errorDerivative;
+  }
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   *
+   * @return The error.
+   */
+  public double getError() {
+    return m_error;
+  }
+
+  /**
+   * Returns the error derivative.
+   *
+   * @return The error derivative.
+   */
+  public double getErrorDerivative() {
+    return m_errorDerivative;
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param setpoint The new setpoint of the controller.
+   * @return The next controller output.
+   */
+  public double calculate(double measurement, double setpoint) {
+    m_setpoint = setpoint;
+    m_haveSetpoint = true;
+    return calculate(measurement);
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @return The next controller output.
+   */
+  public double calculate(double measurement) {
+    m_measurement = measurement;
+    m_prevError = m_error;
+    m_haveMeasurement = true;
+
+    if (m_continuous) {
+      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+      m_error = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+    } else {
+      m_error = m_setpoint - m_measurement;
+    }
+
+    m_errorDerivative = (m_error - m_prevError) / m_period;
+
+    // If the absolute value of the position error is greater than IZone, reset the total error
+    if (Math.abs(m_error) > m_iZone) {
+      m_totalError = 0;
+    } else if (m_ki != 0) {
+      m_totalError =
+          Math.clamp(
+              m_totalError + m_error * m_period,
+              m_minimumIntegral / m_ki,
+              m_maximumIntegral / m_ki);
+    }
+
+    return m_kp * m_error + m_ki * m_totalError + m_kd * m_errorDerivative;
+  }
+
+  /** Resets the previous error and the integral term. */
+  public void reset() {
+    m_error = 0;
+    m_prevError = 0;
+    m_totalError = 0;
+    m_errorDerivative = 0;
+    m_haveMeasurement = false;
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("PIDController");
+    builder.addDoubleProperty("p", this::getP, this::setP);
+    builder.addDoubleProperty("i", this::getI, this::setI);
+    builder.addDoubleProperty("d", this::getD, this::setD);
+    builder.addDoubleProperty(
+        "izone",
+        this::getIZone,
+        (double toSet) -> {
+          try {
+            setIZone(toSet);
+          } catch (IllegalArgumentException e) {
+            MathSharedStore.reportError("IZone must be a non-negative number!", e.getStackTrace());
+          }
+        });
+    builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
+    builder.addDoubleProperty("measurement", () -> m_measurement, null);
+    builder.addDoubleProperty("error", this::getError, null);
+    builder.addDoubleProperty("error derivative", this::getErrorDerivative, null);
+    builder.addDoubleProperty("previous error", () -> this.m_prevError, null);
+    builder.addDoubleProperty("total error", this::getAccumulatedError, null);
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/ProfiledPIDController.java

@@ -0,0 +1,469 @@
+// 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.MathSharedStore;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Implements a PID control loop whose setpoint is constrained by a trapezoid profile. Users should
+ * call reset() when they first start running the controller to avoid unwanted behavior.
+ */
+public class ProfiledPIDController implements Sendable {
+  private static int instances;
+
+  private PIDController m_controller;
+  private double m_minimumInput;
+  private double m_maximumInput;
+
+  private TrapezoidProfile.Constraints m_constraints;
+  private TrapezoidProfile m_profile;
+  private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
+  private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
+
+  /**
+   * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and Kd.
+   *
+   * @param Kp The proportional coefficient.
+   * @param Ki The integral coefficient.
+   * @param Kd The derivative coefficient.
+   * @param constraints Velocity and acceleration constraints for goal.
+   * @throws IllegalArgumentException if kp < 0
+   * @throws IllegalArgumentException if ki < 0
+   * @throws IllegalArgumentException if kd < 0
+   */
+  public ProfiledPIDController(
+      double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints) {
+    this(Kp, Ki, Kd, constraints, 0.02);
+  }
+
+  /**
+   * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and Kd.
+   *
+   * @param Kp The proportional coefficient.
+   * @param Ki The integral coefficient.
+   * @param Kd The derivative coefficient.
+   * @param constraints Velocity and acceleration constraints for goal.
+   * @param period The period between controller updates in seconds. The default is 0.02 seconds.
+   * @throws IllegalArgumentException if kp < 0
+   * @throws IllegalArgumentException if ki < 0
+   * @throws IllegalArgumentException if kd < 0
+   * @throws IllegalArgumentException if period <= 0
+   */
+  @SuppressWarnings("this-escape")
+  public ProfiledPIDController(
+      double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints, double period) {
+    m_controller = new PIDController(Kp, Ki, Kd, period);
+    m_constraints = constraints;
+    m_profile = new TrapezoidProfile(m_constraints);
+    instances++;
+
+    SendableRegistry.add(this, "ProfiledPIDController", instances);
+    MathSharedStore.reportUsage("ProfiledPIDController", String.valueOf(instances));
+  }
+
+  /**
+   * Sets the PID Controller gain parameters.
+   *
+   * <p>Sets the proportional, integral, and differential coefficients.
+   *
+   * @param Kp The proportional coefficient. Must be &gt;= 0.
+   * @param Ki The integral coefficient. Must be &gt;= 0.
+   * @param Kd The differential coefficient. Must be &gt;= 0.
+   */
+  public void setPID(double Kp, double Ki, double Kd) {
+    m_controller.setPID(Kp, Ki, Kd);
+  }
+
+  /**
+   * Sets the proportional coefficient of the PID controller gain.
+   *
+   * @param Kp The proportional coefficient. Must be &gt;= 0.
+   */
+  public void setP(double Kp) {
+    m_controller.setP(Kp);
+  }
+
+  /**
+   * Sets the integral coefficient of the PID controller gain.
+   *
+   * @param Ki The integral coefficient. Must be &gt;= 0.
+   */
+  public void setI(double Ki) {
+    m_controller.setI(Ki);
+  }
+
+  /**
+   * Sets the differential coefficient of the PID controller gain.
+   *
+   * @param Kd The differential coefficient. Must be &gt;= 0.
+   */
+  public void setD(double Kd) {
+    m_controller.setD(Kd);
+  }
+
+  /**
+   * Sets the IZone range. When the absolute value of the position error is greater than IZone, the
+   * total accumulated error will reset to zero, disabling integral gain until the absolute value of
+   * the position error is less than IZone. This is used to prevent integral windup. Must be
+   * non-negative. Passing a value of zero will effectively disable integral gain. Passing a value
+   * of {@link Double#POSITIVE_INFINITY} disables IZone functionality.
+   *
+   * @param iZone Maximum magnitude of error to allow integral control.
+   * @throws IllegalArgumentException if iZone &lt;= 0
+   */
+  public void setIZone(double iZone) {
+    m_controller.setIZone(iZone);
+  }
+
+  /**
+   * Gets the proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  public double getP() {
+    return m_controller.getP();
+  }
+
+  /**
+   * Gets the integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  public double getI() {
+    return m_controller.getI();
+  }
+
+  /**
+   * Gets the differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  public double getD() {
+    return m_controller.getD();
+  }
+
+  /**
+   * Get the IZone range.
+   *
+   * @return Maximum magnitude of error to allow integral control.
+   */
+  public double getIZone() {
+    return m_controller.getIZone();
+  }
+
+  /**
+   * Gets the period of this controller.
+   *
+   * @return The period of the controller.
+   */
+  public double getPeriod() {
+    return m_controller.getPeriod();
+  }
+
+  /**
+   * Returns the position tolerance of this controller.
+   *
+   * @return the position tolerance of the controller.
+   */
+  public double getPositionTolerance() {
+    return m_controller.getErrorTolerance();
+  }
+
+  /**
+   * Returns the velocity tolerance of this controller.
+   *
+   * @return the velocity tolerance of the controller.
+   */
+  public double getVelocityTolerance() {
+    return m_controller.getErrorDerivativeTolerance();
+  }
+
+  /**
+   * Returns the accumulated error used in the integral calculation of this controller.
+   *
+   * @return The accumulated error of this controller.
+   */
+  public double getAccumulatedError() {
+    return m_controller.getAccumulatedError();
+  }
+
+  /**
+   * Sets the goal for the ProfiledPIDController.
+   *
+   * @param goal The desired goal state.
+   */
+  public void setGoal(TrapezoidProfile.State goal) {
+    m_goal = goal;
+  }
+
+  /**
+   * Sets the goal for the ProfiledPIDController.
+   *
+   * @param goal The desired goal position.
+   */
+  public void setGoal(double goal) {
+    m_goal = new TrapezoidProfile.State(goal, 0);
+  }
+
+  /**
+   * Gets the goal for the ProfiledPIDController.
+   *
+   * @return The goal.
+   */
+  public TrapezoidProfile.State getGoal() {
+    return m_goal;
+  }
+
+  /**
+   * Returns true if the error is within the tolerance of the error.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   *
+   * @return True if the error is within the tolerance of the error.
+   */
+  public boolean atGoal() {
+    return atSetpoint() && m_goal.equals(m_setpoint);
+  }
+
+  /**
+   * Set velocity and acceleration constraints for goal.
+   *
+   * @param constraints Velocity and acceleration constraints for goal.
+   */
+  public void setConstraints(TrapezoidProfile.Constraints constraints) {
+    m_constraints = constraints;
+    m_profile = new TrapezoidProfile(m_constraints);
+  }
+
+  /**
+   * Get the velocity and acceleration constraints for this controller.
+   *
+   * @return Velocity and acceleration constraints.
+   */
+  public TrapezoidProfile.Constraints getConstraints() {
+    return m_constraints;
+  }
+
+  /**
+   * Returns the current setpoint of the ProfiledPIDController.
+   *
+   * @return The current setpoint.
+   */
+  public TrapezoidProfile.State getSetpoint() {
+    return m_setpoint;
+  }
+
+  /**
+   * Returns true if the error is within the tolerance of the error.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   *
+   * @return True if the error is within the tolerance of the error.
+   */
+  public boolean atSetpoint() {
+    return m_controller.atSetpoint();
+  }
+
+  /**
+   * Enables continuous input.
+   *
+   * <p>Rather then using the max and min input range as constraints, it considers them to be the
+   * same point and automatically calculates the shortest route to the setpoint.
+   *
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  public void enableContinuousInput(double minimumInput, double maximumInput) {
+    m_controller.enableContinuousInput(minimumInput, maximumInput);
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
+  }
+
+  /** Disables continuous input. */
+  public void disableContinuousInput() {
+    m_controller.disableContinuousInput();
+  }
+
+  /**
+   * Sets the minimum and maximum contributions of the integral term.
+   *
+   * <p>The internal integrator is clamped so that the integral term's contribution to the output
+   * stays between minimumIntegral and maximumIntegral. This prevents integral windup.
+   *
+   * @param minimumIntegral The minimum contribution of the integral term.
+   * @param maximumIntegral The maximum contribution of the integral term.
+   */
+  public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
+    m_controller.setIntegratorRange(minimumIntegral, maximumIntegral);
+  }
+
+  /**
+   * Sets the error which is considered tolerable for use with atSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   */
+  public void setTolerance(double positionTolerance) {
+    setTolerance(positionTolerance, Double.POSITIVE_INFINITY);
+  }
+
+  /**
+   * Sets the error which is considered tolerable for use with atSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   * @param velocityTolerance Velocity error which is tolerable.
+   */
+  public void setTolerance(double positionTolerance, double velocityTolerance) {
+    m_controller.setTolerance(positionTolerance, velocityTolerance);
+  }
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   *
+   * @return The error.
+   */
+  public double getPositionError() {
+    return m_controller.getError();
+  }
+
+  /**
+   * Returns the change in error per second.
+   *
+   * @return The change in error per second.
+   */
+  public double getVelocityError() {
+    return m_controller.getErrorDerivative();
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @return The controller's next output.
+   */
+  public double calculate(double measurement) {
+    if (m_controller.isContinuousInputEnabled()) {
+      // Get error which is the smallest distance between goal and measurement
+      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+      double goalMinDistance =
+          MathUtil.inputModulus(m_goal.position - measurement, -errorBound, errorBound);
+      double setpointMinDistance =
+          MathUtil.inputModulus(m_setpoint.position - measurement, -errorBound, errorBound);
+
+      // Recompute the profile goal with the smallest error, thus giving the shortest path. The goal
+      // may be outside the input range after this operation, but that's OK because the controller
+      // will still go there and report an error of zero. In other words, the setpoint only needs to
+      // be offset from the measurement by the input range modulus; they don't need to be equal.
+      m_goal.position = goalMinDistance + measurement;
+      m_setpoint.position = setpointMinDistance + measurement;
+    }
+
+    m_setpoint = m_profile.calculate(getPeriod(), m_setpoint, m_goal);
+    return m_controller.calculate(measurement, m_setpoint.position);
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   * @return The controller's next output.
+   */
+  public double calculate(double measurement, TrapezoidProfile.State goal) {
+    setGoal(goal);
+    return calculate(measurement);
+  }
+
+  /**
+   * Returns the next output of the PIDController.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   * @return The controller's next output.
+   */
+  public double calculate(double measurement, double goal) {
+    setGoal(goal);
+    return calculate(measurement);
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   * @param constraints Velocity and acceleration constraints for goal.
+   * @return The controller's next output.
+   */
+  public double calculate(
+      double measurement, TrapezoidProfile.State goal, TrapezoidProfile.Constraints constraints) {
+    setConstraints(constraints);
+    return calculate(measurement, goal);
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measurement The current measured State of the system.
+   */
+  public void reset(TrapezoidProfile.State measurement) {
+    m_controller.reset();
+    m_setpoint = measurement;
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system.
+   * @param measuredVelocity The current measured velocity of the system.
+   */
+  public void reset(double measuredPosition, double measuredVelocity) {
+    reset(new TrapezoidProfile.State(measuredPosition, measuredVelocity));
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system. The velocity is assumed to
+   *     be zero.
+   */
+  public void reset(double measuredPosition) {
+    reset(measuredPosition, 0.0);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("ProfiledPIDController");
+    builder.addDoubleProperty("p", this::getP, this::setP);
+    builder.addDoubleProperty("i", this::getI, this::setI);
+    builder.addDoubleProperty("d", this::getD, this::setD);
+    builder.addDoubleProperty(
+        "izone",
+        this::getIZone,
+        (double toSet) -> {
+          try {
+            setIZone(toSet);
+          } catch (IllegalArgumentException e) {
+            MathSharedStore.reportError("IZone must be a non-negative number!", e.getStackTrace());
+          }
+        });
+    builder.addDoubleProperty(
+        "maxVelocity",
+        () -> getConstraints().maxVelocity,
+        maxVelocity ->
+            setConstraints(
+                new TrapezoidProfile.Constraints(maxVelocity, getConstraints().maxAcceleration)));
+    builder.addDoubleProperty(
+        "maxAcceleration",
+        () -> getConstraints().maxAcceleration,
+        maxAcceleration ->
+            setConstraints(
+                new TrapezoidProfile.Constraints(getConstraints().maxVelocity, maxAcceleration)));
+    builder.addDoubleProperty("goal", () -> getGoal().position, this::setGoal);
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/SimpleMotorFeedforward.java

@@ -0,0 +1,256 @@
+// 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.controller.proto.SimpleMotorFeedforwardProto;
+import edu.wpi.first.math.controller.struct.SimpleMotorFeedforwardStruct;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+
+/** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */
+public class SimpleMotorFeedforward implements ProtobufSerializable, StructSerializable {
+  /** The static gain, in volts. */
+  private double ks;
+
+  /** The velocity gain, in V/(units/s). */
+  private double kv;
+
+  /** The acceleration gain, in V/(units/s²). */
+  private double ka;
+
+  /** The period, in seconds. */
+  private final double m_dt;
+
+  /**
+   * Creates a new SimpleMotorFeedforward with the specified gains and period.
+   *
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @param ks The static gain in volts.
+   * @param kv The velocity gain in V/(units/s).
+   * @param ka The acceleration gain in V/(units/s²).
+   * @param dt The period in seconds.
+   * @throws IllegalArgumentException for kv &lt; zero.
+   * @throws IllegalArgumentException for ka &lt; zero.
+   * @throws IllegalArgumentException for period &le; zero.
+   */
+  public SimpleMotorFeedforward(double ks, double kv, double ka, double dt) {
+    this.ks = ks;
+    this.kv = kv;
+    this.ka = ka;
+    if (kv < 0.0) {
+      throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!");
+    }
+    if (ka < 0.0) {
+      throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
+    }
+    if (dt <= 0.0) {
+      throw new IllegalArgumentException("period must be a positive number, got " + dt + "!");
+    }
+    m_dt = dt;
+  }
+
+  /**
+   * Creates a new SimpleMotorFeedforward with the specified gains and period. The period is
+   * defaulted to 20 ms.
+   *
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @param ks The static gain in volts.
+   * @param kv The velocity gain in V/(units/s).
+   * @param ka The acceleration gain in V/(units/s²).
+   * @throws IllegalArgumentException for kv &lt; zero.
+   * @throws IllegalArgumentException for ka &lt; zero.
+   */
+  public SimpleMotorFeedforward(double ks, double kv, double ka) {
+    this(ks, kv, ka, 0.020);
+  }
+
+  /**
+   * Creates a new SimpleMotorFeedforward with the specified gains. Acceleration gain is defaulted
+   * to zero. The period is defaulted to 20 ms.
+   *
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @param ks The static gain in volts.
+   * @param kv The velocity gain in V/(units/s).
+   * @throws IllegalArgumentException for kv &lt; zero.
+   */
+  public SimpleMotorFeedforward(double ks, double kv) {
+    this(ks, kv, 0);
+  }
+
+  /**
+   * Sets the static gain.
+   *
+   * @param ks The static gain in volts.
+   */
+  public void setKs(double ks) {
+    this.ks = ks;
+  }
+
+  /**
+   * Sets the velocity gain.
+   *
+   * @param kv The velocity gain in V/(units/s).
+   */
+  public void setKv(double kv) {
+    this.kv = kv;
+  }
+
+  /**
+   * Sets the acceleration gain.
+   *
+   * @param ka The acceleration gain in V/(units/s²).
+   */
+  public void setKa(double ka) {
+    this.ka = ka;
+  }
+
+  /**
+   * Returns the static gain in volts.
+   *
+   * @return The static gain in volts.
+   */
+  public double getKs() {
+    return ks;
+  }
+
+  /**
+   * Returns the velocity gain in V/(units/s).
+   *
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @return The velocity gain in V/(units/s).
+   */
+  public double getKv() {
+    return kv;
+  }
+
+  /**
+   * Returns the acceleration gain in V/(units/s²).
+   *
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @return The acceleration gain in V/(units/s²).
+   */
+  public double getKa() {
+    return ka;
+  }
+
+  /**
+   * Returns the period in seconds.
+   *
+   * @return The period in seconds.
+   */
+  public double getDt() {
+    return m_dt;
+  }
+
+  /**
+   * Calculates the feedforward from the gains and velocity setpoint assuming continuous control
+   * (acceleration is assumed to be zero).
+   *
+   * @param velocity The velocity setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity) {
+    return calculate(velocity, velocity);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints assuming discrete control.
+   *
+   * <p>Note this method is inaccurate when the velocity crosses 0.
+   *
+   * @param currentVelocity The current velocity setpoint.
+   * @param nextVelocity The next velocity setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double currentVelocity, double nextVelocity) {
+    // See wpimath/algorithms.md#Simple_motor_feedforward for derivation
+    if (ka < 1e-9) {
+      return ks * Math.signum(nextVelocity) + kv * nextVelocity;
+    } else {
+      double A = -kv / ka;
+      double B = 1.0 / ka;
+      double A_d = Math.exp(A * m_dt);
+      double B_d = A > -1e-9 ? B * m_dt : 1.0 / A * (A_d - 1.0) * B;
+      return ks * Math.signum(currentVelocity) + 1.0 / B_d * (nextVelocity - A_d * currentVelocity);
+    }
+  }
+
+  /**
+   * 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.
+   *
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor, in volts.
+   * @param acceleration The acceleration of the motor, in (units/s²).
+   * @return The maximum possible velocity in (units/s) 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.
+   *
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor, in volts.
+   * @param acceleration The acceleration of the motor, in (units/s²).
+   * @return The maximum possible velocity in (units/s) 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.
+   *
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor, in volts.
+   * @param velocity The velocity of the motor, in (units/s).
+   * @return The maximum possible acceleration in (units/s²) at the given velocity.
+   */
+  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
+    return (maxVoltage - ks * Math.signum(velocity) - 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.
+   *
+   * <p>The units should be radians for angular systems and meters for linear systems.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor, in volts.
+   * @param velocity The velocity of the motor, in (units/s).
+   * @return The maximum possible acceleration in (units/s²) at the given velocity.
+   */
+  public double minAchievableAcceleration(double maxVoltage, double velocity) {
+    return maxAchievableAcceleration(-maxVoltage, velocity);
+  }
+
+  /** SimpleMotorFeedforward struct for serialization. */
+  public static final SimpleMotorFeedforwardStruct struct = new SimpleMotorFeedforwardStruct();
+
+  /** SimpleMotorFeedforward protobuf for serialization. */
+  public static final SimpleMotorFeedforwardProto proto = new SimpleMotorFeedforwardProto();
+}

wpimath/src/main/java/org/wpilib/math/controller/proto/ArmFeedforwardProto.java

@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller.proto;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufArmFeedforward;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class ArmFeedforwardProto implements Protobuf<ArmFeedforward, ProtobufArmFeedforward> {
+  @Override
+  public Class<ArmFeedforward> getTypeClass() {
+    return ArmFeedforward.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufArmFeedforward.getDescriptor();
+  }
+
+  @Override
+  public ProtobufArmFeedforward createMessage() {
+    return ProtobufArmFeedforward.newInstance();
+  }
+
+  @Override
+  public ArmFeedforward unpack(ProtobufArmFeedforward msg) {
+    return new ArmFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa(), msg.getDt());
+  }
+
+  @Override
+  public void pack(ProtobufArmFeedforward msg, ArmFeedforward value) {
+    msg.setKs(value.getKs());
+    msg.setKg(value.getKg());
+    msg.setKv(value.getKv());
+    msg.setKa(value.getKa());
+    msg.setDt(value.getDt());
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/proto/DifferentialDriveFeedforwardProto.java

@@ -0,0 +1,42 @@
+// 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.proto;
+
+import edu.wpi.first.math.controller.DifferentialDriveFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufDifferentialDriveFeedforward;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public final class DifferentialDriveFeedforwardProto
+    implements Protobuf<DifferentialDriveFeedforward, ProtobufDifferentialDriveFeedforward> {
+  @Override
+  public Class<DifferentialDriveFeedforward> getTypeClass() {
+    return DifferentialDriveFeedforward.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufDifferentialDriveFeedforward.getDescriptor();
+  }
+
+  @Override
+  public ProtobufDifferentialDriveFeedforward createMessage() {
+    return ProtobufDifferentialDriveFeedforward.newInstance();
+  }
+
+  @Override
+  public DifferentialDriveFeedforward unpack(ProtobufDifferentialDriveFeedforward msg) {
+    return new DifferentialDriveFeedforward(
+        msg.getKvLinear(), msg.getKaLinear(), msg.getKvAngular(), msg.getKaAngular());
+  }
+
+  @Override
+  public void pack(ProtobufDifferentialDriveFeedforward msg, DifferentialDriveFeedforward value) {
+    msg.setKvLinear(value.m_kVLinear);
+    msg.setKaLinear(value.m_kALinear);
+    msg.setKvAngular(value.m_kVAngular);
+    msg.setKaAngular(value.m_kAAngular);
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/proto/DifferentialDriveWheelVoltagesProto.java

@@ -0,0 +1,40 @@
+// 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.proto;
+
+import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages;
+import edu.wpi.first.math.proto.Controller.ProtobufDifferentialDriveWheelVoltages;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class DifferentialDriveWheelVoltagesProto
+    implements Protobuf<DifferentialDriveWheelVoltages, ProtobufDifferentialDriveWheelVoltages> {
+  @Override
+  public Class<DifferentialDriveWheelVoltages> getTypeClass() {
+    return DifferentialDriveWheelVoltages.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufDifferentialDriveWheelVoltages.getDescriptor();
+  }
+
+  @Override
+  public ProtobufDifferentialDriveWheelVoltages createMessage() {
+    return ProtobufDifferentialDriveWheelVoltages.newInstance();
+  }
+
+  @Override
+  public DifferentialDriveWheelVoltages unpack(ProtobufDifferentialDriveWheelVoltages msg) {
+    return new DifferentialDriveWheelVoltages(msg.getLeft(), msg.getRight());
+  }
+
+  @Override
+  public void pack(
+      ProtobufDifferentialDriveWheelVoltages msg, DifferentialDriveWheelVoltages value) {
+    msg.setLeft(value.left);
+    msg.setRight(value.right);
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/proto/ElevatorFeedforwardProto.java

@@ -0,0 +1,42 @@
+// 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.proto;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufElevatorFeedforward;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class ElevatorFeedforwardProto
+    implements Protobuf<ElevatorFeedforward, ProtobufElevatorFeedforward> {
+  @Override
+  public Class<ElevatorFeedforward> getTypeClass() {
+    return ElevatorFeedforward.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufElevatorFeedforward.getDescriptor();
+  }
+
+  @Override
+  public ProtobufElevatorFeedforward createMessage() {
+    return ProtobufElevatorFeedforward.newInstance();
+  }
+
+  @Override
+  public ElevatorFeedforward unpack(ProtobufElevatorFeedforward msg) {
+    return new ElevatorFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa(), msg.getDt());
+  }
+
+  @Override
+  public void pack(ProtobufElevatorFeedforward msg, ElevatorFeedforward value) {
+    msg.setKs(value.getKs());
+    msg.setKg(value.getKg());
+    msg.setKv(value.getKv());
+    msg.setKa(value.getKa());
+    msg.setDt(value.getDt());
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/proto/SimpleMotorFeedforwardProto.java

@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller.proto;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufSimpleMotorFeedforward;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public final class SimpleMotorFeedforwardProto
+    implements Protobuf<SimpleMotorFeedforward, ProtobufSimpleMotorFeedforward> {
+  @Override
+  public Class<SimpleMotorFeedforward> getTypeClass() {
+    return SimpleMotorFeedforward.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufSimpleMotorFeedforward.getDescriptor();
+  }
+
+  @Override
+  public ProtobufSimpleMotorFeedforward createMessage() {
+    return ProtobufSimpleMotorFeedforward.newInstance();
+  }
+
+  @Override
+  public SimpleMotorFeedforward unpack(ProtobufSimpleMotorFeedforward msg) {
+    return new SimpleMotorFeedforward(msg.getKs(), msg.getKv(), msg.getKa(), msg.getDt());
+  }
+
+  @Override
+  public void pack(ProtobufSimpleMotorFeedforward msg, SimpleMotorFeedforward value) {
+    msg.setKs(value.getKs()).setKv(value.getKv()).setKa(value.getKa()).setDt(value.getDt());
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/struct/ArmFeedforwardStruct.java

@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller.struct;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class ArmFeedforwardStruct implements Struct<ArmFeedforward> {
+  @Override
+  public Class<ArmFeedforward> getTypeClass() {
+    return ArmFeedforward.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "ArmFeedforward";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 5;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double ks;double kg;double kv;double ka;double dt";
+  }
+
+  @Override
+  public ArmFeedforward unpack(ByteBuffer bb) {
+    double ks = bb.getDouble();
+    double kg = bb.getDouble();
+    double kv = bb.getDouble();
+    double ka = bb.getDouble();
+    double dt = bb.getDouble();
+    return new ArmFeedforward(ks, kg, kv, ka, dt);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, ArmFeedforward value) {
+    bb.putDouble(value.getKs());
+    bb.putDouble(value.getKg());
+    bb.putDouble(value.getKv());
+    bb.putDouble(value.getKa());
+    bb.putDouble(value.getDt());
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/struct/DifferentialDriveFeedforwardStruct.java

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller.struct;
+
+import edu.wpi.first.math.controller.DifferentialDriveFeedforward;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public final class DifferentialDriveFeedforwardStruct
+    implements Struct<DifferentialDriveFeedforward> {
+  @Override
+  public Class<DifferentialDriveFeedforward> getTypeClass() {
+    return DifferentialDriveFeedforward.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "DifferentialDriveFeedforward";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 4;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double kVLinear;double kALinear;double kVAngular;double kAAngular";
+  }
+
+  @Override
+  public DifferentialDriveFeedforward unpack(ByteBuffer bb) {
+    double kVLinear = bb.getDouble();
+    double kALinear = bb.getDouble();
+    double kVAngular = bb.getDouble();
+    double kAAngular = bb.getDouble();
+    return new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, DifferentialDriveFeedforward value) {
+    bb.putDouble(value.m_kVLinear);
+    bb.putDouble(value.m_kALinear);
+    bb.putDouble(value.m_kVAngular);
+    bb.putDouble(value.m_kAAngular);
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/struct/DifferentialDriveWheelVoltagesStruct.java

@@ -0,0 +1,45 @@
+// 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.struct;
+
+import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class DifferentialDriveWheelVoltagesStruct
+    implements Struct<DifferentialDriveWheelVoltages> {
+  @Override
+  public Class<DifferentialDriveWheelVoltages> getTypeClass() {
+    return DifferentialDriveWheelVoltages.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "DifferentialDriveWheelVoltages";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 2;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double left;double right";
+  }
+
+  @Override
+  public DifferentialDriveWheelVoltages unpack(ByteBuffer bb) {
+    double left = bb.getDouble();
+    double right = bb.getDouble();
+    return new DifferentialDriveWheelVoltages(left, right);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, DifferentialDriveWheelVoltages value) {
+    bb.putDouble(value.left);
+    bb.putDouble(value.right);
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/struct/ElevatorFeedforwardStruct.java

@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller.struct;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class ElevatorFeedforwardStruct implements Struct<ElevatorFeedforward> {
+  @Override
+  public Class<ElevatorFeedforward> getTypeClass() {
+    return ElevatorFeedforward.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "ElevatorFeedforward";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 5;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double ks;double kg;double kv;double ka;double dt";
+  }
+
+  @Override
+  public ElevatorFeedforward unpack(ByteBuffer bb) {
+    double ks = bb.getDouble();
+    double kg = bb.getDouble();
+    double kv = bb.getDouble();
+    double ka = bb.getDouble();
+    double dt = bb.getDouble();
+    return new ElevatorFeedforward(ks, kg, kv, ka, dt);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, ElevatorFeedforward value) {
+    bb.putDouble(value.getKs());
+    bb.putDouble(value.getKg());
+    bb.putDouble(value.getKv());
+    bb.putDouble(value.getKa());
+    bb.putDouble(value.getDt());
+  }
+}

wpimath/src/main/java/org/wpilib/math/controller/struct/SimpleMotorFeedforwardStruct.java

@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller.struct;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public final class SimpleMotorFeedforwardStruct implements Struct<SimpleMotorFeedforward> {
+  @Override
+  public Class<SimpleMotorFeedforward> getTypeClass() {
+    return SimpleMotorFeedforward.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "SimpleMotorFeedforward";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 4;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double ks;double kv;double ka;double dt";
+  }
+
+  @Override
+  public SimpleMotorFeedforward unpack(ByteBuffer bb) {
+    double ks = bb.getDouble();
+    double kv = bb.getDouble();
+    double ka = bb.getDouble();
+    double dt = bb.getDouble();
+    return new SimpleMotorFeedforward(ks, kv, ka, dt);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, SimpleMotorFeedforward value) {
+    bb.putDouble(value.getKs());
+    bb.putDouble(value.getKv());
+    bb.putDouble(value.getKa());
+    bb.putDouble(value.getDt());
+  }
+}

wpimath/src/main/java/org/wpilib/math/estimator/AngleStatistics.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.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;
+
+/** Angle statistics functions. */
+public final class AngleStatistics {
+  /** Utility class. */
+  private AngleStatistics() {}
+
+  /**
+   * Subtracts a and b while normalizing the resulting value in the selected row as if it were an
+   * angle.
+   *
+   * @param <S> Number of rows in vector.
+   * @param a A vector to subtract from.
+   * @param b A vector to subtract with.
+   * @param angleStateIdx The row containing angles to be normalized.
+   * @return Difference of two vectors with angle at the given index 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 <S> Number of rows in vector.
+   * @param angleStateIdx The row containing angles to be normalized.
+   * @return Function returning difference of two vectors with angle at the given index 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 <S> Number of rows in vector.
+   * @param a A vector to add with.
+   * @param b A vector to add with.
+   * @param angleStateIdx The row containing angles to be normalized.
+   * @return Sum of two vectors with angle at the given index 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 <S> Number of rows in vector.
+   * @param angleStateIdx The row containing angles to be normalized.
+   * @return Function returning of two vectors with angle at the given index 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 <S> Number of rows in sigma point matrix.
+   * @param sigmas Sigma points.
+   * @param Wm Weights for the mean.
+   * @param angleStateIdx The row containing the angles.
+   * @return Mean of sigma points.
+   */
+  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 <S> Number of rows in sigma point matrix.
+   * @param angleStateIdx The row containing the angles.
+   * @return Function returning mean of sigma points.
+   */
+  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/org/wpilib/math/estimator/DifferentialDrivePoseEstimator.java

@@ -0,0 +1,136 @@
+// 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.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+
+/**
+ * This class wraps {@link DifferentialDriveOdometry Differential Drive Odometry} to fuse
+ * latency-compensated vision measurements with differential drive encoder measurements. It is
+ * intended to be a drop-in replacement for {@link DifferentialDriveOdometry}; in fact, if you never
+ * call {@link DifferentialDrivePoseEstimator#addVisionMeasurement} and only call {@link
+ * DifferentialDrivePoseEstimator#update} then this will behave exactly the same as
+ * DifferentialDriveOdometry.
+ *
+ * <p>{@link DifferentialDrivePoseEstimator#update} should be called every robot loop.
+ *
+ * <p>{@link DifferentialDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as
+ * you want; if you never call it then this class will behave exactly like regular encoder odometry.
+ */
+public class DifferentialDrivePoseEstimator extends PoseEstimator<DifferentialDriveWheelPositions> {
+  /**
+   * Constructs a DifferentialDrivePoseEstimator with default standard deviations for the model and
+   * vision measurements.
+   *
+   * <p>The default standard deviations of the model states are 0.02 meters for x, 0.02 meters for
+   * y, and 0.01 radians for heading. The default standard deviations of the vision measurements are
+   * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param leftDistance The distance traveled by the left encoder in meters.
+   * @param rightDistance The distance traveled by the right encoder in meters.
+   * @param initialPose The starting pose estimate.
+   */
+  public DifferentialDrivePoseEstimator(
+      DifferentialDriveKinematics kinematics,
+      Rotation2d gyroAngle,
+      double leftDistance,
+      double rightDistance,
+      Pose2d initialPose) {
+    this(
+        kinematics,
+        gyroAngle,
+        leftDistance,
+        rightDistance,
+        initialPose,
+        VecBuilder.fill(0.02, 0.02, 0.01),
+        VecBuilder.fill(0.1, 0.1, 0.1));
+  }
+
+  /**
+   * Constructs a DifferentialDrivePoseEstimator.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param gyroAngle The gyro angle of the robot.
+   * @param leftDistance The distance traveled by the left encoder in meters.
+   * @param rightDistance The distance traveled by the right encoder in meters.
+   * @param initialPose The estimated initial pose.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
+   *     in meters, and heading in radians). Increase these numbers to trust your state estimate
+   *     less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
+   *     the vision pose measurement less.
+   */
+  public DifferentialDrivePoseEstimator(
+      DifferentialDriveKinematics kinematics,
+      Rotation2d gyroAngle,
+      double leftDistance,
+      double rightDistance,
+      Pose2d initialPose,
+      Matrix<N3, N1> stateStdDevs,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    super(
+        kinematics,
+        new DifferentialDriveOdometry(gyroAngle, leftDistance, rightDistance, initialPose),
+        stateStdDevs,
+        visionMeasurementStdDevs);
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
+   * automatically takes care of offsetting the gyro angle.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftPosition The distance traveled by the left encoder in meters.
+   * @param rightPosition The distance traveled by the right encoder in meters.
+   * @param pose The position on the field that your robot is at.
+   */
+  public void resetPosition(
+      Rotation2d gyroAngle, double leftPosition, double rightPosition, Pose2d pose) {
+    resetPosition(
+        gyroAngle, new DifferentialDriveWheelPositions(leftPosition, rightPosition), pose);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+   * loop.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param distanceLeft The total distance travelled by the left wheel in meters.
+   * @param distanceRight The total distance travelled by the right wheel in meters.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose2d update(Rotation2d gyroAngle, double distanceLeft, double distanceRight) {
+    return update(gyroAngle, new DifferentialDriveWheelPositions(distanceLeft, distanceRight));
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+   * loop.
+   *
+   * @param currentTime Time at which this method was called, in seconds.
+   * @param gyroAngle The current gyro angle.
+   * @param distanceLeft The total distance travelled by the left wheel in meters.
+   * @param distanceRight The total distance travelled by the right wheel in meters.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose2d updateWithTime(
+      double currentTime, Rotation2d gyroAngle, double distanceLeft, double distanceRight) {
+    return updateWithTime(
+        currentTime, gyroAngle, new DifferentialDriveWheelPositions(distanceLeft, distanceRight));
+  }
+}

wpimath/src/main/java/org/wpilib/math/estimator/DifferentialDrivePoseEstimator3d.java

@@ -0,0 +1,146 @@
+// 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.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry3d;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N4;
+
+/**
+ * This class wraps {@link DifferentialDriveOdometry3d Differential Drive Odometry} to fuse
+ * latency-compensated vision measurements with differential drive encoder measurements. It is
+ * intended to be a drop-in replacement for {@link DifferentialDriveOdometry3d}; in fact, if you
+ * never call {@link DifferentialDrivePoseEstimator3d#addVisionMeasurement} and only call {@link
+ * DifferentialDrivePoseEstimator3d#update} then this will behave exactly the same as
+ * DifferentialDriveOdometry3d. It is also intended to be an easy replacement for {@link
+ * DifferentialDrivePoseEstimator}, only requiring the addition of a standard deviation for Z and
+ * appropriate conversions between 2D and 3D versions of geometry classes. (See {@link
+ * Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
+ * Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
+ *
+ * <p>{@link DifferentialDrivePoseEstimator3d#update} should be called every robot loop.
+ *
+ * <p>{@link DifferentialDrivePoseEstimator3d#addVisionMeasurement} can be called as infrequently as
+ * you want; if you never call it then this class will behave exactly like regular encoder odometry.
+ */
+public class DifferentialDrivePoseEstimator3d
+    extends PoseEstimator3d<DifferentialDriveWheelPositions> {
+  /**
+   * Constructs a DifferentialDrivePoseEstimator3d with default standard deviations for the model
+   * and vision measurements.
+   *
+   * <p>The default standard deviations of the model states are 0.02 meters for x, 0.02 meters for
+   * y, 0.02 meters for z, and 0.01 radians for angle. The default standard deviations of the vision
+   * measurements are 0.1 meters for x, 0.1 meters for y, 0.1 meters for z, and 0.1 radians for
+   * angle.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param leftDistance The distance traveled by the left encoder in meters.
+   * @param rightDistance The distance traveled by the right encoder in meters.
+   * @param initialPose The starting pose estimate.
+   */
+  public DifferentialDrivePoseEstimator3d(
+      DifferentialDriveKinematics kinematics,
+      Rotation3d gyroAngle,
+      double leftDistance,
+      double rightDistance,
+      Pose3d initialPose) {
+    this(
+        kinematics,
+        gyroAngle,
+        leftDistance,
+        rightDistance,
+        initialPose,
+        VecBuilder.fill(0.02, 0.02, 0.02, 0.01),
+        VecBuilder.fill(0.1, 0.1, 0.1, 0.1));
+  }
+
+  /**
+   * Constructs a DifferentialDrivePoseEstimator3d.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param gyroAngle The gyro angle of the robot.
+   * @param leftDistance The distance traveled by the left encoder in meters.
+   * @param rightDistance The distance traveled by the right encoder in meters.
+   * @param initialPose The estimated initial pose.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
+   *     in meters, and heading in radians). Increase these numbers to trust your state estimate
+   *     less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
+   *     the vision pose measurement less.
+   */
+  public DifferentialDrivePoseEstimator3d(
+      DifferentialDriveKinematics kinematics,
+      Rotation3d gyroAngle,
+      double leftDistance,
+      double rightDistance,
+      Pose3d initialPose,
+      Matrix<N4, N1> stateStdDevs,
+      Matrix<N4, N1> visionMeasurementStdDevs) {
+    super(
+        kinematics,
+        new DifferentialDriveOdometry3d(gyroAngle, leftDistance, rightDistance, initialPose),
+        stateStdDevs,
+        visionMeasurementStdDevs);
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
+   * automatically takes care of offsetting the gyro angle.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftPosition The distance traveled by the left encoder in meters.
+   * @param rightPosition The distance traveled by the right encoder in meters.
+   * @param pose The position on the field that your robot is at.
+   */
+  public void resetPosition(
+      Rotation3d gyroAngle, double leftPosition, double rightPosition, Pose3d pose) {
+    resetPosition(
+        gyroAngle, new DifferentialDriveWheelPositions(leftPosition, rightPosition), pose);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+   * loop.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param distanceLeft The total distance travelled by the left wheel in meters.
+   * @param distanceRight The total distance travelled by the right wheel in meters.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose3d update(Rotation3d gyroAngle, double distanceLeft, double distanceRight) {
+    return update(gyroAngle, new DifferentialDriveWheelPositions(distanceLeft, distanceRight));
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+   * loop.
+   *
+   * @param currentTime Time at which this method was called, in seconds.
+   * @param gyroAngle The current gyro angle.
+   * @param distanceLeft The total distance travelled by the left wheel in meters.
+   * @param distanceRight The total distance travelled by the right wheel in meters.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose3d updateWithTime(
+      double currentTime, Rotation3d gyroAngle, double distanceLeft, double distanceRight) {
+    return updateWithTime(
+        currentTime, gyroAngle, new DifferentialDriveWheelPositions(distanceLeft, distanceRight));
+  }
+}

wpimath/src/main/java/org/wpilib/math/estimator/ExtendedKalmanFilter.java

@@ -0,0 +1,393 @@
+// 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.DARE;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.Discretization;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.NumericalJacobian;
+import java.util.function.BiFunction;
+
+/**
+ * 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>An extended Kalman filter supports nonlinear state and measurement models. It propagates the
+ * error covariance by linearizing the models around the state estimate, then applying the linear
+ * Kalman filter equations.
+ *
+ * <p>For more on the underlying math, read <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
+ * chapter 9 "Stochastic control theory".
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
+ */
+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;
+
+  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<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;
+
+  private Matrix<States, N1> m_xHat;
+
+  private Matrix<States, States> m_P;
+
+  private double m_dt;
+
+  /**
+   * Constructs an extended Kalman filter.
+   *
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
+   * for how to select the standard deviations.
+   *
+   * @param states a Nat representing the number of states.
+   * @param inputs a Nat representing the number of inputs.
+   * @param outputs a Nat representing the number of outputs.
+   * @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 dt Nominal discretization timestep in seconds.
+   */
+  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 dt) {
+    this(
+        states,
+        inputs,
+        outputs,
+        f,
+        h,
+        stateStdDevs,
+        measurementStdDevs,
+        Matrix::minus,
+        Matrix::plus,
+        dt);
+  }
+
+  /**
+   * Constructs an extended Kalman filter.
+   *
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
+   * for how to select the standard deviations.
+   *
+   * @param states a Nat representing the number of states.
+   * @param inputs a Nat representing the number of inputs.
+   * @param outputs a Nat representing the number of outputs.
+   * @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 dt Nominal discretization timestep in seconds.
+   */
+  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 dt) {
+    m_states = states;
+    m_outputs = outputs;
+
+    m_f = f;
+    m_h = h;
+
+    m_residualFuncY = residualFuncY;
+    m_addFuncX = addFuncX;
+
+    m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
+    m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+    m_dt = dt;
+
+    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.discretizeAQ(contA, m_contQ, dt);
+    final var discA = discPair.getFirst();
+    final var discQ = discPair.getSecond();
+
+    final var discR = Discretization.discretizeR(m_contR, dt);
+
+    if (StateSpaceUtil.isDetectable(discA, C) && outputs.getNum() <= states.getNum()) {
+      m_initP = DARE.dare(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 that row.
+   */
+  @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.
+   */
+  @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 final 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 dt Timestep for prediction in seconds.
+   */
+  @Override
+  public void predict(Matrix<Inputs, N1> u, double dt) {
+    predict(u, m_f, dt);
+  }
+
+  /**
+   * 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 linearize the model.
+   * @param dt Timestep for prediction in seconds.
+   */
+  public void predict(
+      Matrix<Inputs, N1> u,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      double dt) {
+    // 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.discretizeAQ(contA, m_contQ, dt);
+    final var discA = discPair.getFirst();
+    final var discQ = discPair.getSecond();
+
+    m_xHat = NumericalIntegration.rk4(f, m_xHat, u, dt);
+
+    // Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
+    m_P = discA.times(m_P).times(discA.transpose()).plus(discQ);
+
+    m_dt = dt;
+  }
+
+  /**
+   * 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.
+   */
+  @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 measurement noise covariances vary.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y, Matrix<Outputs, Outputs> R) {
+    correct(m_outputs, u, y, m_h, R, m_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 Continuous measurement noise covariance matrix.
+   */
+  public <Rows extends Num> void correct(
+      Nat<Rows> rows,
+      Matrix<Inputs, N1> u,
+      Matrix<Rows, N1> y,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
+      Matrix<Rows, Rows> R) {
+    correct(rows, u, y, h, R, Matrix::minus, Matrix::plus);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
+   * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
+   * of this function).
+   *
+   * @param <Rows> Number of rows in the result of f(x, u).
+   * @param rows Number of rows in the result of f(x, u).
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   * @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
+   *     subtracts them.)
+   * @param addFuncX A function that adds two state vectors.
+   */
+  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_dt);
+
+    final var S = C.times(m_P).times(C.transpose()).plus(discR);
+
+    // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
+    // efficiently.
+    //
+    // K = PCᵀS⁻¹
+    // KS = PCᵀ
+    // (KS)ᵀ = (PCᵀ)ᵀ
+    // SᵀKᵀ = CPᵀ
+    //
+    // The solution of Ax = b can be found via x = A.solve(b).
+    //
+    // Kᵀ = Sᵀ.solve(CPᵀ)
+    // K = (Sᵀ.solve(CPᵀ))ᵀ
+    //
+    // Drop the transposes on symmetric matrices S and P.
+    //
+    // K = (S.solve(CP))ᵀ
+    final Matrix<States, Rows> K = S.solve(C.times(m_P)).transpose();
+
+    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
+    m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, h.apply(m_xHat, u))));
+
+    // Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
+    // Use Joseph form for numerical stability
+    m_P =
+        Matrix.eye(m_states)
+            .minus(K.times(C))
+            .times(m_P)
+            .times(Matrix.eye(m_states).minus(K.times(C)).transpose())
+            .plus(K.times(discR).times(K.transpose()));
+  }
+}

wpimath/src/main/java/org/wpilib/math/estimator/KalmanFilter.java

@@ -0,0 +1,251 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.DARE;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.Discretization;
+import edu.wpi.first.math.system.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 <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
+ * chapter 9 "Stochastic control theory".
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
+ */
+public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
+    implements KalmanTypeFilter<States, Inputs, Outputs> {
+  private final Nat<States> m_states;
+
+  private final LinearSystem<States, Inputs, Outputs> m_plant;
+  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 double m_dt;
+
+  private final Matrix<States, States> m_initP;
+
+  /**
+   * Constructs a Kalman filter with the given plant.
+   *
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
+   * for how to select the standard deviations.
+   *
+   * @param states A Nat representing the states of the system.
+   * @param outputs A Nat representing the outputs of the system.
+   * @param plant The plant used for the prediction step.
+   * @param stateStdDevs Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dt Nominal discretization timestep in seconds.
+   * @throws IllegalArgumentException If the system is undetectable.
+   */
+  public KalmanFilter(
+      Nat<States> states,
+      Nat<Outputs> outputs,
+      LinearSystem<States, Inputs, Outputs> plant,
+      Matrix<States, N1> stateStdDevs,
+      Matrix<Outputs, N1> measurementStdDevs,
+      double dt) {
+    this.m_states = states;
+
+    this.m_plant = plant;
+
+    m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
+    m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+    m_dt = dt;
+
+    // Find discrete A and Q
+    var pair = Discretization.discretizeAQ(plant.getA(), m_contQ, dt);
+    var discA = pair.getFirst();
+    var discQ = pair.getSecond();
+
+    var discR = Discretization.discretizeR(m_contR, dt);
+
+    var C = plant.getC();
+
+    m_initP = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR));
+
+    reset();
+  }
+
+  /**
+   * 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 that row.
+   */
+  @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.
+   */
+  @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 final 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 dt Timestep for prediction in seconds.
+   */
+  @Override
+  public void predict(Matrix<Inputs, N1> u, double dt) {
+    // Find discrete A and Q
+    final var discPair = Discretization.discretizeAQ(m_plant.getA(), m_contQ, dt);
+    final var discA = discPair.getFirst();
+    final var discQ = discPair.getSecond();
+
+    m_xHat = m_plant.calculateX(m_xHat, u, dt);
+
+    // Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
+    m_P = discA.times(m_P).times(discA.transpose()).plus(discQ);
+
+    m_dt = dt;
+  }
+
+  /**
+   * 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.
+   */
+  @Override
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
+    correct(u, y, m_contR);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurement noise covariances vary.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y, Matrix<Outputs, Outputs> R) {
+    final var C = m_plant.getC();
+    final var D = m_plant.getD();
+
+    final var discR = Discretization.discretizeR(R, m_dt);
+
+    final var S = C.times(m_P).times(C.transpose()).plus(discR);
+
+    // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
+    // efficiently.
+    //
+    // K = PCᵀS⁻¹
+    // KS = PCᵀ
+    // (KS)ᵀ = (PCᵀ)ᵀ
+    // SᵀKᵀ = CPᵀ
+    //
+    // The solution of Ax = b can be found via x = A.solve(b).
+    //
+    // Kᵀ = Sᵀ.solve(CPᵀ)
+    // K = (Sᵀ.solve(CPᵀ))ᵀ
+    //
+    // Drop the transposes on symmetric matrices S and P.
+    //
+    // K = (S.solve(CP))ᵀ
+    final Matrix<States, Outputs> K = S.solve(C.times(m_P)).transpose();
+
+    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
+    m_xHat = m_xHat.plus(K.times(y.minus(C.times(m_xHat).plus(D.times(u)))));
+
+    // Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
+    // Use Joseph form for numerical stability
+    m_P =
+        Matrix.eye(m_states)
+            .minus(K.times(C))
+            .times(m_P)
+            .times(Matrix.eye(m_states).minus(K.times(C)).transpose())
+            .plus(K.times(discR).times(K.transpose()));
+  }
+}

wpimath/src/main/java/org/wpilib/math/estimator/KalmanFilterLatencyCompensator.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.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;
+
+/**
+ * This class incorporates time-delayed measurements into a Kalman filter's state estimate.
+ *
+ * @param <S> The number of states.
+ * @param <I> The number of inputs.
+ * @param <O> The number of outputs.
+ */
+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;
+
+  /** Default constructor. */
+  KalmanFilterLatencyCompensator() {
+    m_pastObserverSnapshots = new ArrayList<>();
+  }
+
+  /** Clears the observer snapshot buffer. */
+  public void reset() {
+    m_pastObserverSnapshots.clear();
+  }
+
+  /**
+   * 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 timestamp The timestamp of the state in seconds.
+   */
+  public void addObserverState(
+      KalmanTypeFilter<S, I, O> observer, Matrix<I, N1> u, Matrix<O, N1> localY, double timestamp) {
+    m_pastObserverSnapshots.add(Map.entry(timestamp, 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 nominalDt The nominal timestep in seconds.
+   * @param y The measurement.
+   * @param globalMeasurementCorrect The function take calls correct() on the observer.
+   * @param timestamp The timestamp of the measurement in seconds.
+   */
+  public <R extends Num> void applyPastGlobalMeasurement(
+      Nat<R> rows,
+      KalmanTypeFilter<S, I, O> observer,
+      double nominalDt,
+      Matrix<R, N1> y,
+      BiConsumer<Matrix<I, N1>, Matrix<R, N1>> globalMeasurementCorrect,
+      double timestamp) {
+    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;
+    }
+
+    int maxIdx = m_pastObserverSnapshots.size() - 1;
+    int low = 0;
+    int high = maxIdx;
+
+    // Perform a binary search to find the index of first snapshot whose
+    // timestamp is greater than or equal to the global measurement timestamp
+    while (low != high) {
+      int mid = (low + high) / 2;
+      if (m_pastObserverSnapshots.get(mid).getKey() < timestamp) {
+        // 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;
+      }
+    }
+
+    int indexOfClosestEntry;
+
+    if (low == 0) {
+      // If the global measurement is older than any snapshot, throw out the
+      // measurement because there's no state estimate into which to incorporate
+      // the measurement
+      if (timestamp < m_pastObserverSnapshots.get(low).getKey()) {
+        return;
+      }
+
+      // If the first snapshot has same timestamp as the global measurement, use
+      // that snapshot
+      indexOfClosestEntry = 0;
+    } else if (low == maxIdx && m_pastObserverSnapshots.get(low).getKey() < timestamp) {
+      // If all snapshots are older than the global measurement, use the newest
+      // snapshot
+      indexOfClosestEntry = maxIdx;
+    } else {
+      // Index of snapshot taken after the global measurement
+      int nextIdx = low;
+
+      // Index of snapshot taken before the global measurement. Since we already
+      // handled the case where the index points to the first snapshot, this
+      // computation is guaranteed to be non-negative.
+      int prevIdx = nextIdx - 1;
+
+      // Find the snapshot closest in time to global measurement
+      double prevTimeDiff = Math.abs(timestamp - m_pastObserverSnapshots.get(prevIdx).getKey());
+      double nextTimeDiff = Math.abs(timestamp - m_pastObserverSnapshots.get(nextIdx).getKey());
+      indexOfClosestEntry = prevTimeDiff <= nextTimeDiff ? prevIdx : nextIdx;
+    }
+
+    double lastTimestamp = m_pastObserverSnapshots.get(indexOfClosestEntry).getKey() - nominalDt;
+
+    // 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, y);
+      }
+      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. */
+  public final class ObserverSnapshot {
+    /** The state estimate. */
+    public final Matrix<S, N1> xHat;
+
+    /** The error covariance. */
+    public final Matrix<S, S> errorCovariances;
+
+    /** The inputs. */
+    public final Matrix<I, N1> inputs;
+
+    /** The local measurements. */
+    public final Matrix<O, N1> localMeasurements;
+
+    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/org/wpilib/math/estimator/KalmanTypeFilter.java

@@ -0,0 +1,90 @@
+// 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;
+
+/**
+ * Interface for Kalman filters for use with KalmanFilterLatencyCompensator.
+ *
+ * @param <States> The number of states.
+ * @param <Inputs> The number of inputs.
+ * @param <Outputs> The number of outputs.
+ */
+public interface KalmanTypeFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
+  /**
+   * Returns the error covariance matrix.
+   *
+   * @return The error covariance matrix.
+   */
+  Matrix<States, States> getP();
+
+  /**
+   * Returns an element of the error covariance matrix.
+   *
+   * @param i The row.
+   * @param j The column.
+   * @return An element of the error covariance matrix.
+   */
+  double getP(int i, int j);
+
+  /**
+   * Sets the error covariance matrix.
+   *
+   * @param newP The error covariance matrix.
+   */
+  void setP(Matrix<States, States> newP);
+
+  /**
+   * Returns the state estimate.
+   *
+   * @return The state estimate.
+   */
+  Matrix<States, N1> getXhat();
+
+  /**
+   * Returns an element of the state estimate.
+   *
+   * @param i The row.
+   * @return An element of the state estimate.
+   */
+  double getXhat(int i);
+
+  /**
+   * Sets the state estimate.
+   *
+   * @param xHat The state estimate.
+   */
+  void setXhat(Matrix<States, N1> xHat);
+
+  /**
+   * Sets an element of the state estimate.
+   *
+   * @param i The row.
+   * @param value The value.
+   */
+  void setXhat(int i, double value);
+
+  /** Resets the observer. */
+  void reset();
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u New control input from controller.
+   * @param dt Timestep for prediction in seconds.
+   */
+  void predict(Matrix<Inputs, N1> u, double dt);
+
+  /**
+   * 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.
+   */
+  void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y);
+}

wpimath/src/main/java/org/wpilib/math/estimator/MecanumDrivePoseEstimator.java

@@ -0,0 +1,83 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.Matrix;
+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.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+
+/**
+ * This class wraps {@link MecanumDriveOdometry Mecanum Drive Odometry} to fuse latency-compensated
+ * vision measurements with mecanum drive encoder distance measurements. It will correct for noisy
+ * measurements and encoder drift. It is intended to be a drop-in replacement for {@link
+ * MecanumDriveOdometry}.
+ *
+ * <p>{@link MecanumDrivePoseEstimator#update} should be called every robot loop.
+ *
+ * <p>{@link MecanumDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
+ * want; if you never call it, then this class will behave mostly like regular encoder odometry.
+ */
+public class MecanumDrivePoseEstimator extends PoseEstimator<MecanumDriveWheelPositions> {
+  /**
+   * Constructs a MecanumDrivePoseEstimator with default standard deviations for the model and
+   * vision measurements.
+   *
+   * <p>The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
+   * and 0.1 radians for heading. The default standard deviations of the vision measurements are
+   * 0.45 meters for x, 0.45 meters for y, and 0.45 radians for heading.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The distances driven by each wheel.
+   * @param initialPose The starting pose estimate.
+   */
+  public MecanumDrivePoseEstimator(
+      MecanumDriveKinematics kinematics,
+      Rotation2d gyroAngle,
+      MecanumDriveWheelPositions wheelPositions,
+      Pose2d initialPose) {
+    this(
+        kinematics,
+        gyroAngle,
+        wheelPositions,
+        initialPose,
+        VecBuilder.fill(0.1, 0.1, 0.1),
+        VecBuilder.fill(0.45, 0.45, 0.45));
+  }
+
+  /**
+   * Constructs a MecanumDrivePoseEstimator.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The distance measured by each wheel.
+   * @param initialPose The starting pose estimate.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
+   *     in meters, and heading in radians). Increase these numbers to trust your state estimate
+   *     less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
+   *     the vision pose measurement less.
+   */
+  public MecanumDrivePoseEstimator(
+      MecanumDriveKinematics kinematics,
+      Rotation2d gyroAngle,
+      MecanumDriveWheelPositions wheelPositions,
+      Pose2d initialPose,
+      Matrix<N3, N1> stateStdDevs,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    super(
+        kinematics,
+        new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPose),
+        stateStdDevs,
+        visionMeasurementStdDevs);
+  }
+}

wpimath/src/main/java/org/wpilib/math/estimator/MecanumDrivePoseEstimator3d.java

@@ -0,0 +1,92 @@
+// 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.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveOdometry3d;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N4;
+
+/**
+ * This class wraps {@link MecanumDriveOdometry3d Mecanum Drive Odometry} to fuse
+ * latency-compensated vision measurements with mecanum drive encoder distance measurements. It will
+ * correct for noisy measurements and encoder drift. It is intended to be a drop-in replacement for
+ * {@link MecanumDriveOdometry3d}. It is also intended to be an easy replacement for {@link
+ * MecanumDrivePoseEstimator}, only requiring the addition of a standard deviation for Z and
+ * appropriate conversions between 2D and 3D versions of geometry classes. (See {@link
+ * Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
+ * Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
+ *
+ * <p>{@link MecanumDrivePoseEstimator3d#update} should be called every robot loop.
+ *
+ * <p>{@link MecanumDrivePoseEstimator3d#addVisionMeasurement} can be called as infrequently as you
+ * want; if you never call it, then this class will behave mostly like regular encoder odometry.
+ */
+public class MecanumDrivePoseEstimator3d extends PoseEstimator3d<MecanumDriveWheelPositions> {
+  /**
+   * Constructs a MecanumDrivePoseEstimator3d with default standard deviations for the model and
+   * vision measurements.
+   *
+   * <p>The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
+   * 0.1 meters for z, and 0.1 radians for angle. The default standard deviations of the vision
+   * measurements are 0.45 meters for x, 0.45 meters for y, 0.45 meters for z, and 0.45 radians for
+   * angle.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The distances driven by each wheel.
+   * @param initialPose The starting pose estimate.
+   */
+  public MecanumDrivePoseEstimator3d(
+      MecanumDriveKinematics kinematics,
+      Rotation3d gyroAngle,
+      MecanumDriveWheelPositions wheelPositions,
+      Pose3d initialPose) {
+    this(
+        kinematics,
+        gyroAngle,
+        wheelPositions,
+        initialPose,
+        VecBuilder.fill(0.1, 0.1, 0.1, 0.1),
+        VecBuilder.fill(0.45, 0.45, 0.45, 0.45));
+  }
+
+  /**
+   * Constructs a MecanumDrivePoseEstimator3d.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The distance measured by each wheel.
+   * @param initialPose The starting pose estimate.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
+   *     in meters, and heading in radians). Increase these numbers to trust your state estimate
+   *     less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
+   *     the vision pose measurement less.
+   */
+  public MecanumDrivePoseEstimator3d(
+      MecanumDriveKinematics kinematics,
+      Rotation3d gyroAngle,
+      MecanumDriveWheelPositions wheelPositions,
+      Pose3d initialPose,
+      Matrix<N4, N1> stateStdDevs,
+      Matrix<N4, N1> visionMeasurementStdDevs) {
+    super(
+        kinematics,
+        new MecanumDriveOdometry3d(kinematics, gyroAngle, wheelPositions, initialPose),
+        stateStdDevs,
+        visionMeasurementStdDevs);
+  }
+}

wpimath/src/main/java/org/wpilib/math/estimator/MerweScaledSigmaPoints.java

@@ -0,0 +1,167 @@
+// 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. S3SigmaPoints is generally preferred due to its greater performance with
+ * nearly identical accuracy.
+ *
+ * <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 Probabilistic Inference in Dynamic
+ * State-Space Models" (Doctoral dissertation)
+ *
+ * @param <S> The dimensionality of the state. 2 * States + 1 weights will be generated.
+ */
+public class MerweScaledSigmaPoints<S extends Num> implements SigmaPoints<S> {
+  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.
+   */
+  @Override
+  public int getNumSigmas() {
+    return 2 * m_states.getNum() + 1;
+  }
+
+  /**
+   * Computes the sigma points for an unscented Kalman filter given the mean (x) and square-root
+   * covariance (s) of the filter.
+   *
+   * @param x An array of the means.
+   * @param s Square-root covariance of the filter.
+   * @return Two-dimensional array of sigma points. Each column contains all the sigmas for one
+   *     dimension in the problem space. Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
+   */
+  @Override
+  public Matrix<S, ?> squareRootSigmaPoints(Matrix<S, N1> x, Matrix<S, S> s) {
+    double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum();
+    double eta = Math.sqrt(lambda + m_states.getNum());
+
+    Matrix<S, S> U = s.times(eta);
+
+    // 2 * states + 1 by states
+    Matrix<S, ?> sigmas =
+        new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
+
+    // equation (17)
+    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.
+   */
+  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.
+   */
+  @Override
+  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.
+   */
+  @Override
+  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.
+   */
+  @Override
+  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.
+   */
+  @Override
+  public double getWc(int element) {
+    return m_wc.get(element, 0);
+  }
+}

wpimath/src/main/java/org/wpilib/math/estimator/MerweUKF.java

@@ -0,0 +1,117 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.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.function.BiFunction;
+
+/**
+ * An Unscented Kalman Filter using sigma points and weights from Van der Merwe's 2004 dissertation.
+ * S3UKF is generally preferred due to its greater performance with nearly identical accuracy.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
+ */
+public class MerweUKF<States extends Num, Inputs extends Num, Outputs extends Num>
+    extends UnscentedKalmanFilter<States, Inputs, Outputs> {
+  /**
+   * Constructs a Merwe Unscented Kalman Filter.
+   *
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
+   * for how to select the standard deviations.
+   *
+   * @param states A Nat representing the number of states.
+   * @param outputs A Nat representing the number of outputs.
+   * @param f A vector-valued function of x and u that returns the derivative of the state vector.
+   * @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 nominalDt Nominal discretization timestep in seconds.
+   */
+  public MerweUKF(
+      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 nominalDt) {
+    super(
+        new MerweScaledSigmaPoints<>(states),
+        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,
+        nominalDt);
+  }
+
+  /**
+   * Constructs a Merwe 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.
+   *
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
+   * for how to select the standard deviations.
+   *
+   * @param states A Nat representing the number of states.
+   * @param outputs A Nat representing the number of outputs.
+   * @param f A vector-valued function of x and u that returns the derivative of the state vector.
+   * @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 NumSigmas state vectors using a given set
+   *     of weights.
+   * @param meanFuncY A function that computes the mean of NumSigmas 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 nominalDt Nominal discretization timestep in seconds.
+   */
+  public MerweUKF(
+      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 nominalDt) {
+    super(
+        new MerweScaledSigmaPoints<>(states),
+        states,
+        outputs,
+        f,
+        h,
+        stateStdDevs,
+        measurementStdDevs,
+        meanFuncX,
+        meanFuncY,
+        residualFuncX,
+        residualFuncY,
+        addFuncX,
+        nominalDt);
+  }
+}

wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator.java

@@ -0,0 +1,398 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
+import edu.wpi.first.math.kinematics.Kinematics;
+import edu.wpi.first.math.kinematics.Odometry;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import java.util.NavigableMap;
+import java.util.Optional;
+import java.util.TreeMap;
+
+/**
+ * This class wraps {@link Odometry} to fuse latency-compensated vision measurements with encoder
+ * measurements. Robot code should not use this directly- Instead, use the particular type for your
+ * drivetrain (e.g., {@link DifferentialDrivePoseEstimator}). It is intended to be a drop-in
+ * replacement for {@link Odometry}; in fact, if you never call {@link
+ * PoseEstimator#addVisionMeasurement} and only call {@link PoseEstimator#update} then this will
+ * behave exactly the same as Odometry.
+ *
+ * <p>{@link PoseEstimator#update} should be called every robot loop.
+ *
+ * <p>{@link PoseEstimator#addVisionMeasurement} can be called as infrequently as you want; if you
+ * never call it then this class will behave exactly like regular encoder odometry.
+ *
+ * @param <T> Wheel positions type.
+ */
+public class PoseEstimator<T> {
+  private final Odometry<T> m_odometry;
+  private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
+  private final Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
+
+  private static final double kBufferDuration = 1.5;
+  // Maps timestamps to odometry-only pose estimates
+  private final TimeInterpolatableBuffer<Pose2d> m_odometryPoseBuffer =
+      TimeInterpolatableBuffer.createBuffer(kBufferDuration);
+  // Maps timestamps to vision updates
+  // Always contains one entry before the oldest entry in m_odometryPoseBuffer, unless there have
+  // been no vision measurements after the last reset
+  private final NavigableMap<Double, VisionUpdate> m_visionUpdates = new TreeMap<>();
+
+  private Pose2d m_poseEstimate;
+
+  /**
+   * Constructs a PoseEstimator.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param odometry A correctly-configured odometry object for your drivetrain.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
+   *     in meters, and heading in radians). Increase these numbers to trust your state estimate
+   *     less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
+   *     the vision pose measurement less.
+   */
+  @SuppressWarnings("PMD.UnusedFormalParameter")
+  public PoseEstimator(
+      Kinematics<?, T> kinematics,
+      Odometry<T> odometry,
+      Matrix<N3, N1> stateStdDevs,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    m_odometry = odometry;
+
+    m_poseEstimate = m_odometry.getPose();
+
+    for (int i = 0; i < 3; ++i) {
+      m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
+    }
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+  }
+
+  /**
+   * Sets the pose estimator's trust of global measurements. This might be used to change trust in
+   * vision measurements after the autonomous period, or to change trust as distance to a vision
+   * target increases.
+   *
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]ᵀ, with units in meters and radians.
+   */
+  public final void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
+    var r = new double[3];
+    for (int i = 0; i < 3; ++i) {
+      r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
+    }
+
+    // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+    // and C = I. See wpimath/algorithms.md.
+    for (int row = 0; row < 3; ++row) {
+      if (m_q.get(row, 0) == 0.0) {
+        m_visionK.set(row, row, 0.0);
+      } else {
+        m_visionK.set(
+            row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
+      }
+    }
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
+   * automatically takes care of offsetting the gyro angle.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current encoder readings.
+   * @param pose The position on the field that your robot is at.
+   */
+  public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d pose) {
+    // Reset state estimate and error covariance
+    m_odometry.resetPosition(gyroAngle, wheelPositions, pose);
+    m_odometryPoseBuffer.clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.getPose();
+  }
+
+  /**
+   * Resets the robot's pose.
+   *
+   * @param pose The pose to reset to.
+   */
+  public void resetPose(Pose2d pose) {
+    m_odometry.resetPose(pose);
+    m_odometryPoseBuffer.clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.getPose();
+  }
+
+  /**
+   * Resets the robot's translation.
+   *
+   * @param translation The pose to translation to.
+   */
+  public void resetTranslation(Translation2d translation) {
+    m_odometry.resetTranslation(translation);
+    m_odometryPoseBuffer.clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.getPose();
+  }
+
+  /**
+   * Resets the robot's rotation.
+   *
+   * @param rotation The rotation to reset to.
+   */
+  public void resetRotation(Rotation2d rotation) {
+    m_odometry.resetRotation(rotation);
+    m_odometryPoseBuffer.clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.getPose();
+  }
+
+  /**
+   * Gets the estimated robot pose.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  public Pose2d getEstimatedPosition() {
+    return m_poseEstimate;
+  }
+
+  /**
+   * Return the pose at a given timestamp, if the buffer is not empty.
+   *
+   * @param timestamp The pose's timestamp in seconds.
+   * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
+   */
+  public Optional<Pose2d> sampleAt(double timestamp) {
+    // Step 0: If there are no odometry updates to sample, skip.
+    if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()) {
+      return Optional.empty();
+    }
+
+    // Step 1: Make sure timestamp matches the sample from the odometry pose buffer. (When sampling,
+    // the buffer will always use a timestamp between the first and last timestamps)
+    double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey();
+    double newestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().lastKey();
+    timestamp = Math.clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp);
+
+    // Step 2: If there are no applicable vision updates, use the odometry-only information.
+    if (m_visionUpdates.isEmpty() || timestamp < m_visionUpdates.firstKey()) {
+      return m_odometryPoseBuffer.getSample(timestamp);
+    }
+
+    // Step 3: Get the latest vision update from before or at the timestamp to sample at.
+    double floorTimestamp = m_visionUpdates.floorKey(timestamp);
+    var visionUpdate = m_visionUpdates.get(floorTimestamp);
+
+    // Step 4: Get the pose measured by odometry at the time of the sample.
+    var odometryEstimate = m_odometryPoseBuffer.getSample(timestamp);
+
+    // Step 5: Apply the vision compensation to the odometry pose.
+    return odometryEstimate.map(odometryPose -> visionUpdate.compensate(odometryPose));
+  }
+
+  /** Removes stale vision updates that won't affect sampling. */
+  private void cleanUpVisionUpdates() {
+    // Step 0: If there are no odometry samples, skip.
+    if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()) {
+      return;
+    }
+
+    // Step 1: Find the oldest timestamp that needs a vision update.
+    double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey();
+
+    // Step 2: If there are no vision updates before that timestamp, skip.
+    if (m_visionUpdates.isEmpty() || oldestOdometryTimestamp < m_visionUpdates.firstKey()) {
+      return;
+    }
+
+    // Step 3: Find the newest vision update timestamp before or at the oldest timestamp.
+    double newestNeededVisionUpdateTimestamp = m_visionUpdates.floorKey(oldestOdometryTimestamp);
+
+    // Step 4: Remove all entries strictly before the newest timestamp we need.
+    m_visionUpdates.headMap(newestNeededVisionUpdateTimestamp, false).clear();
+  }
+
+  /**
+   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+   * while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+   * PoseEstimator#update} every loop.
+   *
+   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
+   * recommend only adding vision measurements that are already within one meter or so of the
+   * current pose estimate.
+   *
+   * @param visionRobotPose The pose of the robot as measured by the vision camera.
+   * @param timestamp The timestamp of the vision measurement in seconds. Note that if you don't use
+   *     your own time source by calling {@link
+   *     PoseEstimator#updateWithTime(double,Rotation2d,Object)} then you must use a timestamp with
+   *     an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as {@link
+   *     edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you should use {@link
+   *     edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or sync the epochs.
+   */
+  public void addVisionMeasurement(Pose2d visionRobotPose, double timestamp) {
+    // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
+    if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()
+        || m_odometryPoseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestamp) {
+      return;
+    }
+
+    // Step 1: Clean up any old entries
+    cleanUpVisionUpdates();
+
+    // Step 2: Get the pose measured by odometry at the moment the vision measurement was made.
+    var odometrySample = m_odometryPoseBuffer.getSample(timestamp);
+
+    if (odometrySample.isEmpty()) {
+      return;
+    }
+
+    // Step 3: Get the vision-compensated pose estimate at the moment the vision measurement was
+    // made.
+    var visionSample = sampleAt(timestamp);
+
+    if (visionSample.isEmpty()) {
+      return;
+    }
+
+    // Step 4: Measure the twist between the old pose estimate and the vision pose.
+    var twist = visionRobotPose.minus(visionSample.get()).log();
+
+    // Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman
+    // gain matrix representing how much we trust vision measurements compared to our current pose.
+    var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
+
+    // Step 6: Convert back to Twist2d.
+    var scaledTwist =
+        new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
+
+    // Step 7: Calculate and record the vision update.
+    var visionUpdate =
+        new VisionUpdate(visionSample.get().plus(scaledTwist.exp()), odometrySample.get());
+    m_visionUpdates.put(timestamp, visionUpdate);
+
+    // Step 8: Remove later vision measurements. (Matches previous behavior)
+    m_visionUpdates.tailMap(timestamp, false).entrySet().clear();
+
+    // Step 9: Update latest pose estimate. Since we cleared all updates after this vision update,
+    // it's guaranteed to be the latest vision update.
+    m_poseEstimate = visionUpdate.compensate(m_odometry.getPose());
+  }
+
+  /**
+   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+   * while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+   * PoseEstimator#update} every loop.
+   *
+   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
+   * recommend only adding vision measurements that are already within one meter or so of the
+   * current pose estimate.
+   *
+   * <p>Note that the vision measurement standard deviations passed into this method will continue
+   * to apply to future measurements until a subsequent call to {@link
+   * PoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
+   *
+   * @param visionRobotPose The pose of the robot as measured by the vision camera.
+   * @param timestamp The timestamp of the vision measurement in seconds. Note that if you don't use
+   *     your own time source by calling {@link #updateWithTime}, then you must use a timestamp with
+   *     an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as {@link
+   *     edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you should use {@link
+   *     edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in this case.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
+   *     the vision pose measurement less.
+   */
+  public void addVisionMeasurement(
+      Pose2d visionRobotPose, double timestamp, Matrix<N3, N1> visionMeasurementStdDevs) {
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    addVisionMeasurement(visionRobotPose, timestamp);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+   * loop.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The current encoder readings.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
+    return updateWithTime(MathSharedStore.getTimestamp(), gyroAngle, wheelPositions);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+   * loop.
+   *
+   * @param currentTime Time at which this method was called, in seconds.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The current encoder readings.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose2d updateWithTime(double currentTime, Rotation2d gyroAngle, T wheelPositions) {
+    var odometryEstimate = m_odometry.update(gyroAngle, wheelPositions);
+
+    m_odometryPoseBuffer.addSample(currentTime, odometryEstimate);
+
+    if (m_visionUpdates.isEmpty()) {
+      m_poseEstimate = odometryEstimate;
+    } else {
+      var visionUpdate = m_visionUpdates.get(m_visionUpdates.lastKey());
+      m_poseEstimate = visionUpdate.compensate(odometryEstimate);
+    }
+
+    return getEstimatedPosition();
+  }
+
+  /**
+   * Represents a vision update record. The record contains the vision-compensated pose estimate as
+   * well as the corresponding odometry pose estimate.
+   */
+  private static final class VisionUpdate {
+    // The vision-compensated pose estimate.
+    private final Pose2d visionPose;
+
+    // The pose estimated based solely on odometry.
+    private final Pose2d odometryPose;
+
+    /**
+     * Constructs a vision update record with the specified parameters.
+     *
+     * @param visionPose The vision-compensated pose estimate.
+     * @param odometryPose The pose estimate based solely on odometry.
+     */
+    private VisionUpdate(Pose2d visionPose, Pose2d odometryPose) {
+      this.visionPose = visionPose;
+      this.odometryPose = odometryPose;
+    }
+
+    /**
+     * Returns the vision-compensated version of the pose. Specifically, changes the pose from being
+     * relative to this record's odometry pose to being relative to this record's vision pose.
+     *
+     * @param pose The pose to compensate.
+     * @return The compensated pose.
+     */
+    public Pose2d compensate(Pose2d pose) {
+      var delta = pose.minus(this.odometryPose);
+      return this.visionPose.plus(delta);
+    }
+  }
+}

wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator3d.java

@@ -0,0 +1,419 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.geometry.Twist3d;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
+import edu.wpi.first.math.kinematics.Kinematics;
+import edu.wpi.first.math.kinematics.Odometry3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N4;
+import edu.wpi.first.math.numbers.N6;
+import java.util.NavigableMap;
+import java.util.Optional;
+import java.util.TreeMap;
+
+/**
+ * This class wraps {@link Odometry3d} to fuse latency-compensated vision measurements with encoder
+ * measurements. Robot code should not use this directly- Instead, use the particular type for your
+ * drivetrain (e.g., {@link DifferentialDrivePoseEstimator3d}). It is intended to be a drop-in
+ * replacement for {@link Odometry3d}; in fact, if you never call {@link
+ * PoseEstimator3d#addVisionMeasurement} and only call {@link PoseEstimator3d#update} then this will
+ * behave exactly the same as Odometry3d. It is also intended to be an easy replacement for {@link
+ * PoseEstimator}, only requiring the addition of a standard deviation for Z and appropriate
+ * conversions between 2D and 3D versions of geometry classes. (See {@link Pose3d#Pose3d(Pose2d)},
+ * {@link Rotation3d#Rotation3d(Rotation2d)}, {@link Translation3d#Translation3d(Translation2d)},
+ * and {@link Pose3d#toPose2d()}.)
+ *
+ * <p>{@link PoseEstimator3d#update} should be called every robot loop.
+ *
+ * <p>{@link PoseEstimator3d#addVisionMeasurement} can be called as infrequently as you want; if you
+ * never call it then this class will behave exactly like regular encoder odometry.
+ *
+ * @param <T> Wheel positions type.
+ */
+public class PoseEstimator3d<T> {
+  private final Odometry3d<T> m_odometry;
+  private final Matrix<N4, N1> m_q = new Matrix<>(Nat.N4(), Nat.N1());
+  private final Matrix<N6, N6> m_visionK = new Matrix<>(Nat.N6(), Nat.N6());
+
+  private static final double kBufferDuration = 1.5;
+  // Maps timestamps to odometry-only pose estimates
+  private final TimeInterpolatableBuffer<Pose3d> m_odometryPoseBuffer =
+      TimeInterpolatableBuffer.createBuffer(kBufferDuration);
+  // Maps timestamps to vision updates
+  // Always contains one entry before the oldest entry in m_odometryPoseBuffer, unless there have
+  // been no vision measurements after the last reset
+  private final NavigableMap<Double, VisionUpdate> m_visionUpdates = new TreeMap<>();
+
+  private Pose3d m_poseEstimate;
+
+  /**
+   * Constructs a PoseEstimator3d.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param odometry A correctly-configured odometry object for your drivetrain.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
+   *     in meters, z position in meters, and angle in radians). Increase these numbers to trust
+   *     your state estimate less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+   *     in meters, y position in meters, z position in meters, and angle in radians). Increase
+   *     these numbers to trust the vision pose measurement less.
+   */
+  @SuppressWarnings("PMD.UnusedFormalParameter")
+  public PoseEstimator3d(
+      Kinematics<?, T> kinematics,
+      Odometry3d<T> odometry,
+      Matrix<N4, N1> stateStdDevs,
+      Matrix<N4, N1> visionMeasurementStdDevs) {
+    m_odometry = odometry;
+
+    m_poseEstimate = m_odometry.getPose();
+
+    for (int i = 0; i < 4; ++i) {
+      m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
+    }
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+  }
+
+  /**
+   * Sets the pose estimator's trust of global measurements. This might be used to change trust in
+   * vision measurements after the autonomous period, or to change trust as distance to a vision
+   * target increases.
+   *
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y, z,
+   *     theta]ᵀ, with units in meters and radians.
+   */
+  public final void setVisionMeasurementStdDevs(Matrix<N4, N1> visionMeasurementStdDevs) {
+    var r = new double[4];
+    for (int i = 0; i < 4; ++i) {
+      r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
+    }
+
+    // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+    // and C = I. See wpimath/algorithms.md.
+    for (int row = 0; row < 4; ++row) {
+      if (m_q.get(row, 0) == 0.0) {
+        m_visionK.set(row, row, 0.0);
+      } else {
+        m_visionK.set(
+            row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
+      }
+    }
+    // Fill in the gains for the other components of the rotation vector
+    double angle_gain = m_visionK.get(3, 3);
+    m_visionK.set(4, 4, angle_gain);
+    m_visionK.set(5, 5, angle_gain);
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
+   * automatically takes care of offsetting the gyro angle.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current encoder readings.
+   * @param pose The position on the field that your robot is at.
+   */
+  public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d pose) {
+    // Reset state estimate and error covariance
+    m_odometry.resetPosition(gyroAngle, wheelPositions, pose);
+    m_odometryPoseBuffer.clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.getPose();
+  }
+
+  /**
+   * Resets the robot's pose.
+   *
+   * @param pose The pose to reset to.
+   */
+  public void resetPose(Pose3d pose) {
+    m_odometry.resetPose(pose);
+    m_odometryPoseBuffer.clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.getPose();
+  }
+
+  /**
+   * Resets the robot's translation.
+   *
+   * @param translation The pose to translation to.
+   */
+  public void resetTranslation(Translation3d translation) {
+    m_odometry.resetTranslation(translation);
+    m_odometryPoseBuffer.clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.getPose();
+  }
+
+  /**
+   * Resets the robot's rotation.
+   *
+   * @param rotation The rotation to reset to.
+   */
+  public void resetRotation(Rotation3d rotation) {
+    m_odometry.resetRotation(rotation);
+    m_odometryPoseBuffer.clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.getPose();
+  }
+
+  /**
+   * Gets the estimated robot pose.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  public Pose3d getEstimatedPosition() {
+    return m_poseEstimate;
+  }
+
+  /**
+   * Return the pose at a given timestamp, if the buffer is not empty.
+   *
+   * @param timestamp The pose's timestamp in seconds.
+   * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
+   */
+  public Optional<Pose3d> sampleAt(double timestamp) {
+    // Step 0: If there are no odometry updates to sample, skip.
+    if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()) {
+      return Optional.empty();
+    }
+
+    // Step 1: Make sure timestamp matches the sample from the odometry pose buffer. (When sampling,
+    // the buffer will always use a timestamp between the first and last timestamps)
+    double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey();
+    double newestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().lastKey();
+    timestamp = Math.clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp);
+
+    // Step 2: If there are no applicable vision updates, use the odometry-only information.
+    if (m_visionUpdates.isEmpty() || timestamp < m_visionUpdates.firstKey()) {
+      return m_odometryPoseBuffer.getSample(timestamp);
+    }
+
+    // Step 3: Get the latest vision update from before or at the timestamp to sample at.
+    double floorTimestamp = m_visionUpdates.floorKey(timestamp);
+    var visionUpdate = m_visionUpdates.get(floorTimestamp);
+
+    // Step 4: Get the pose measured by odometry at the time of the sample.
+    var odometryEstimate = m_odometryPoseBuffer.getSample(timestamp);
+
+    // Step 5: Apply the vision compensation to the odometry pose.
+    return odometryEstimate.map(odometryPose -> visionUpdate.compensate(odometryPose));
+  }
+
+  /** Removes stale vision updates that won't affect sampling. */
+  private void cleanUpVisionUpdates() {
+    // Step 0: If there are no odometry samples, skip.
+    if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()) {
+      return;
+    }
+
+    // Step 1: Find the oldest timestamp that needs a vision update.
+    double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey();
+
+    // Step 2: If there are no vision updates before that timestamp, skip.
+    if (m_visionUpdates.isEmpty() || oldestOdometryTimestamp < m_visionUpdates.firstKey()) {
+      return;
+    }
+
+    // Step 3: Find the newest vision update timestamp before or at the oldest timestamp.
+    double newestNeededVisionUpdateTimestamp = m_visionUpdates.floorKey(oldestOdometryTimestamp);
+
+    // Step 4: Remove all entries strictly before the newest timestamp we need.
+    m_visionUpdates.headMap(newestNeededVisionUpdateTimestamp, false).clear();
+  }
+
+  /**
+   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+   * while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+   * PoseEstimator3d#update} every loop.
+   *
+   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
+   * recommend only adding vision measurements that are already within one meter or so of the
+   * current pose estimate.
+   *
+   * @param visionRobotPose The pose of the robot as measured by the vision camera.
+   * @param timestamp The timestamp of the vision measurement in seconds. Note that if you don't use
+   *     your own time source by calling {@link
+   *     PoseEstimator3d#updateWithTime(double,Rotation3d,Object)} then you must use a timestamp
+   *     with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as
+   *     {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you should use
+   *     {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or sync the
+   *     epochs.
+   */
+  public void addVisionMeasurement(Pose3d visionRobotPose, double timestamp) {
+    // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
+    if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()
+        || m_odometryPoseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestamp) {
+      return;
+    }
+
+    // Step 1: Clean up any old entries
+    cleanUpVisionUpdates();
+
+    // Step 2: Get the pose measured by odometry at the moment the vision measurement was made.
+    var odometrySample = m_odometryPoseBuffer.getSample(timestamp);
+
+    if (odometrySample.isEmpty()) {
+      return;
+    }
+
+    // Step 3: Get the vision-compensated pose estimate at the moment the vision measurement was
+    // made.
+    var visionSample = sampleAt(timestamp);
+
+    if (visionSample.isEmpty()) {
+      return;
+    }
+
+    // Step 4: Measure the twist between the old pose estimate and the vision pose.
+    var twist = visionRobotPose.minus(visionSample.get()).log();
+
+    // Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman
+    // gain matrix representing how much we trust vision measurements compared to our current pose.
+    var k_times_twist =
+        m_visionK.times(
+            VecBuilder.fill(twist.dx, twist.dy, twist.dz, twist.rx, twist.ry, twist.rz));
+
+    // Step 6: Convert back to Twist3d.
+    var scaledTwist =
+        new Twist3d(
+            k_times_twist.get(0, 0),
+            k_times_twist.get(1, 0),
+            k_times_twist.get(2, 0),
+            k_times_twist.get(3, 0),
+            k_times_twist.get(4, 0),
+            k_times_twist.get(5, 0));
+
+    // Step 7: Calculate and record the vision update.
+    var visionUpdate =
+        new VisionUpdate(visionSample.get().plus(scaledTwist.exp()), odometrySample.get());
+    m_visionUpdates.put(timestamp, visionUpdate);
+
+    // Step 8: Remove later vision measurements. (Matches previous behavior)
+    m_visionUpdates.tailMap(timestamp, false).entrySet().clear();
+
+    // Step 9: Update latest pose estimate. Since we cleared all updates after this vision update,
+    // it's guaranteed to be the latest vision update.
+    m_poseEstimate = visionUpdate.compensate(m_odometry.getPose());
+  }
+
+  /**
+   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+   * while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+   * PoseEstimator3d#update} every loop.
+   *
+   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
+   * recommend only adding vision measurements that are already within one meter or so of the
+   * current pose estimate.
+   *
+   * <p>Note that the vision measurement standard deviations passed into this method will continue
+   * to apply to future measurements until a subsequent call to {@link
+   * PoseEstimator3d#setVisionMeasurementStdDevs(Matrix)} or this method.
+   *
+   * @param visionRobotPose The pose of the robot as measured by the vision camera.
+   * @param timestamp The timestamp of the vision measurement in seconds. Note that if you don't use
+   *     your own time source by calling {@link #updateWithTime}, then you must use a timestamp with
+   *     an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as {@link
+   *     edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you should use {@link
+   *     edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in this case.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+   *     in meters, y position in meters, z position in meters, and angle in radians). Increase
+   *     these numbers to trust the vision pose measurement less.
+   */
+  public void addVisionMeasurement(
+      Pose3d visionRobotPose, double timestamp, Matrix<N4, N1> visionMeasurementStdDevs) {
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    addVisionMeasurement(visionRobotPose, timestamp);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+   * loop.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The current encoder readings.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose3d update(Rotation3d gyroAngle, T wheelPositions) {
+    return updateWithTime(MathSharedStore.getTimestamp(), gyroAngle, wheelPositions);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+   * loop.
+   *
+   * @param currentTime Time at which this method was called, in seconds.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The current encoder readings.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose3d updateWithTime(double currentTime, Rotation3d gyroAngle, T wheelPositions) {
+    var odometryEstimate = m_odometry.update(gyroAngle, wheelPositions);
+
+    m_odometryPoseBuffer.addSample(currentTime, odometryEstimate);
+
+    if (m_visionUpdates.isEmpty()) {
+      m_poseEstimate = odometryEstimate;
+    } else {
+      var visionUpdate = m_visionUpdates.get(m_visionUpdates.lastKey());
+      m_poseEstimate = visionUpdate.compensate(odometryEstimate);
+    }
+
+    return getEstimatedPosition();
+  }
+
+  /**
+   * Represents a vision update record. The record contains the vision-compensated pose estimate as
+   * well as the corresponding odometry pose estimate.
+   */
+  private static final class VisionUpdate {
+    // The vision-compensated pose estimate.
+    private final Pose3d visionPose;
+
+    // The pose estimated based solely on odometry.
+    private final Pose3d odometryPose;
+
+    /**
+     * Constructs a vision update record with the specified parameters.
+     *
+     * @param visionPose The vision-compensated pose estimate.
+     * @param odometryPose The pose estimate based solely on odometry.
+     */
+    private VisionUpdate(Pose3d visionPose, Pose3d odometryPose) {
+      this.visionPose = visionPose;
+      this.odometryPose = odometryPose;
+    }
+
+    /**
+     * Returns the vision-compensated version of the pose. Specifically, changes the pose from being
+     * relative to this record's odometry pose to being relative to this record's vision pose.
+     *
+     * @param pose The pose to compensate.
+     * @return The compensated pose.
+     */
+    public Pose3d compensate(Pose3d pose) {
+      var delta = pose.minus(this.odometryPose);
+      return this.visionPose.plus(delta);
+    }
+  }
+}

wpimath/src/main/java/org/wpilib/math/estimator/S3SigmaPoints.java

@@ -0,0 +1,172 @@
+// 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 Papakonstantinou's paper[1] for the
+ * UnscentedKalmanFilter class.
+ *
+ * <p>It parameterizes the sigma points using alpha and beta terms. Unless you know better, this
+ * should be your default choice due to its high accuracy and performance.
+ *
+ * <p>[1] K. Papakonstantinou "A Scaled Spherical Simplex Filter (S3F) with a decreased n + 2 sigma
+ * points set size and equivalent 2n + 1 Unscented Kalman Filter (UKF) accuracy"
+ *
+ * @param <States> The dimenstionality of the state. States + 2 weights will be generated.
+ */
+public class S3SigmaPoints<States extends Num> implements SigmaPoints<States> {
+  private final Nat<States> m_states;
+  private final double m_alpha;
+  private Matrix<?, N1> m_wm;
+  private Matrix<?, N1> m_wc;
+
+  /**
+   * Constructs a generator for Papakonstantinou 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.
+   */
+  @SuppressWarnings("this-escape")
+  public S3SigmaPoints(Nat<States> states, double alpha, double beta) {
+    m_states = states;
+    m_alpha = alpha;
+
+    computeWeights(beta);
+  }
+
+  /**
+   * Constructs a generator for Papakonstantinou sigma points with default values for alpha, beta,
+   * and kappa.
+   *
+   * @param states an instance of Num that represents the number of states.
+   */
+  public S3SigmaPoints(Nat<States> states) {
+    this(states, 1e-3, 2);
+  }
+
+  /**
+   * 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.
+   */
+  @Override
+  public int getNumSigmas() {
+    return m_states.getNum() + 2;
+  }
+
+  /**
+   * Computes the sigma points for an unscented Kalman filter given the mean (x) and square-root
+   * covariance (s) of the filter.
+   *
+   * @param x An array of the means.
+   * @param s Square-root covariance of the filter.
+   * @return Two-dimensional array of sigma points. Each column contains all the sigmas for one
+   *     dimension in the problem space. Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
+   */
+  @Override
+  public Matrix<States, ?> squareRootSigmaPoints(Matrix<States, N1> x, Matrix<States, States> s) {
+    // table (1), equation (12)
+    double[] q = new double[m_states.getNum()];
+    for (int t = 1; t <= m_states.getNum(); ++t) {
+      q[t - 1] = m_alpha * Math.sqrt(t * (m_states.getNum() + 1) / (double) (t + 1));
+    }
+
+    Matrix<States, ?> C = new Matrix<>(new SimpleMatrix(m_states.getNum(), getNumSigmas()));
+    for (int row = 0; row < m_states.getNum(); ++row) {
+      C.set(row, 0, 0.0);
+    }
+    for (int col = 1; col < getNumSigmas(); ++col) {
+      for (int row = 0; row < m_states.getNum(); ++row) {
+        if (row < col - 2) {
+          C.set(row, col, 0.0);
+        } else if (row == col - 2) {
+          C.set(row, col, q[row]);
+        } else {
+          C.set(row, col, -q[row] / (row + 1));
+        }
+      }
+    }
+
+    Matrix<States, ?> sigmas = new Matrix<>(new SimpleMatrix(m_states.getNum(), getNumSigmas()));
+    for (int col = 0; col < getNumSigmas(); ++col) {
+      sigmas.setColumn(col, x.plus(s.times(C.extractColumnVector(col))));
+    }
+
+    return sigmas;
+  }
+
+  /**
+   * Computes the weights for the scaled unscented Kalman filter.
+   *
+   * @param beta Incorporates prior knowledge of the distribution of the mean.
+   */
+  private void computeWeights(double beta) {
+    double alpha_sq = m_alpha * m_alpha;
+
+    double c = 1.0 / (alpha_sq * (m_states.getNum() + 1));
+
+    Matrix<?, N1> wM = new Matrix<>(new SimpleMatrix(getNumSigmas(), 1));
+    Matrix<?, N1> wC = new Matrix<>(new SimpleMatrix(getNumSigmas(), 1));
+    wM.fill(c);
+    wC.fill(c);
+
+    wM.set(0, 0, 1.0 - 1.0 / alpha_sq);
+    wC.set(0, 0, 1.0 - 1.0 / alpha_sq + (1 - alpha_sq + 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.
+   */
+  @Override
+  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.
+   */
+  @Override
+  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.
+   */
+  @Override
+  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.
+   */
+  @Override
+  public double getWc(int element) {
+    return m_wc.get(element, 0);
+  }
+}

wpimath/src/main/java/org/wpilib/math/estimator/S3UKF.java

@@ -0,0 +1,117 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.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.function.BiFunction;
+
+/**
+ * An Unscented Kalman Filter using sigma points and weights from Papakonstantinou's paper. This is
+ * generally preferred over other UKF variants due to its high accuracy and performance.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
+ */
+public class S3UKF<States extends Num, Inputs extends Num, Outputs extends Num>
+    extends UnscentedKalmanFilter<States, Inputs, Outputs> {
+  /**
+   * Constructs a Scaled Spherical Simplex (S3) Unscented Kalman Filter.
+   *
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
+   * for how to select the standard deviations.
+   *
+   * @param states A Nat representing the number of states.
+   * @param outputs A Nat representing the number of outputs.
+   * @param f A vector-valued function of x and u that returns the derivative of the state vector.
+   * @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 nominalDt Nominal discretization timestep in seconds.
+   */
+  public S3UKF(
+      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 nominalDt) {
+    super(
+        new S3SigmaPoints<>(states),
+        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,
+        nominalDt);
+  }
+
+  /**
+   * Constructs a Scaled Spherical Simplex (S3) 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.
+   *
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
+   * for how to select the standard deviations.
+   *
+   * @param states A Nat representing the number of states.
+   * @param outputs A Nat representing the number of outputs.
+   * @param f A vector-valued function of x and u that returns the derivative of the state vector.
+   * @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 NumSigmas state vectors using a given set
+   *     of weights.
+   * @param meanFuncY A function that computes the mean of NumSigmas 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 nominalDt Nominal discretization timestep in seconds.
+   */
+  public S3UKF(
+      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 nominalDt) {
+    super(
+        new S3SigmaPoints<>(states),
+        states,
+        outputs,
+        f,
+        h,
+        stateStdDevs,
+        measurementStdDevs,
+        meanFuncX,
+        meanFuncY,
+        residualFuncX,
+        residualFuncY,
+        addFuncX,
+        nominalDt);
+  }
+}

wpimath/src/main/java/org/wpilib/math/estimator/SigmaPoints.java

@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+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;
+
+/**
+ * A sigma points generator for the UnscentedKalmanFilter class.
+ *
+ * @param <States> The dimensionality of the state.
+ */
+public interface SigmaPoints<States extends Num> {
+  /**
+   * 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.
+   */
+  int getNumSigmas();
+
+  /**
+   * Computes the sigma points for an unscented Kalman filter given the mean (x) and square-root
+   * covariance (s) of the filter.
+   *
+   * @param x An array of the means.
+   * @param s Square-root covariance of the filter.
+   * @return Two-dimensional array of sigma points. Each column contains all the sigmas for one
+   *     dimension in the problem space. Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
+   */
+  Matrix<States, ?> squareRootSigmaPoints(Matrix<States, N1> x, Matrix<States, States> s);
+
+  /**
+   * Returns the weight for each sigma point for the mean.
+   *
+   * @return the weight for each sigma point for the mean.
+   */
+  Matrix<?, N1> getWm();
+
+  /**
+   * 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.
+   */
+  double getWm(int element);
+
+  /**
+   * Returns the weight for each sigma point for the covariance.
+   *
+   * @return the weight for each sigma point for the covariance.
+   */
+  Matrix<?, N1> getWc();
+
+  /**
+   * 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.
+   */
+  double getWc(int element);
+}

wpimath/src/main/java/org/wpilib/math/estimator/SteadyStateKalmanFilter.java

@@ -0,0 +1,198 @@
+// 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.DARE;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.Discretization;
+import edu.wpi.first.math.system.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>This class assumes predict() and correct() are called in pairs, so the Kalman gain converges
+ * to a steady-state value. If they aren't, use {@link KalmanFilter} instead.
+ *
+ * <p>For more on the underlying math, read <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
+ * chapter 9 "Stochastic control theory".
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
+ */
+public class SteadyStateKalmanFilter<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. */
+  private final Matrix<States, Outputs> m_K;
+
+  /** The state estimate. */
+  private Matrix<States, N1> m_xHat;
+
+  /**
+   * Constructs a steady-state Kalman filter with the given plant.
+   *
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
+   * for how to select the standard deviations.
+   *
+   * @param states A Nat representing the states of the system.
+   * @param outputs A Nat representing the outputs of the system.
+   * @param plant The plant used for the prediction step.
+   * @param stateStdDevs Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dt Nominal discretization timestep in seconds.
+   * @throws IllegalArgumentException If the system is undetectable.
+   */
+  public SteadyStateKalmanFilter(
+      Nat<States> states,
+      Nat<Outputs> outputs,
+      LinearSystem<States, Inputs, Outputs> plant,
+      Matrix<States, N1> stateStdDevs,
+      Matrix<Outputs, N1> measurementStdDevs,
+      double dt) {
+    this.m_states = states;
+
+    this.m_plant = plant;
+
+    var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
+    var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+
+    var pair = Discretization.discretizeAQ(plant.getA(), contQ, dt);
+    var discA = pair.getFirst();
+    var discQ = pair.getSecond();
+
+    var discR = Discretization.discretizeR(contR, dt);
+
+    var C = plant.getC();
+
+    var P = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR));
+
+    // S = CPCᵀ + R
+    var S = C.times(P).times(C.transpose()).plus(discR);
+
+    // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
+    // efficiently.
+    //
+    // K = PCᵀS⁻¹
+    // KS = PCᵀ
+    // (KS)ᵀ = (PCᵀ)ᵀ
+    // SᵀKᵀ = CPᵀ
+    //
+    // The solution of Ax = b can be found via x = A.solve(b).
+    //
+    // Kᵀ = Sᵀ.solve(CPᵀ)
+    // K = (Sᵀ.solve(CPᵀ))ᵀ
+    //
+    // Drop the transposes on symmetric matrices S and P.
+    //
+    // K = (S.solve(CP))ᵀ
+    m_K = new Matrix<>(S.getStorage().solve(C.times(P).getStorage()).transpose());
+
+    reset();
+  }
+
+  /** Resets the observer. */
+  public final 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 that row.
+   */
+  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 dt Timestep for prediction in seconds.
+   */
+  public void predict(Matrix<Inputs, N1> u, double dt) {
+    this.m_xHat = m_plant.calculateX(m_xHat, u, dt);
+  }
+
+  /**
+   * 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.
+   */
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
+    final var C = m_plant.getC();
+    final var D = m_plant.getD();
+
+    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
+    m_xHat = m_xHat.plus(m_K.times(y.minus(C.times(m_xHat).plus(D.times(u)))));
+  }
+}

wpimath/src/main/java/org/wpilib/math/estimator/SwerveDrivePoseEstimator.java

@@ -0,0 +1,98 @@
+// 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.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+
+/**
+ * This class wraps {@link SwerveDriveOdometry Swerve Drive Odometry} to fuse latency-compensated
+ * vision measurements with swerve drive encoder distance measurements. It is intended to be a
+ * drop-in replacement for {@link SwerveDriveOdometry}.
+ *
+ * <p>{@link SwerveDrivePoseEstimator#update} should be called every robot loop.
+ *
+ * <p>{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
+ * want; if you never call it, then this class will behave as regular encoder odometry.
+ */
+public class SwerveDrivePoseEstimator extends PoseEstimator<SwerveModulePosition[]> {
+  private final int m_numModules;
+
+  /**
+   * Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision
+   * measurements.
+   *
+   * <p>The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
+   * and 0.1 radians for heading. The default standard deviations of the vision measurements are 0.9
+   * meters for x, 0.9 meters for y, and 0.9 radians for heading.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param modulePositions The current distance measurements and rotations of the swerve modules.
+   * @param initialPose The starting pose estimate.
+   */
+  public SwerveDrivePoseEstimator(
+      SwerveDriveKinematics kinematics,
+      Rotation2d gyroAngle,
+      SwerveModulePosition[] modulePositions,
+      Pose2d initialPose) {
+    this(
+        kinematics,
+        gyroAngle,
+        modulePositions,
+        initialPose,
+        VecBuilder.fill(0.1, 0.1, 0.1),
+        VecBuilder.fill(0.9, 0.9, 0.9));
+  }
+
+  /**
+   * Constructs a SwerveDrivePoseEstimator.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param modulePositions The current distance and rotation measurements of the swerve modules.
+   * @param initialPose The starting pose estimate.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
+   *     in meters, and heading in radians). Increase these numbers to trust your state estimate
+   *     less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
+   *     the vision pose measurement less.
+   */
+  public SwerveDrivePoseEstimator(
+      SwerveDriveKinematics kinematics,
+      Rotation2d gyroAngle,
+      SwerveModulePosition[] modulePositions,
+      Pose2d initialPose,
+      Matrix<N3, N1> stateStdDevs,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    super(
+        kinematics,
+        new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPose),
+        stateStdDevs,
+        visionMeasurementStdDevs);
+
+    m_numModules = modulePositions.length;
+  }
+
+  @Override
+  public Pose2d updateWithTime(
+      double currentTime, Rotation2d gyroAngle, SwerveModulePosition[] wheelPositions) {
+    if (wheelPositions.length != m_numModules) {
+      throw new IllegalArgumentException(
+          "Number of modules is not consistent with number of wheel locations provided in "
+              + "constructor");
+    }
+
+    return super.updateWithTime(currentTime, gyroAngle, wheelPositions);
+  }
+}

wpimath/src/main/java/org/wpilib/math/estimator/SwerveDrivePoseEstimator3d.java

@@ -0,0 +1,107 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry3d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N4;
+
+/**
+ * This class wraps {@link SwerveDriveOdometry3d Swerve Drive Odometry} to fuse latency-compensated
+ * vision measurements with swerve drive encoder distance measurements. It is intended to be a
+ * drop-in replacement for {@link SwerveDriveOdometry3d}. It is also intended to be an easy
+ * replacement for {@link SwerveDrivePoseEstimator}, only requiring the addition of a standard
+ * deviation for Z and appropriate conversions between 2D and 3D versions of geometry classes. (See
+ * {@link Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
+ * Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
+ *
+ * <p>{@link SwerveDrivePoseEstimator3d#update} should be called every robot loop.
+ *
+ * <p>{@link SwerveDrivePoseEstimator3d#addVisionMeasurement} can be called as infrequently as you
+ * want; if you never call it, then this class will behave as regular encoder odometry.
+ */
+public class SwerveDrivePoseEstimator3d extends PoseEstimator3d<SwerveModulePosition[]> {
+  private final int m_numModules;
+
+  /**
+   * Constructs a SwerveDrivePoseEstimator3d with default standard deviations for the model and
+   * vision measurements.
+   *
+   * <p>The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
+   * 0.1 meters for z, and 0.1 radians for angle. The default standard deviations of the vision
+   * measurements are 0.9 meters for x, 0.9 meters for y, 0.9 meters for z, and 0.9 radians for
+   * angle.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param modulePositions The current distance measurements and rotations of the swerve modules.
+   * @param initialPose The starting pose estimate.
+   */
+  public SwerveDrivePoseEstimator3d(
+      SwerveDriveKinematics kinematics,
+      Rotation3d gyroAngle,
+      SwerveModulePosition[] modulePositions,
+      Pose3d initialPose) {
+    this(
+        kinematics,
+        gyroAngle,
+        modulePositions,
+        initialPose,
+        VecBuilder.fill(0.1, 0.1, 0.1, 0.1),
+        VecBuilder.fill(0.9, 0.9, 0.9, 0.9));
+  }
+
+  /**
+   * Constructs a SwerveDrivePoseEstimator3d.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param modulePositions The current distance and rotation measurements of the swerve modules.
+   * @param initialPose The starting pose estimate.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
+   *     in meters, and heading in radians). Increase these numbers to trust your state estimate
+   *     less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
+   *     the vision pose measurement less.
+   */
+  public SwerveDrivePoseEstimator3d(
+      SwerveDriveKinematics kinematics,
+      Rotation3d gyroAngle,
+      SwerveModulePosition[] modulePositions,
+      Pose3d initialPose,
+      Matrix<N4, N1> stateStdDevs,
+      Matrix<N4, N1> visionMeasurementStdDevs) {
+    super(
+        kinematics,
+        new SwerveDriveOdometry3d(kinematics, gyroAngle, modulePositions, initialPose),
+        stateStdDevs,
+        visionMeasurementStdDevs);
+
+    m_numModules = modulePositions.length;
+  }
+
+  @Override
+  public Pose3d updateWithTime(
+      double currentTime, Rotation3d gyroAngle, SwerveModulePosition[] wheelPositions) {
+    if (wheelPositions.length != m_numModules) {
+      throw new IllegalArgumentException(
+          "Number of modules is not consistent with number of wheel locations provided in "
+              + "constructor");
+    }
+
+    return super.updateWithTime(currentTime, gyroAngle, wheelPositions);
+  }
+}

wpimath/src/main/java/org/wpilib/math/estimator/UnscentedKalmanFilter.java

@@ -0,0 +1,599 @@
+// 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.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.Discretization;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.NumericalJacobian;
+import java.util.function.BiFunction;
+import org.ejml.dense.row.decomposition.qr.QRDecompositionHouseholder_DDRM;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * 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>This class's constructors require a SigmaPoints generator. For convenience, {@link S3UKF} and
+ * {@link MerweUKF} subclasses are provided to create a suitable generator for you. S3UKF is
+ * generally preferred over MerweUKF because of its greater performance while maintaining nearly
+ * identical accuracy.
+ *
+ * <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>An unscented Kalman filter uses nonlinear state and measurement models. It propagates the
+ * error covariance using sigma points chosen to approximate the true probability distribution.
+ *
+ * <p>For more on the underlying math, read <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
+ * chapter 9 "Stochastic control theory".
+ *
+ * <p>This class implements a square-root-form unscented Kalman filter (SR-UKF). The main reason for
+ * this is to guarantee that the covariance matrix remains positive definite. For more information
+ * about the SR-UKF, see https://www.researchgate.net/publication/3908304.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
+ */
+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_S;
+  private final Matrix<States, States> m_contQ;
+  private final Matrix<Outputs, Outputs> m_contR;
+  private Matrix<States, ?> m_sigmasF;
+  private double m_dt;
+
+  private final SigmaPoints<States> m_pts;
+
+  /**
+   * Constructs an Unscented Kalman Filter.
+   *
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
+   * for how to select the standard deviations.
+   *
+   * @param pts A sigma points and weights generator.
+   * @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 nominalDt Nominal discretization timestep in seconds.
+   */
+  public UnscentedKalmanFilter(
+      SigmaPoints<States> pts,
+      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 nominalDt) {
+    this(
+        pts,
+        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,
+        nominalDt);
+  }
+
+  /**
+   * 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.
+   *
+   * <p>See <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
+   * for how to select the standard deviations.
+   *
+   * @param pts A sigma points and weights generator.
+   * @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 NumSigmas state vectors using a given set
+   *     of weights.
+   * @param meanFuncY A function that computes the mean of NumSigmas 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 nominalDt Nominal discretization timestep in seconds.
+   */
+  public UnscentedKalmanFilter(
+      SigmaPoints<States> pts,
+      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 nominalDt) {
+    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_dt = nominalDt;
+
+    m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
+    m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+
+    m_pts = pts;
+
+    reset();
+  }
+
+  static <C extends Num> Pair<Matrix<C, N1>, Matrix<C, C>> squareRootUnscentedTransform(
+      Nat<C> covdim,
+      int numSigmas,
+      Matrix<C, ?> sigmas,
+      Matrix<?, N1> Wm,
+      Matrix<?, N1> Wc,
+      BiFunction<Matrix<C, ?>, Matrix<?, N1>, Matrix<C, N1>> meanFunc,
+      BiFunction<Matrix<C, N1>, Matrix<C, N1>, Matrix<C, N1>> residualFunc,
+      Matrix<C, C> squareRootR) {
+    if (sigmas.getNumRows() != covdim.getNum() || sigmas.getNumCols() != numSigmas) {
+      throw new IllegalArgumentException(
+          "Sigmas must be covDim by numSigmas! Got "
+              + sigmas.getNumRows()
+              + " by "
+              + sigmas.getNumCols());
+    }
+
+    if (Wm.getNumRows() != numSigmas || Wm.getNumCols() != 1) {
+      throw new IllegalArgumentException(
+          "Wm must be numSigmas by 1! Got " + Wm.getNumRows() + " by " + Wm.getNumCols());
+    }
+
+    if (Wc.getNumRows() != numSigmas || Wc.getNumCols() != 1) {
+      throw new IllegalArgumentException(
+          "Wc must be numSigmas by 1! Got " + Wc.getNumRows() + " by " + Wc.getNumCols());
+    }
+
+    // New mean is usually just the sum of the sigmas * weights:
+    //
+    //      2n
+    //   x̂ = Σ Wᵢ⁽ᵐ⁾𝒳ᵢ
+    //      i=0
+    //
+    // equations (19) and (23) in the paper show this,
+    // but we allow a custom function, usually for angle wrapping
+    Matrix<C, N1> x = meanFunc.apply(sigmas, Wm);
+
+    // Form an intermediate matrix S⁻ as:
+    //
+    //   [√{W₁⁽ᶜ⁾}(𝒳_{1:2L} - x̂) √{Rᵛ}]
+    //
+    // the part of equations (20) and (24) within the "qr{}"
+    //
+    // Note that we allow a custom function instead of the difference to allow
+    // angle wrapping. Furthermore, we allow an arbitrary number of sigma points to
+    // support similar methods such as the Scaled Spherical Simplex Filter (S3F).
+    Matrix<C, ?> Sbar =
+        new Matrix<>(new SimpleMatrix(covdim.getNum(), numSigmas - 1 + covdim.getNum()));
+    for (int i = 0; i < numSigmas - 1; i++) {
+      Sbar.setColumn(
+          i,
+          residualFunc.apply(sigmas.extractColumnVector(1 + i), x).times(Math.sqrt(Wc.get(1, 0))));
+    }
+    Sbar.assignBlock(0, numSigmas - 1, squareRootR);
+
+    QRDecompositionHouseholder_DDRM qr = new QRDecompositionHouseholder_DDRM();
+    var qrStorage = Sbar.transpose().getStorage();
+
+    if (!qr.decompose(qrStorage.getDDRM())) {
+      throw new RuntimeException("QR decomposition failed! Input matrix:\n" + qrStorage);
+    }
+
+    // Compute the square-root covariance of the sigma points
+    //
+    // We transpose S⁻ first because we formed it by horizontally
+    // concatenating each part; it should be vertical so we can take
+    // the QR decomposition as defined in the "QR Decomposition" passage
+    // of section 3. "EFFICIENT SQUARE-ROOT IMPLEMENTATION"
+    //
+    // The resulting matrix R is the square-root covariance S, but it
+    // is upper triangular, so we need to transpose it.
+    //
+    // equations (20) and (24)
+    Matrix<C, C> newS = new Matrix<>(new SimpleMatrix(qr.getR(null, true)).transpose());
+
+    // Update or downdate the square-root covariance with (𝒳₀-x̂)
+    // depending on whether its weight (W₀⁽ᶜ⁾) is positive or negative.
+    //
+    // equations (21) and (25)
+    newS.rankUpdate(residualFunc.apply(sigmas.extractColumnVector(0), x), Wc.get(0, 0), true);
+
+    return new Pair<>(x, newS);
+  }
+
+  /**
+   * Returns the square-root error covariance matrix S.
+   *
+   * @return the square-root error covariance matrix S.
+   */
+  public Matrix<States, States> getS() {
+    return m_S;
+  }
+
+  /**
+   * Returns an element of the square-root error covariance matrix S.
+   *
+   * @param row Row of S.
+   * @param col Column of S.
+   * @return the value of the square-root error covariance matrix S at (i, j).
+   */
+  public double getS(int row, int col) {
+    return m_S.get(row, col);
+  }
+
+  /**
+   * Sets the entire square-root error covariance matrix S.
+   *
+   * @param newS The new value of S to use.
+   */
+  public void setS(Matrix<States, States> newS) {
+    m_S = newS;
+  }
+
+  /**
+   * Returns the reconstructed error covariance matrix P.
+   *
+   * @return the error covariance matrix P.
+   */
+  @Override
+  public Matrix<States, States> getP() {
+    return m_S.times(m_S.transpose());
+  }
+
+  /**
+   * 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).
+   * @throws UnsupportedOperationException indexing into the reconstructed P matrix is not supported
+   */
+  @Override
+  public double getP(int row, int col) {
+    throw new UnsupportedOperationException(
+        "indexing into the reconstructed P matrix is not supported");
+  }
+
+  /**
+   * 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_S = newP.lltDecompose(true);
+  }
+
+  /**
+   * 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.
+   */
+  @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 final void reset() {
+    m_xHat = new Matrix<>(m_states, Nat.N1());
+    m_S = new Matrix<>(m_states, m_states);
+    m_sigmasF = new Matrix<>(new SimpleMatrix(m_states.getNum(), m_pts.getNumSigmas()));
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u New control input from controller.
+   * @param dt Timestep for prediction in seconds.
+   */
+  @Override
+  public void predict(Matrix<Inputs, N1> u, double dt) {
+    // 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.discretizeAQ(contA, m_contQ, dt).getSecond();
+    var squareRootDiscQ = discQ.lltDecompose(true);
+
+    // Generate sigma points around the state mean
+    //
+    // equation (17)
+    var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
+
+    // Project each sigma point forward in time according to the
+    // dynamics f(x, u)
+    //
+    //   sigmas  = 𝒳ₖ₋₁
+    //   sigmasF = 𝒳ₖ,ₖ₋₁ or just 𝒳 for readability
+    //
+    // equation (18)
+    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, dt));
+    }
+
+    // Pass the predicted sigmas (𝒳) through the Unscented Transform
+    // to compute the prior state mean and covariance
+    //
+    // equations (18) (19) and (20)
+    var ret =
+        squareRootUnscentedTransform(
+            m_states,
+            m_pts.getNumSigmas(),
+            m_sigmasF,
+            m_pts.getWm(),
+            m_pts.getWc(),
+            m_meanFuncX,
+            m_residualFuncX,
+            squareRootDiscQ);
+
+    m_xHat = ret.getFirst();
+    m_S = ret.getSecond();
+    m_dt = dt;
+  }
+
+  /**
+   * 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.
+   */
+  @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 measurement noise covariances vary.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y, Matrix<Outputs, Outputs> R) {
+    correct(m_outputs, u, y, m_h, R, m_meanFuncY, m_residualFuncY, m_residualFuncX, m_addFuncX);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
+   * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
+   * of this function).
+   *
+   * @param <R> Number of measurements in y.
+   * @param rows Number of rows in y.
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  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 <R> Number of measurements in y.
+   * @param rows Number of rows in y.
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   * @param meanFuncY A function that computes the mean of NumSigmas 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.
+   */
+  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_dt);
+    final var squareRootDiscR = discR.lltDecompose(true);
+
+    // Generate new sigma points from the prior mean and covariance
+    // and transform them into measurement space using h(x, u)
+    //
+    //   sigmas  = 𝒳
+    //   sigmasH = 𝒴
+    //
+    // This differs from equation (22) which uses
+    // the prior sigma points, regenerating them allows
+    // multiple measurement updates per time update
+    Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(rows.getNum(), m_pts.getNumSigmas()));
+    var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
+    for (int i = 0; i < m_pts.getNumSigmas(); i++) {
+      Matrix<R, N1> hRet = h.apply(sigmas.extractColumnVector(i), u);
+      sigmasH.setColumn(i, hRet);
+    }
+
+    // Pass the predicted measurement sigmas through the Unscented Transform
+    // to compute the mean predicted measurement and square-root innovation
+    // covariance.
+    //
+    // equations (23) (24) and (25)
+    var transRet =
+        squareRootUnscentedTransform(
+            rows,
+            m_pts.getNumSigmas(),
+            sigmasH,
+            m_pts.getWm(),
+            m_pts.getWc(),
+            meanFuncY,
+            residualFuncY,
+            squareRootDiscR);
+    var yHat = transRet.getFirst();
+    var Sy = transRet.getSecond();
+
+    // Compute cross covariance of the predicted state and measurement sigma
+    // points given as:
+    //
+    //           2n
+    //   P_{xy} = Σ Wᵢ⁽ᶜ⁾[𝒳ᵢ - x̂][𝒴ᵢ - ŷ⁻]ᵀ
+    //           i=0
+    //
+    // equation (26)
+    Matrix<States, R> Pxy = new Matrix<>(m_states, rows);
+    for (int i = 0; i < m_pts.getNumSigmas(); 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)));
+    }
+
+    // Compute the Kalman gain. We use Eigen's QR decomposition to solve. This
+    // is equivalent to MATLAB's \ operator, so we need to rearrange to use
+    // that.
+    //
+    //   K = (P_{xy} / S_{y}ᵀ) / S_{y}
+    //   K = (S_{y} \ P_{xy})ᵀ / S_{y}
+    //   K = (S_{y}ᵀ \ (S_{y} \ P_{xy}ᵀ))ᵀ
+    //
+    // equation (27)
+    Matrix<States, R> K =
+        Sy.transpose()
+            .solveFullPivHouseholderQr(Sy.solveFullPivHouseholderQr(Pxy.transpose()))
+            .transpose();
+
+    // Compute the posterior state mean
+    //
+    // x̂ = x̂⁻ + K(y − ŷ⁻)
+    //
+    // second part of equation (27)
+    m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, yHat)));
+
+    // Compute the intermediate matrix U for downdating
+    // the square-root covariance
+    //
+    // equation (28)
+    Matrix<States, R> U = K.times(Sy);
+
+    // Downdate the posterior square-root state covariance
+    //
+    // equation (29)
+    for (int i = 0; i < rows.getNum(); i++) {
+      m_S.rankUpdate(U.extractColumnVector(i), -1, true);
+    }
+  }
+}

wpimath/src/main/java/org/wpilib/math/filter/Debouncer.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.filter;
+
+import edu.wpi.first.math.MathSharedStore;
+
+/**
+ * A simple debounce filter for boolean streams. Requires that the boolean change value from
+ * baseline for a specified period of time before the filtered value changes.
+ */
+public class Debouncer {
+  /** Type of debouncing to perform. */
+  public enum DebounceType {
+    /** Rising edge. */
+    kRising,
+    /** Falling edge. */
+    kFalling,
+    /** Both rising and falling edges. */
+    kBoth
+  }
+
+  private double m_debounceTime;
+  private DebounceType m_debounceType;
+  private boolean m_baseline;
+
+  private double m_prevTime;
+
+  /**
+   * Creates a new Debouncer.
+   *
+   * @param debounceTime The number of seconds the value must change from baseline for the filtered
+   *     value to change.
+   * @param type Which type of state change the debouncing will be performed on.
+   */
+  public Debouncer(double debounceTime, DebounceType type) {
+    m_debounceTime = debounceTime;
+    m_debounceType = type;
+
+    resetTimer();
+
+    m_baseline = m_debounceType == DebounceType.kFalling;
+  }
+
+  /**
+   * Creates a new Debouncer. Baseline value defaulted to "false."
+   *
+   * @param debounceTime The number of seconds the value must change from baseline for the filtered
+   *     value to change.
+   */
+  public Debouncer(double debounceTime) {
+    this(debounceTime, DebounceType.kRising);
+  }
+
+  private void resetTimer() {
+    m_prevTime = MathSharedStore.getTimestamp();
+  }
+
+  private boolean hasElapsed() {
+    return MathSharedStore.getTimestamp() - m_prevTime >= m_debounceTime;
+  }
+
+  /**
+   * Applies the debouncer to the input stream.
+   *
+   * @param input The current value of the input stream.
+   * @return The debounced value of the input stream.
+   */
+  public boolean calculate(boolean input) {
+    if (input == m_baseline) {
+      resetTimer();
+    }
+
+    if (hasElapsed()) {
+      if (m_debounceType == DebounceType.kBoth) {
+        m_baseline = input;
+        resetTimer();
+      }
+      return input;
+    } else {
+      return m_baseline;
+    }
+  }
+
+  /**
+   * Sets the time to debounce.
+   *
+   * @param time The number of seconds the value must change from baseline for the filtered value to
+   *     change.
+   */
+  public void setDebounceTime(double time) {
+    m_debounceTime = time;
+  }
+
+  /**
+   * Gets the time to debounce.
+   *
+   * @return The number of seconds the value must change from baseline for the filtered value to
+   *     change.
+   */
+  public double getDebounceTime() {
+    return m_debounceTime;
+  }
+
+  /**
+   * Sets the debounce type.
+   *
+   * @param type Which type of state change the debouncing will be performed on.
+   */
+  public void setDebounceType(DebounceType type) {
+    m_debounceType = type;
+
+    m_baseline = m_debounceType == DebounceType.kFalling;
+  }
+
+  /**
+   * Gets the debounce type.
+   *
+   * @return Which type of state change the debouncing will be performed on.
+   */
+  public DebounceType getDebounceType() {
+    return m_debounceType;
+  }
+}

wpimath/src/main/java/org/wpilib/math/filter/LinearFilter.java

@@ -0,0 +1,338 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.filter;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.util.DoubleCircularBuffer;
+import java.util.Arrays;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * 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>
+ * <a
+ * href="https://en.wikipedia.org/wiki/Linear_filter">https://en.wikipedia.org/wiki/Linear_filter</a>
+ * <br>
+ * <a href="https://en.wikipedia.org/wiki/Iir_filter">https://en.wikipedia.org/wiki/Iir_filter</a>
+ * <br>
+ * <a href="https://en.wikipedia.org/wiki/Fir_filter">https://en.wikipedia.org/wiki/Fir_filter</a>
+ * <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 DoubleCircularBuffer m_inputs;
+  private final DoubleCircularBuffer m_outputs;
+  private final double[] m_inputGains;
+  private final double[] m_outputGains;
+  private double m_lastOutput;
+
+  private static int instances;
+
+  /**
+   * Create a linear FIR or IIR filter.
+   *
+   * @param ffGains The "feedforward" or FIR gains.
+   * @param fbGains The "feedback" or IIR gains.
+   */
+  public LinearFilter(double[] ffGains, double[] fbGains) {
+    m_inputs = new DoubleCircularBuffer(ffGains.length);
+    m_outputs = new DoubleCircularBuffer(fbGains.length);
+    m_inputGains = Arrays.copyOf(ffGains, ffGains.length);
+    m_outputGains = Arrays.copyOf(fbGains, fbGains.length);
+
+    instances++;
+    MathSharedStore.reportUsage("LinearFilter", String.valueOf(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<sup>-dt / T</sup>, T is the time constant in seconds.
+   *
+   * <p>Note: T = 1 / (2π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.
+   * @return Linear filter.
+   */
+  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<sup>-dt / T</sup>, T is the time constant in seconds.
+   *
+   * <p>Note: T = 1 / (2π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.
+   * @return Linear filter.
+   */
+  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.
+   * @return Linear filter.
+   * @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];
+    Arrays.fill(ffGains, 1.0 / taps);
+
+    double[] fbGains = new double[0];
+
+    return new LinearFilter(ffGains, fbGains);
+  }
+
+  /**
+   * Creates a finite difference filter that computes the nth derivative of the input given the
+   * specified stencil points.
+   *
+   * <p>Stencil points are the indices of the samples to use in the finite difference. 0 is the
+   * current sample, -1 is the previous sample, -2 is the sample before that, etc. Don't use
+   * positive stencil points (samples from the future) if the LinearFilter will be used for
+   * stream-based online filtering (e.g., taking derivative of encoder samples in real-time).
+   *
+   * @param derivative The order of the derivative to compute.
+   * @param stencil List of stencil points. Its length is the number of samples to use to compute
+   *     the given derivative. This must be one more than the order of the derivative or higher.
+   * @param period The period in seconds between samples taken by the user.
+   * @return Linear filter.
+   * @throws IllegalArgumentException if derivative &lt; 1, samples &lt;= 0, or derivative &gt;=
+   *     samples.
+   */
+  public static LinearFilter finiteDifference(int derivative, int[] stencil, double period) {
+    // See
+    // https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points
+    //
+    // For a given list of stencil points s of length n and the order of
+    // derivative d < n, the finite difference coefficients can be obtained by
+    // solving the following linear system for the vector a.
+    //
+    // [s₁⁰   ⋯  sₙ⁰ ][a₁]      [ δ₀,d ]
+    // [ ⋮    ⋱  ⋮   ][⋮ ] = d! [  ⋮   ]
+    // [s₁ⁿ⁻¹ ⋯ sₙⁿ⁻¹][aₙ]      [δₙ₋₁,d]
+    //
+    // where δᵢ,ⱼ are the Kronecker delta. The FIR gains are the elements of the
+    // vector 'a' in reverse order divided by hᵈ.
+    //
+    // The order of accuracy of the approximation is of the form O(hⁿ⁻ᵈ).
+
+    if (derivative < 1) {
+      throw new IllegalArgumentException(
+          "Order of derivative must be greater than or equal to one.");
+    }
+
+    int samples = stencil.length;
+
+    if (samples <= 0) {
+      throw new IllegalArgumentException("Number of samples must be greater than zero.");
+    }
+
+    if (derivative >= samples) {
+      throw new IllegalArgumentException(
+          "Order of derivative must be less than number of samples.");
+    }
+
+    var S = new SimpleMatrix(samples, samples);
+    for (int row = 0; row < samples; ++row) {
+      for (int col = 0; col < samples; ++col) {
+        S.set(row, col, Math.pow(stencil[col], row));
+      }
+    }
+
+    // Fill in Kronecker deltas: https://en.wikipedia.org/wiki/Kronecker_delta
+    var d = new SimpleMatrix(samples, 1);
+    for (int i = 0; i < samples; ++i) {
+      d.set(i, 0, (i == derivative) ? factorial(derivative) : 0.0);
+    }
+
+    var a = S.solve(d).divide(Math.pow(period, derivative));
+
+    // Reverse gains list
+    double[] ffGains = new double[samples];
+    for (int i = 0; i < samples; ++i) {
+      ffGains[i] = a.get(samples - i - 1, 0);
+    }
+
+    return new LinearFilter(ffGains, new double[0]);
+  }
+
+  /**
+   * Creates a backward finite difference filter that computes the nth derivative of the input given
+   * the specified number of samples.
+   *
+   * <p>For example, a first derivative filter that uses two samples and a sample period of 20 ms
+   * would be
+   *
+   * <pre><code>
+   * LinearFilter.backwardFiniteDifference(1, 2, 0.02);
+   * </code></pre>
+   *
+   * @param derivative The order of the derivative to compute.
+   * @param samples The number of samples to use to compute the given derivative. This must be one
+   *     more than the order of derivative or higher.
+   * @param period The period in seconds between samples taken by the user.
+   * @return Linear filter.
+   */
+  public static LinearFilter backwardFiniteDifference(int derivative, int samples, double period) {
+    // Generate stencil points from -(samples - 1) to 0
+    int[] stencil = new int[samples];
+    for (int i = 0; i < samples; ++i) {
+      stencil[i] = -(samples - 1) + i;
+    }
+
+    return finiteDifference(derivative, stencil, period);
+  }
+
+  /** Reset the filter state. */
+  public void reset() {
+    m_inputs.clear();
+    m_outputs.clear();
+  }
+
+  /**
+   * Resets the filter state, initializing internal buffers to the provided values.
+   *
+   * <p>These are the expected lengths of the buffers, depending on what type of linear filter used:
+   *
+   * <table>
+   * <tr><th>Type</th><th>Input Buffer Length</th><th>Output Buffer Length</th></tr>
+   * <tr><td>Unspecified</td><td>length of {@code ffGains}</td><td>length of {@code fbGains}</td>
+   * </tr>
+   * <tr><td>Single Pole IIR</td><td>1</td><td>1</td></tr>
+   * <tr><td>High-Pass</td><td>2</td><td>1</td></tr>
+   * <tr><td>Moving Average</td><td>{@code taps}</td><td>0</td></tr>
+   * <tr><td>Finite Difference</td><td>length of {@code stencil}</td><td>0</td></tr>
+   * <tr><td>Backward Finite Difference</td><td>{@code samples}</td><td>0</td></tr>
+   * </table>
+   *
+   * @param inputBuffer Values to initialize input buffer.
+   * @param outputBuffer Values to initialize output buffer.
+   * @throws IllegalArgumentException if length of inputBuffer or outputBuffer does not match the
+   *     length of ffGains and fbGains provided in the constructor.
+   */
+  public void reset(double[] inputBuffer, double[] outputBuffer) {
+    // Clear buffers
+    reset();
+
+    if (inputBuffer.length != m_inputGains.length || outputBuffer.length != m_outputGains.length) {
+      throw new IllegalArgumentException("Incorrect length of inputBuffer or outputBuffer");
+    }
+
+    for (double input : inputBuffer) {
+      m_inputs.addFirst(input);
+    }
+    for (double output : outputBuffer) {
+      m_outputs.addFirst(output);
+    }
+  }
+
+  /**
+   * 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
+    if (m_inputGains.length > 0) {
+      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
+    if (m_outputGains.length > 0) {
+      m_outputs.addFirst(retVal);
+    }
+
+    m_lastOutput = retVal;
+    return retVal;
+  }
+
+  /**
+   * Returns the last value calculated by the LinearFilter.
+   *
+   * @return The last value.
+   */
+  public double lastValue() {
+    return m_lastOutput;
+  }
+
+  /**
+   * Factorial of n.
+   *
+   * @param n Argument of which to take factorial.
+   */
+  private static int factorial(int n) {
+    if (n < 2) {
+      return 1;
+    } else {
+      return n * factorial(n - 1);
+    }
+  }
+}

wpimath/src/main/java/org/wpilib/math/filter/MedianFilter.java

@@ -0,0 +1,89 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.filter;
+
+import edu.wpi.first.util.DoubleCircularBuffer;
+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 DoubleCircularBuffer 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 DoubleCircularBuffer(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;
+    }
+
+    // Add next value to circular buffer
+    m_valueBuffer.addFirst(next);
+
+    if (curSize % 2 != 0) {
+      // 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;
+    }
+  }
+
+  /**
+   * Returns the last value calculated by the MedianFilter.
+   *
+   * @return The last value.
+   */
+  public double lastValue() {
+    return m_valueBuffer.getFirst();
+  }
+
+  /** Resets the filter, clearing the window of all elements. */
+  public void reset() {
+    m_orderedValues.clear();
+    m_valueBuffer.clear();
+  }
+}

wpimath/src/main/java/org/wpilib/math/filter/SlewRateLimiter.java

@@ -0,0 +1,84 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.filter;
+
+import edu.wpi.first.math.MathSharedStore;
+
+/**
+ * A class that limits the rate of change of an input value. Useful for implementing voltage,
+ * setpoint, and/or output ramps. A slew-rate limit is most appropriate when the quantity being
+ * controlled is a velocity or a voltage; when controlling a position, consider using a {@link
+ * edu.wpi.first.math.trajectory.TrapezoidProfile} instead.
+ */
+public class SlewRateLimiter {
+  private final double m_positiveRateLimit;
+  private final double m_negativeRateLimit;
+  private double m_prevVal;
+  private double m_prevTime;
+
+  /**
+   * Creates a new SlewRateLimiter with the given positive and negative rate limits and initial
+   * value.
+   *
+   * @param positiveRateLimit The rate-of-change limit in the positive direction, in units per
+   *     second. This is expected to be positive.
+   * @param negativeRateLimit The rate-of-change limit in the negative direction, in units per
+   *     second. This is expected to be negative.
+   * @param initialValue The initial value of the input.
+   */
+  public SlewRateLimiter(double positiveRateLimit, double negativeRateLimit, double initialValue) {
+    m_positiveRateLimit = positiveRateLimit;
+    m_negativeRateLimit = negativeRateLimit;
+    m_prevVal = initialValue;
+    m_prevTime = MathSharedStore.getTimestamp();
+  }
+
+  /**
+   * Creates a new SlewRateLimiter with the given positive rate limit and negative rate limit of
+   * -rateLimit.
+   *
+   * @param rateLimit The rate-of-change limit, in units per second.
+   */
+  public SlewRateLimiter(double rateLimit) {
+    this(rateLimit, -rateLimit, 0);
+  }
+
+  /**
+   * Filters the input to limit its slew rate.
+   *
+   * @param input The input value whose slew rate is to be limited.
+   * @return The filtered value, which will not change faster than the slew rate.
+   */
+  public double calculate(double input) {
+    double currentTime = MathSharedStore.getTimestamp();
+    double elapsedTime = currentTime - m_prevTime;
+    m_prevVal +=
+        Math.clamp(
+            input - m_prevVal,
+            m_negativeRateLimit * elapsedTime,
+            m_positiveRateLimit * elapsedTime);
+    m_prevTime = currentTime;
+    return m_prevVal;
+  }
+
+  /**
+   * Returns the value last calculated by the SlewRateLimiter.
+   *
+   * @return The last value.
+   */
+  public double lastValue() {
+    return m_prevVal;
+  }
+
+  /**
+   * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so.
+   *
+   * @param value The value to reset to.
+   */
+  public void reset(double value) {
+    m_prevVal = value;
+    m_prevTime = MathSharedStore.getTimestamp();
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/CoordinateAxis.java

@@ -0,0 +1,87 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.numbers.N3;
+
+/** A class representing a coordinate system axis within the NWU coordinate system. */
+public class CoordinateAxis {
+  private static final CoordinateAxis m_n = new CoordinateAxis(1.0, 0.0, 0.0);
+  private static final CoordinateAxis m_s = new CoordinateAxis(-1.0, 0.0, 0.0);
+  private static final CoordinateAxis m_e = new CoordinateAxis(0.0, -1.0, 0.0);
+  private static final CoordinateAxis m_w = new CoordinateAxis(0.0, 1.0, 0.0);
+  private static final CoordinateAxis m_u = new CoordinateAxis(0.0, 0.0, 1.0);
+  private static final CoordinateAxis m_d = new CoordinateAxis(0.0, 0.0, -1.0);
+
+  final Vector<N3> m_axis;
+
+  /**
+   * Constructs a coordinate system axis within the NWU coordinate system and normalizes it.
+   *
+   * @param x The x component.
+   * @param y The y component.
+   * @param z The z component.
+   */
+  public CoordinateAxis(double x, double y, double z) {
+    double norm = Math.sqrt(x * x + y * y + z * z);
+    m_axis = VecBuilder.fill(x / norm, y / norm, z / norm);
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to +X in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to +X in the NWU coordinate system.
+   */
+  public static CoordinateAxis N() {
+    return m_n;
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to -X in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to -X in the NWU coordinate system.
+   */
+  public static CoordinateAxis S() {
+    return m_s;
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to -Y in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to -Y in the NWU coordinate system.
+   */
+  public static CoordinateAxis E() {
+    return m_e;
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to +Y in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to +Y in the NWU coordinate system.
+   */
+  public static CoordinateAxis W() {
+    return m_w;
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to +Z in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to +Z in the NWU coordinate system.
+   */
+  public static CoordinateAxis U() {
+    return m_u;
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to -Z in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to -Z in the NWU coordinate system.
+   */
+  public static CoordinateAxis D() {
+    return m_d;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/CoordinateSystem.java

@@ -0,0 +1,132 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+
+/** A helper class that converts Pose3d objects between different standard coordinate frames. */
+public class CoordinateSystem {
+  private static final CoordinateSystem m_nwu =
+      new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.W(), CoordinateAxis.U());
+  private static final CoordinateSystem m_edn =
+      new CoordinateSystem(CoordinateAxis.E(), CoordinateAxis.D(), CoordinateAxis.N());
+  private static final CoordinateSystem m_ned =
+      new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.E(), CoordinateAxis.D());
+
+  // Rotation from this coordinate system to NWU coordinate system
+  private final Rotation3d m_rotation;
+
+  /**
+   * Constructs a coordinate system with the given cardinal directions for each axis.
+   *
+   * @param positiveX The cardinal direction of the positive x-axis.
+   * @param positiveY The cardinal direction of the positive y-axis.
+   * @param positiveZ The cardinal direction of the positive z-axis.
+   * @throws IllegalArgumentException if the coordinate system isn't special orthogonal
+   */
+  public CoordinateSystem(
+      CoordinateAxis positiveX, CoordinateAxis positiveY, CoordinateAxis positiveZ) {
+    // Construct a change of basis matrix from the source coordinate system to the
+    // NWU coordinate system. Each column vector in the change of basis matrix is
+    // one of the old basis vectors mapped to its representation in the new basis.
+    var R = new Matrix<>(Nat.N3(), Nat.N3());
+    R.assignBlock(0, 0, positiveX.m_axis);
+    R.assignBlock(0, 1, positiveY.m_axis);
+    R.assignBlock(0, 2, positiveZ.m_axis);
+
+    // The change of basis matrix should be a pure rotation. The Rotation3d
+    // constructor will verify this by checking for special orthogonality.
+    m_rotation = new Rotation3d(R);
+  }
+
+  /**
+   * Returns an instance of the North-West-Up (NWU) coordinate system.
+   *
+   * <p>The +X axis is north, the +Y axis is west, and the +Z axis is up.
+   *
+   * @return An instance of the North-West-Up (NWU) coordinate system.
+   */
+  public static CoordinateSystem NWU() {
+    return m_nwu;
+  }
+
+  /**
+   * Returns an instance of the East-Down-North (EDN) coordinate system.
+   *
+   * <p>The +X axis is east, the +Y axis is down, and the +Z axis is north.
+   *
+   * @return An instance of the East-Down-North (EDN) coordinate system.
+   */
+  public static CoordinateSystem EDN() {
+    return m_edn;
+  }
+
+  /**
+   * Returns an instance of the North-East-Down (NED) coordinate system.
+   *
+   * <p>The +X axis is north, the +Y axis is east, and the +Z axis is down.
+   *
+   * @return An instance of the North-East-Down (NED) coordinate system.
+   */
+  public static CoordinateSystem NED() {
+    return m_ned;
+  }
+
+  /**
+   * Converts the given translation from one coordinate system to another.
+   *
+   * @param translation The translation to convert.
+   * @param from The coordinate system the pose starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given translation in the desired coordinate system.
+   */
+  public static Translation3d convert(
+      Translation3d translation, CoordinateSystem from, CoordinateSystem to) {
+    return translation.rotateBy(from.m_rotation.minus(to.m_rotation));
+  }
+
+  /**
+   * Converts the given rotation from one coordinate system to another.
+   *
+   * @param rotation The rotation to convert.
+   * @param from The coordinate system the rotation starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given rotation in the desired coordinate system.
+   */
+  public static Rotation3d convert(
+      Rotation3d rotation, CoordinateSystem from, CoordinateSystem to) {
+    return rotation.rotateBy(from.m_rotation.minus(to.m_rotation));
+  }
+
+  /**
+   * Converts the given pose from one coordinate system to another.
+   *
+   * @param pose The pose to convert.
+   * @param from The coordinate system the pose starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given pose in the desired coordinate system.
+   */
+  public static Pose3d convert(Pose3d pose, CoordinateSystem from, CoordinateSystem to) {
+    return new Pose3d(
+        convert(pose.getTranslation(), from, to), convert(pose.getRotation(), from, to));
+  }
+
+  /**
+   * Converts the given transform from one coordinate system to another.
+   *
+   * @param transform The transform to convert.
+   * @param from The coordinate system the transform starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given transform in the desired coordinate system.
+   */
+  public static Transform3d convert(
+      Transform3d transform, CoordinateSystem from, CoordinateSystem to) {
+    var coordRot = from.m_rotation.minus(to.m_rotation);
+    return new Transform3d(
+        convert(transform.getTranslation(), from, to),
+        coordRot.unaryMinus().plus(transform.getRotation().rotateBy(coordRot)));
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/Ellipse2d.java

@@ -0,0 +1,295 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.proto.Ellipse2dProto;
+import edu.wpi.first.math.geometry.struct.Ellipse2dStruct;
+import edu.wpi.first.math.jni.Ellipse2dJNI;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Objects;
+
+/** Represents a 2d ellipse space containing translational, rotational, and scaling components. */
+public class Ellipse2d implements ProtobufSerializable, StructSerializable {
+  private final Pose2d m_center;
+  private final double m_xSemiAxis;
+  private final double m_ySemiAxis;
+
+  /**
+   * Constructs an ellipse around a center point and two semi-axes, a horizontal and vertical one.
+   *
+   * @param center The center of the ellipse.
+   * @param xSemiAxis The x semi-axis.
+   * @param ySemiAxis The y semi-axis.
+   */
+  public Ellipse2d(Pose2d center, double xSemiAxis, double ySemiAxis) {
+    if (xSemiAxis <= 0 || ySemiAxis <= 0) {
+      throw new IllegalArgumentException("Ellipse2d semi-axes must be positive");
+    }
+
+    m_center = center;
+    m_xSemiAxis = xSemiAxis;
+    m_ySemiAxis = ySemiAxis;
+  }
+
+  /**
+   * Constructs an ellipse around a center point and two semi-axes, a horizontal and vertical one.
+   * The X and Y semi-axis will be converted to and tracked as meters.
+   *
+   * @param center The center of the ellipse.
+   * @param xSemiAxis The x semi-axis.
+   * @param ySemiAxis The y semi-axis.
+   */
+  public Ellipse2d(Pose2d center, Distance xSemiAxis, Distance ySemiAxis) {
+    this(center, xSemiAxis.in(Meters), ySemiAxis.in(Meters));
+  }
+
+  /**
+   * Constructs a perfectly circular ellipse with the specified radius.
+   *
+   * @param center The center of the circle.
+   * @param radius The radius of the circle.
+   */
+  public Ellipse2d(Translation2d center, double radius) {
+    this(new Pose2d(center, Rotation2d.kZero), radius, radius);
+  }
+
+  /**
+   * Constructs a perfectly circular ellipse with the specified radius. The radius will be converted
+   * to and tracked as meters.
+   *
+   * @param center The center of the circle.
+   * @param radius The radius of the circle.
+   */
+  public Ellipse2d(Translation2d center, Distance radius) {
+    this(new Pose2d(center, Rotation2d.kZero), radius, radius);
+  }
+
+  /**
+   * Returns the center of the ellipse.
+   *
+   * @return The center of the ellipse.
+   */
+  public Pose2d getCenter() {
+    return m_center;
+  }
+
+  /**
+   * Returns the rotational component of the ellipse.
+   *
+   * @return The rotational component of the ellipse.
+   */
+  public Rotation2d getRotation() {
+    return m_center.getRotation();
+  }
+
+  /**
+   * Returns the x semi-axis.
+   *
+   * @return The x semi-axis.
+   */
+  public double getXSemiAxis() {
+    return m_xSemiAxis;
+  }
+
+  /**
+   * Returns the y semi-axis.
+   *
+   * @return The y semi-axis.
+   */
+  public double getYSemiAxis() {
+    return m_ySemiAxis;
+  }
+
+  /**
+   * Returns the x semi-axis in a measure.
+   *
+   * @return The x semi-axis in a measure.
+   */
+  public Distance getMeasureXSemiAxis() {
+    return Meters.of(m_xSemiAxis);
+  }
+
+  /**
+   * Returns the y semi-axis in a measure.
+   *
+   * @return The y semi-axis in a measure.
+   */
+  public Distance getMeasureYSemiAxis() {
+    return Meters.of(m_ySemiAxis);
+  }
+
+  /**
+   * Returns the focal points of the ellipse. In a perfect circle, this will always return the
+   * center.
+   *
+   * @return The focal points.
+   */
+  public Pair<Translation2d, Translation2d> getFocalPoints() {
+    // Major semi-axis
+    double a = Math.max(m_xSemiAxis, m_ySemiAxis);
+
+    // Minor semi-axis
+    double b = Math.min(m_xSemiAxis, m_ySemiAxis);
+
+    double c = Math.sqrt(a * a - b * b);
+
+    if (m_xSemiAxis > m_ySemiAxis) {
+      return new Pair<>(
+          m_center.plus(new Transform2d(-c, 0.0, Rotation2d.kZero)).getTranslation(),
+          m_center.plus(new Transform2d(c, 0.0, Rotation2d.kZero)).getTranslation());
+    } else {
+      return new Pair<>(
+          m_center.plus(new Transform2d(0.0, -c, Rotation2d.kZero)).getTranslation(),
+          m_center.plus(new Transform2d(0.0, c, Rotation2d.kZero)).getTranslation());
+    }
+  }
+
+  /**
+   * Transforms the center of the ellipse and returns the new ellipse.
+   *
+   * @param other The transform to transform by.
+   * @return The transformed ellipse.
+   */
+  public Ellipse2d transformBy(Transform2d other) {
+    return new Ellipse2d(m_center.transformBy(other), m_xSemiAxis, m_ySemiAxis);
+  }
+
+  /**
+   * Rotates the center of the ellipse and returns the new ellipse.
+   *
+   * @param other The rotation to transform by.
+   * @return The rotated ellipse.
+   */
+  public Ellipse2d rotateBy(Rotation2d other) {
+    return new Ellipse2d(m_center.rotateBy(other), m_xSemiAxis, m_ySemiAxis);
+  }
+
+  /**
+   * Checks if a point is intersected by this ellipse's circumference.
+   *
+   * @param point The point to check.
+   * @return True, if this ellipse's circumference intersects the point.
+   */
+  public boolean intersects(Translation2d point) {
+    return Math.abs(1.0 - solveEllipseEquation(point)) <= 1E-9;
+  }
+
+  /**
+   * Checks if a point is contained within this ellipse. This is inclusive, if the point lies on the
+   * circumference this will return {@code true}.
+   *
+   * @param point The point to check.
+   * @return True, if the point is within or on the ellipse.
+   */
+  public boolean contains(Translation2d point) {
+    return solveEllipseEquation(point) <= 1.0;
+  }
+
+  /**
+   * Returns the distance between the perimeter of the ellipse and the point.
+   *
+   * @param point The point to check.
+   * @return The distance (0, if the point is contained by the ellipse)
+   */
+  public double getDistance(Translation2d point) {
+    return nearest(point).getDistance(point);
+  }
+
+  /**
+   * Returns the distance between the perimeter of the ellipse and the point in a measure.
+   *
+   * @param point The point to check.
+   * @return The distance (0, if the point is contained by the ellipse) in a measure.
+   */
+  public Distance getMeasureDistance(Translation2d point) {
+    return Meters.of(getDistance(point));
+  }
+
+  /**
+   * Returns the nearest point that is contained within the ellipse.
+   *
+   * @param point The point that this will find the nearest point to.
+   * @return A new point that is nearest to {@code point} and contained in the ellipse.
+   */
+  public Translation2d nearest(Translation2d point) {
+    // Check if already in ellipse
+    if (contains(point)) {
+      return point;
+    }
+
+    // Find nearest point
+    var nearestPoint = new double[2];
+    Ellipse2dJNI.nearest(
+        m_center.getX(),
+        m_center.getY(),
+        m_center.getRotation().getRadians(),
+        m_xSemiAxis,
+        m_ySemiAxis,
+        point.getX(),
+        point.getY(),
+        nearestPoint);
+    return new Translation2d(nearestPoint[0], nearestPoint[1]);
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "Ellipse2d(center: %s, x: %.2f, y:%.2f)", m_center, m_xSemiAxis, m_ySemiAxis);
+  }
+
+  /**
+   * Checks equality between this Ellipse2d 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 Ellipse2d) {
+      return ((Ellipse2d) obj).getCenter().equals(m_center)
+          && ((Ellipse2d) obj).getXSemiAxis() == m_xSemiAxis
+          && ((Ellipse2d) obj).getYSemiAxis() == m_ySemiAxis;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_center, m_xSemiAxis, m_ySemiAxis);
+  }
+
+  /**
+   * Solves the equation of an ellipse from the given point. This is a helper function used to
+   * determine if that point lies inside of or on an ellipse.
+   *
+   * <pre>
+   * (x - h)²/a² + (y - k)²/b² = 1
+   * </pre>
+   *
+   * @param point The point to solve for.
+   * @return < 1.0 if the point lies inside the ellipse, == 1.0 if a point lies on the ellipse, and
+   *     > 1.0 if the point lies outsides the ellipse.
+   */
+  private double solveEllipseEquation(Translation2d point) {
+    // Rotate the point by the inverse of the ellipse's rotation
+    point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus());
+
+    double x = point.getX() - m_center.getX();
+    double y = point.getY() - m_center.getY();
+
+    return (x * x) / (m_xSemiAxis * m_xSemiAxis) + (y * y) / (m_ySemiAxis * m_ySemiAxis);
+  }
+
+  /** Ellipse2d protobuf for serialization. */
+  public static final Ellipse2dProto proto = new Ellipse2dProto();
+
+  /** Ellipse2d struct for serialization. */
+  public static final Ellipse2dStruct struct = new Ellipse2dStruct();
+}

wpimath/src/main/java/org/wpilib/math/geometry/Pose2d.java

@@ -0,0 +1,332 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.geometry.proto.Pose2dProto;
+import edu.wpi.first.math.geometry.struct.Pose2dStruct;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Collection;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.Objects;
+
+/** Represents a 2D pose containing translational and rotational elements. */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, StructSerializable {
+  /**
+   * A preallocated Pose2d representing the origin.
+   *
+   * <p>This exists to avoid allocations for common poses.
+   */
+  public static final Pose2d kZero = new Pose2d();
+
+  private final Translation2d m_translation;
+  private final Rotation2d m_rotation;
+
+  /** Constructs a pose at the origin facing toward the positive X axis. */
+  public Pose2d() {
+    m_translation = Translation2d.kZero;
+    m_rotation = Rotation2d.kZero;
+  }
+
+  /**
+   * 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;
+  }
+
+  /**
+   * Constructs a pose with x and y translations instead of a separate Translation2d.
+   *
+   * @param x The x component of the translational component of the pose.
+   * @param y The y component of the translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  public Pose2d(double x, double y, Rotation2d rotation) {
+    m_translation = new Translation2d(x, y);
+    m_rotation = rotation;
+  }
+
+  /**
+   * Constructs a pose with x and y translations instead of a separate Translation2d. The X and Y
+   * translations will be converted to and tracked as meters.
+   *
+   * @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(Distance x, Distance y, Rotation2d rotation) {
+    this(x.in(Meters), y.in(Meters), rotation);
+  }
+
+  /**
+   * Constructs a pose with the specified affine transformation matrix.
+   *
+   * @param matrix The affine transformation matrix.
+   * @throws IllegalArgumentException if the affine transformation matrix is invalid.
+   */
+  public Pose2d(Matrix<N3, N3> matrix) {
+    m_translation = new Translation2d(matrix.get(0, 2), matrix.get(1, 2));
+    m_rotation = new Rotation2d(matrix.block(2, 2, 0, 0));
+    if (matrix.get(2, 0) != 0.0 || matrix.get(2, 1) != 0.0 || matrix.get(2, 2) != 1.0) {
+      throw new IllegalArgumentException("Affine transformation matrix is invalid");
+    }
+  }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new transformed pose.
+   *
+   * <pre>
+   * [x_new]    [cos, -sin, 0][transform.x]
+   * [y_new] += [sin,  cos, 0][transform.y]
+   * [t_new]    [  0,    0, 1][transform.t]
+   * </pre>
+   *
+   * @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 X component of the pose's translation in a measure.
+   *
+   * @return The x component of the pose's translation in a measure.
+   */
+  public Distance getMeasureX() {
+    return m_translation.getMeasureX();
+  }
+
+  /**
+   * Returns the Y component of the pose's translation in a measure.
+   *
+   * @return The y component of the pose's translation in a measure.
+   */
+  public Distance getMeasureY() {
+    return m_translation.getMeasureY();
+  }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return The rotational component of the pose.
+   */
+  @JsonProperty
+  public Rotation2d getRotation() {
+    return m_rotation;
+  }
+
+  /**
+   * Multiplies the current pose by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Pose2d.
+   */
+  public Pose2d times(double scalar) {
+    return new Pose2d(m_translation.times(scalar), m_rotation.times(scalar));
+  }
+
+  /**
+   * Divides the current pose by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Pose2d.
+   */
+  public Pose2d div(double scalar) {
+    return times(1.0 / scalar);
+  }
+
+  /**
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by.
+   * @return The transformed pose.
+   */
+  public Pose2d rotateBy(Rotation2d other) {
+    return new Pose2d(m_translation.rotateBy(other), m_rotation.rotateBy(other));
+  }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new pose. See + operator for
+   * the matrix multiplication performed.
+   *
+   * @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)),
+        other.getRotation().plus(m_rotation));
+  }
+
+  /**
+   * Returns the current pose relative to the given pose.
+   *
+   * <p>This function can often be used for trajectory tracking or pose stabilization algorithms to
+   * get the error between the reference and the current pose.
+   *
+   * @param other The pose that is the origin of the new coordinate frame that the current pose will
+   *     be converted into.
+   * @return The current pose relative to the new origin pose.
+   */
+  public Pose2d relativeTo(Pose2d other) {
+    var transform = new Transform2d(other, this);
+    return new Pose2d(transform.getTranslation(), transform.getRotation());
+  }
+
+  /**
+   * Rotates the current pose around a point in 2D space.
+   *
+   * @param point The point in 2D space to rotate around.
+   * @param rot The rotation to rotate the pose by.
+   * @return The new rotated pose.
+   */
+  public Pose2d rotateAround(Translation2d point, Rotation2d rot) {
+    return new Pose2d(m_translation.rotateAround(point, rot), m_rotation.rotateBy(rot));
+  }
+
+  /**
+   * Returns an affine transformation matrix representation of this pose.
+   *
+   * @return An affine transformation matrix representation of this pose.
+   */
+  public Matrix<N3, N3> toMatrix() {
+    var vec = m_translation.toVector();
+    var mat = m_rotation.toMatrix();
+    return MatBuilder.fill(
+        Nat.N3(),
+        Nat.N3(),
+        mat.get(0, 0),
+        mat.get(0, 1),
+        vec.get(0),
+        mat.get(1, 0),
+        mat.get(1, 1),
+        vec.get(1),
+        0.0,
+        0.0,
+        1.0);
+  }
+
+  /**
+   * Returns the nearest Pose2d from a collection of poses. If two or more poses in the collection
+   * have the same distance from this pose, return the one with the closest rotation component.
+   *
+   * @param poses The collection of poses to find the nearest.
+   * @return The nearest Pose2d from the collection.
+   */
+  public Pose2d nearest(Collection<Pose2d> poses) {
+    return Collections.min(
+        poses,
+        Comparator.comparing(
+                (Pose2d other) -> this.getTranslation().getDistance(other.getTranslation()))
+            .thenComparing(
+                (Pose2d other) ->
+                    Math.abs(this.getRotation().minus(other.getRotation()).getRadians())));
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
+  }
+
+  /**
+   * 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) {
+    return obj instanceof Pose2d pose
+        && m_translation.equals(pose.m_translation)
+        && m_rotation.equals(pose.m_rotation);
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_translation, m_rotation);
+  }
+
+  @Override
+  public Pose2d interpolate(Pose2d endValue, double t) {
+    if (t < 0) {
+      return this;
+    } else if (t >= 1) {
+      return endValue;
+    } else {
+      var twist = endValue.minus(this).log();
+      var scaledTwist = new Twist2d(twist.dx * t, twist.dy * t, twist.dtheta * t);
+      return this.plus(scaledTwist.exp());
+    }
+  }
+
+  /** Pose2d protobuf for serialization. */
+  public static final Pose2dProto proto = new Pose2dProto();
+
+  /** Pose2d struct for serialization. */
+  public static final Pose2dStruct struct = new Pose2dStruct();
+}

wpimath/src/main/java/org/wpilib/math/geometry/Pose3d.java

@@ -0,0 +1,384 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.geometry.proto.Pose3dProto;
+import edu.wpi.first.math.geometry.struct.Pose3dStruct;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.numbers.N4;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Collection;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.Objects;
+
+/** Represents a 3D pose containing translational and rotational elements. */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, StructSerializable {
+  /**
+   * A preallocated Pose3d representing the origin.
+   *
+   * <p>This exists to avoid allocations for common poses.
+   */
+  public static final Pose3d kZero = new Pose3d();
+
+  private final Translation3d m_translation;
+  private final Rotation3d m_rotation;
+
+  /** Constructs a pose at the origin facing toward the positive X axis. */
+  public Pose3d() {
+    m_translation = Translation3d.kZero;
+    m_rotation = Rotation3d.kZero;
+  }
+
+  /**
+   * Constructs a pose with the specified translation and rotation.
+   *
+   * @param translation The translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  @JsonCreator
+  public Pose3d(
+      @JsonProperty(required = true, value = "translation") Translation3d translation,
+      @JsonProperty(required = true, value = "rotation") Rotation3d rotation) {
+    m_translation = translation;
+    m_rotation = rotation;
+  }
+
+  /**
+   * Constructs a pose with x, y, and z translations instead of a separate Translation3d.
+   *
+   * @param x The x component of the translational component of the pose.
+   * @param y The y component of the translational component of the pose.
+   * @param z The z component of the translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  public Pose3d(double x, double y, double z, Rotation3d rotation) {
+    m_translation = new Translation3d(x, y, z);
+    m_rotation = rotation;
+  }
+
+  /**
+   * Constructs a pose with x, y, and z translations instead of a separate Translation3d. The X, Y,
+   * and Z translations will be converted to and tracked as meters.
+   *
+   * @param x The x component of the translational component of the pose.
+   * @param y The y component of the translational component of the pose.
+   * @param z The z component of the translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  public Pose3d(Distance x, Distance y, Distance z, Rotation3d rotation) {
+    this(x.in(Meters), y.in(Meters), z.in(Meters), rotation);
+  }
+
+  /**
+   * Constructs a pose with the specified affine transformation matrix.
+   *
+   * @param matrix The affine transformation matrix.
+   * @throws IllegalArgumentException if the affine transformation matrix is invalid.
+   */
+  public Pose3d(Matrix<N4, N4> matrix) {
+    m_translation = new Translation3d(matrix.get(0, 3), matrix.get(1, 3), matrix.get(2, 3));
+    m_rotation = new Rotation3d(matrix.block(3, 3, 0, 0));
+    if (matrix.get(3, 0) != 0.0
+        || matrix.get(3, 1) != 0.0
+        || matrix.get(3, 2) != 0.0
+        || matrix.get(3, 3) != 1.0) {
+      throw new IllegalArgumentException("Affine transformation matrix is invalid");
+    }
+  }
+
+  /**
+   * Constructs a 3D pose from a 2D pose in the X-Y plane.
+   *
+   * @param pose The 2D pose.
+   * @see Rotation3d#Rotation3d(Rotation2d)
+   * @see Translation3d#Translation3d(Translation2d)
+   */
+  public Pose3d(Pose2d pose) {
+    m_translation = new Translation3d(pose.getX(), pose.getY(), 0.0);
+    m_rotation = new Rotation3d(0.0, 0.0, pose.getRotation().getRadians());
+  }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new transformed pose. The
+   * transform is applied relative to the pose's frame. Note that this differs from {@link
+   * Pose3d#rotateBy(Rotation3d)}, which is applied relative to the global frame and around the
+   * origin.
+   *
+   * @param other The transform to transform the pose by.
+   * @return The transformed pose.
+   */
+  public Pose3d plus(Transform3d other) {
+    return transformBy(other);
+  }
+
+  /**
+   * Returns the Transform3d that maps the one pose to another.
+   *
+   * @param other The initial pose of the transformation.
+   * @return The transform that maps the other pose to the current pose.
+   */
+  public Transform3d minus(Pose3d other) {
+    final var pose = this.relativeTo(other);
+    return new Transform3d(pose.getTranslation(), pose.getRotation());
+  }
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return The translational component of the pose.
+   */
+  @JsonProperty
+  public Translation3d getTranslation() {
+    return m_translation;
+  }
+
+  /**
+   * Returns the X component of the pose's translation.
+   *
+   * @return The x component of the pose's translation.
+   */
+  public double getX() {
+    return m_translation.getX();
+  }
+
+  /**
+   * Returns the Y component of the pose's translation.
+   *
+   * @return The y component of the pose's translation.
+   */
+  public double getY() {
+    return m_translation.getY();
+  }
+
+  /**
+   * Returns the Z component of the pose's translation.
+   *
+   * @return The z component of the pose's translation.
+   */
+  public double getZ() {
+    return m_translation.getZ();
+  }
+
+  /**
+   * Returns the X component of the pose's translation in a measure.
+   *
+   * @return The x component of the pose's translation in a measure.
+   */
+  public Distance getMeasureX() {
+    return m_translation.getMeasureX();
+  }
+
+  /**
+   * Returns the Y component of the pose's translation in a measure.
+   *
+   * @return The y component of the pose's translation in a measure.
+   */
+  public Distance getMeasureY() {
+    return m_translation.getMeasureY();
+  }
+
+  /**
+   * Returns the Z component of the pose's translation in a measure.
+   *
+   * @return The z component of the pose's translation in a measure.
+   */
+  public Distance getMeasureZ() {
+    return m_translation.getMeasureZ();
+  }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return The rotational component of the pose.
+   */
+  @JsonProperty
+  public Rotation3d getRotation() {
+    return m_rotation;
+  }
+
+  /**
+   * Multiplies the current pose by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Pose3d.
+   */
+  public Pose3d times(double scalar) {
+    return new Pose3d(m_translation.times(scalar), m_rotation.times(scalar));
+  }
+
+  /**
+   * Divides the current pose by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Pose3d.
+   */
+  public Pose3d div(double scalar) {
+    return times(1.0 / scalar);
+  }
+
+  /**
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by, which is applied extrinsically (from the
+   *     global frame).
+   * @return The rotated pose.
+   */
+  public Pose3d rotateBy(Rotation3d other) {
+    return new Pose3d(m_translation.rotateBy(other), m_rotation.rotateBy(other));
+  }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new transformed pose. The
+   * transform is applied relative to the pose's frame. Note that this differs from {@link
+   * Pose3d#rotateBy(Rotation3d)}, which is applied relative to the global frame and around the
+   * origin.
+   *
+   * @param other The transform to transform the pose by.
+   * @return The transformed pose.
+   */
+  public Pose3d transformBy(Transform3d other) {
+    return new Pose3d(
+        m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
+        other.getRotation().plus(m_rotation));
+  }
+
+  /**
+   * Returns the current pose relative to the given pose.
+   *
+   * <p>This function can often be used for trajectory tracking or pose stabilization algorithms to
+   * get the error between the reference and the current pose.
+   *
+   * @param other The pose that is the origin of the new coordinate frame that the current pose will
+   *     be converted into.
+   * @return The current pose relative to the new origin pose.
+   */
+  public Pose3d relativeTo(Pose3d other) {
+    var transform = new Transform3d(other, this);
+    return new Pose3d(transform.getTranslation(), transform.getRotation());
+  }
+
+  /**
+   * Rotates the current pose around a point in 3D space.
+   *
+   * @param point The point in 3D space to rotate around.
+   * @param rot The rotation to rotate the pose by.
+   * @return The new rotated pose.
+   */
+  public Pose3d rotateAround(Translation3d point, Rotation3d rot) {
+    return new Pose3d(m_translation.rotateAround(point, rot), m_rotation.rotateBy(rot));
+  }
+
+  /**
+   * Returns an affine transformation matrix representation of this pose.
+   *
+   * @return An affine transformation matrix representation of this pose.
+   */
+  public Matrix<N4, N4> toMatrix() {
+    var vec = m_translation.toVector();
+    var mat = m_rotation.toMatrix();
+    return MatBuilder.fill(
+        Nat.N4(),
+        Nat.N4(),
+        mat.get(0, 0),
+        mat.get(0, 1),
+        mat.get(0, 2),
+        vec.get(0),
+        mat.get(1, 0),
+        mat.get(1, 1),
+        mat.get(1, 2),
+        vec.get(1),
+        mat.get(2, 0),
+        mat.get(2, 1),
+        mat.get(2, 2),
+        vec.get(2),
+        0.0,
+        0.0,
+        0.0,
+        1.0);
+  }
+
+  /**
+   * Returns a Pose2d representing this Pose3d projected into the X-Y plane.
+   *
+   * @return A Pose2d representing this Pose3d projected into the X-Y plane.
+   */
+  public Pose2d toPose2d() {
+    return new Pose2d(m_translation.toTranslation2d(), m_rotation.toRotation2d());
+  }
+
+  /**
+   * Returns the nearest Pose3d from a collection of poses. If two or more poses in the collection
+   * have the same distance from this pose, return the one with the closest rotation component.
+   *
+   * @param poses The collection of poses to find the nearest.
+   * @return The nearest Pose3d from the collection.
+   */
+  public Pose3d nearest(Collection<Pose3d> poses) {
+    return Collections.min(
+        poses,
+        Comparator.comparing(
+                (Pose3d other) -> this.getTranslation().getDistance(other.getTranslation()))
+            .thenComparing(
+                (Pose3d other) -> this.getRotation().minus(other.getRotation()).getAngle()));
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Pose3d(%s, %s)", m_translation, m_rotation);
+  }
+
+  /**
+   * Checks equality between this Pose3d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    return obj instanceof Pose3d pose
+        && m_translation.equals(pose.m_translation)
+        && m_rotation.equals(pose.m_rotation);
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_translation, m_rotation);
+  }
+
+  @Override
+  public Pose3d interpolate(Pose3d endValue, double t) {
+    if (t < 0) {
+      return this;
+    } else if (t >= 1) {
+      return endValue;
+    } else {
+      var twist = endValue.minus(this).log();
+      var scaledTwist =
+          new Twist3d(
+              twist.dx * t, twist.dy * t, twist.dz * t, twist.rx * t, twist.ry * t, twist.rz * t);
+      return this.plus(scaledTwist.exp());
+    }
+  }
+
+  /** Pose3d protobuf for serialization. */
+  public static final Pose3dProto proto = new Pose3dProto();
+
+  /** Pose3d struct for serialization. */
+  public static final Pose3dStruct struct = new Pose3dStruct();
+}

wpimath/src/main/java/org/wpilib/math/geometry/Quaternion.java

@@ -0,0 +1,413 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.proto.QuaternionProto;
+import edu.wpi.first.math.geometry.struct.QuaternionStruct;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Objects;
+
+/** Represents a quaternion. */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Quaternion implements ProtobufSerializable, StructSerializable {
+  // Scalar r in versor form
+  private final double m_w;
+
+  // Vector v in versor form
+  private final double m_x;
+  private final double m_y;
+  private final double m_z;
+
+  /** Constructs a quaternion with a default angle of 0 degrees. */
+  public Quaternion() {
+    m_w = 1.0;
+    m_x = 0.0;
+    m_y = 0.0;
+    m_z = 0.0;
+  }
+
+  /**
+   * Constructs a quaternion with the given components.
+   *
+   * @param w W component of the quaternion.
+   * @param x X component of the quaternion.
+   * @param y Y component of the quaternion.
+   * @param z Z component of the quaternion.
+   */
+  @JsonCreator
+  public Quaternion(
+      @JsonProperty(required = true, value = "W") double w,
+      @JsonProperty(required = true, value = "X") double x,
+      @JsonProperty(required = true, value = "Y") double y,
+      @JsonProperty(required = true, value = "Z") double z) {
+    m_w = w;
+    m_x = x;
+    m_y = y;
+    m_z = z;
+  }
+
+  /**
+   * Adds another quaternion to this quaternion entrywise.
+   *
+   * @param other The other quaternion.
+   * @return The quaternion sum.
+   */
+  public Quaternion plus(Quaternion other) {
+    return new Quaternion(
+        getW() + other.getW(), getX() + other.getX(), getY() + other.getY(), getZ() + other.getZ());
+  }
+
+  /**
+   * Subtracts another quaternion from this quaternion entrywise.
+   *
+   * @param other The other quaternion.
+   * @return The quaternion difference.
+   */
+  public Quaternion minus(Quaternion other) {
+    return new Quaternion(
+        getW() - other.getW(), getX() - other.getX(), getY() - other.getY(), getZ() - other.getZ());
+  }
+
+  /**
+   * Divides by a scalar.
+   *
+   * @param scalar The value to scale each component by.
+   * @return The scaled quaternion.
+   */
+  public Quaternion divide(double scalar) {
+    return new Quaternion(getW() / scalar, getX() / scalar, getY() / scalar, getZ() / scalar);
+  }
+
+  /**
+   * Multiplies with a scalar.
+   *
+   * @param scalar The value to scale each component by.
+   * @return The scaled quaternion.
+   */
+  public Quaternion times(double scalar) {
+    return new Quaternion(getW() * scalar, getX() * scalar, getY() * scalar, getZ() * scalar);
+  }
+
+  /**
+   * Multiply with another quaternion.
+   *
+   * @param other The other quaternion.
+   * @return The quaternion product.
+   */
+  public Quaternion times(Quaternion other) {
+    // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
+    final var r1 = m_w;
+    final var r2 = other.m_w;
+
+    // v₁ ⋅ v₂
+    double dot = m_x * other.m_x + m_y * other.m_y + m_z * other.m_z;
+
+    // v₁ x v₂
+    double cross_x = m_y * other.m_z - other.m_y * m_z;
+    double cross_y = other.m_x * m_z - m_x * other.m_z;
+    double cross_z = m_x * other.m_y - other.m_x * m_y;
+
+    return new Quaternion(
+        // r = r₁r₂ − v₁ ⋅ v₂
+        r1 * r2 - dot,
+        // v = r₁v₂ + r₂v₁ + v₁ x v₂
+        r1 * other.m_x + r2 * m_x + cross_x,
+        r1 * other.m_y + r2 * m_y + cross_y,
+        r1 * other.m_z + r2 * m_z + cross_z);
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Quaternion(%s, %s, %s, %s)", getW(), getX(), getY(), getZ());
+  }
+
+  /**
+   * Checks equality between this Quaternion and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    return obj instanceof Quaternion other
+        && Math.abs(dot(other) - norm() * other.norm()) < 1e-9
+        && Math.abs(norm() - other.norm()) < 1e-9;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_w, m_x, m_y, m_z);
+  }
+
+  /**
+   * Returns the conjugate of the quaternion.
+   *
+   * @return The conjugate quaternion.
+   */
+  public Quaternion conjugate() {
+    return new Quaternion(getW(), -getX(), -getY(), -getZ());
+  }
+
+  /**
+   * Returns the elementwise product of two quaternions.
+   *
+   * @param other The other quaternion.
+   * @return The dot product of two quaternions.
+   */
+  public double dot(final Quaternion other) {
+    return getW() * other.getW()
+        + getX() * other.getX()
+        + getY() * other.getY()
+        + getZ() * other.getZ();
+  }
+
+  /**
+   * Returns the inverse of the quaternion.
+   *
+   * @return The inverse quaternion.
+   */
+  public Quaternion inverse() {
+    var norm = norm();
+    return conjugate().divide(norm * norm);
+  }
+
+  /**
+   * Calculates the L2 norm of the quaternion.
+   *
+   * @return The L2 norm.
+   */
+  public double norm() {
+    return Math.sqrt(dot(this));
+  }
+
+  /**
+   * Normalizes the quaternion.
+   *
+   * @return The normalized quaternion.
+   */
+  public Quaternion normalize() {
+    double norm = norm();
+    if (norm == 0.0) {
+      return new Quaternion();
+    } else {
+      return new Quaternion(getW() / norm, getX() / norm, getY() / norm, getZ() / norm);
+    }
+  }
+
+  /**
+   * Rational power of a quaternion.
+   *
+   * @param t the power to raise this quaternion to.
+   * @return The quaternion power
+   */
+  public Quaternion pow(double t) {
+    // q^t = e^(ln(q^t)) = e^(t * ln(q))
+    return this.log().times(t).exp();
+  }
+
+  /**
+   * Matrix exponential of a quaternion.
+   *
+   * @param adjustment the "Twist" that will be applied to this quaternion.
+   * @return The quaternion product of exp(adjustment) * this
+   */
+  public Quaternion exp(Quaternion adjustment) {
+    return adjustment.exp().times(this);
+  }
+
+  /**
+   * Matrix exponential of a quaternion.
+   *
+   * <p>source: wpimath/algorithms.md
+   *
+   * <p>If this quaternion is in 𝖘𝖔(3) and you are looking for an element of SO(3), use {@link
+   * fromRotationVector}
+   *
+   * @return The Matrix exponential of this quaternion.
+   */
+  public Quaternion exp() {
+    var scalar = Math.exp(getW());
+
+    var axial_magnitude = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
+    var cosine = Math.cos(axial_magnitude);
+
+    double axial_scalar;
+
+    if (axial_magnitude < 1e-9) {
+      // Taylor series of sin(θ) / θ near θ = 0: 1 − θ²/6 + θ⁴/120 + O(n⁶)
+      var axial_magnitude_sq = axial_magnitude * axial_magnitude;
+      var axial_magnitude_sq_sq = axial_magnitude_sq * axial_magnitude_sq;
+      axial_scalar = 1.0 - axial_magnitude_sq / 6.0 + axial_magnitude_sq_sq / 120.0;
+    } else {
+      axial_scalar = Math.sin(axial_magnitude) / axial_magnitude;
+    }
+
+    return new Quaternion(
+        cosine * scalar,
+        getX() * axial_scalar * scalar,
+        getY() * axial_scalar * scalar,
+        getZ() * axial_scalar * scalar);
+  }
+
+  /**
+   * Log operator of a quaternion.
+   *
+   * @param end The quaternion to map this quaternion onto.
+   * @return The "Twist" that maps this quaternion to the argument.
+   */
+  public Quaternion log(Quaternion end) {
+    return end.times(this.inverse()).log();
+  }
+
+  /**
+   * The Log operator of a general quaternion.
+   *
+   * <p>source: wpimath/algorithms.md
+   *
+   * <p>If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3), use {@link
+   * toRotationVector}
+   *
+   * @return The logarithm of this quaternion.
+   */
+  public Quaternion log() {
+    var norm = norm();
+    var scalar = Math.log(norm);
+
+    var v_norm = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
+
+    var s_norm = getW() / norm;
+
+    if (Math.abs(s_norm + 1) < 1e-9) {
+      return new Quaternion(scalar, -Math.PI, 0, 0);
+    }
+
+    double v_scalar;
+
+    if (v_norm < 1e-9) {
+      // Taylor series expansion of atan2(y/x)/y at y = 0:
+      //
+      //   1/x - 1/3 y²/x³ + O(y⁴)
+      v_scalar = 1.0 / getW() - 1.0 / 3.0 * v_norm * v_norm / (getW() * getW() * getW());
+    } else {
+      v_scalar = Math.atan2(v_norm, getW()) / v_norm;
+    }
+
+    return new Quaternion(scalar, v_scalar * getX(), v_scalar * getY(), v_scalar * getZ());
+  }
+
+  /**
+   * Returns W component of the quaternion.
+   *
+   * @return W component of the quaternion.
+   */
+  @JsonProperty(value = "W")
+  public double getW() {
+    return m_w;
+  }
+
+  /**
+   * Returns X component of the quaternion.
+   *
+   * @return X component of the quaternion.
+   */
+  @JsonProperty(value = "X")
+  public double getX() {
+    return m_x;
+  }
+
+  /**
+   * Returns Y component of the quaternion.
+   *
+   * @return Y component of the quaternion.
+   */
+  @JsonProperty(value = "Y")
+  public double getY() {
+    return m_y;
+  }
+
+  /**
+   * Returns Z component of the quaternion.
+   *
+   * @return Z component of the quaternion.
+   */
+  @JsonProperty(value = "Z")
+  public double getZ() {
+    return m_z;
+  }
+
+  /**
+   * Returns the quaternion representation of this rotation vector.
+   *
+   * <p>This is also the exp operator of 𝖘𝖔(3).
+   *
+   * <p>source: wpimath/algorithms.md
+   *
+   * @param rvec The rotation vector.
+   * @return The quaternion representation of this rotation vector.
+   */
+  public static Quaternion fromRotationVector(Vector<N3> rvec) {
+    double theta = rvec.norm();
+
+    double cos = Math.cos(theta / 2);
+
+    double axial_scalar;
+
+    if (theta < 1e-9) {
+      // taylor series expansion of sin(θ/2) / θ = 1/2 - θ²/48 + O(θ⁴)
+      axial_scalar = 1.0 / 2.0 - theta * theta / 48.0;
+    } else {
+      axial_scalar = Math.sin(theta / 2) / theta;
+    }
+
+    return new Quaternion(
+        cos,
+        axial_scalar * rvec.get(0, 0),
+        axial_scalar * rvec.get(1, 0),
+        axial_scalar * rvec.get(2, 0));
+  }
+
+  /**
+   * Returns the rotation vector representation of this quaternion.
+   *
+   * <p>This is also the log operator of SO(3).
+   *
+   * @return The rotation vector representation of this quaternion.
+   */
+  public Vector<N3> toRotationVector() {
+    // See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
+    // Sound State Representation through Encapsulation of Manifolds"
+    //
+    // https://arxiv.org/pdf/1107.1119.pdf
+    double norm = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
+
+    double coeff;
+    if (norm < 1e-9) {
+      coeff = 2.0 / getW() - 2.0 / 3.0 * norm * norm / (getW() * getW() * getW());
+    } else {
+      if (getW() < 0.0) {
+        coeff = 2.0 * Math.atan2(-norm, -getW()) / norm;
+      } else {
+        coeff = 2.0 * Math.atan2(norm, getW()) / norm;
+      }
+    }
+
+    return VecBuilder.fill(coeff * getX(), coeff * getY(), coeff * getZ());
+  }
+
+  /** Quaternion protobuf for serialization. */
+  public static final QuaternionProto proto = new QuaternionProto();
+
+  /** Quaternion struct for serialization. */
+  public static final QuaternionStruct struct = new QuaternionStruct();
+}

wpimath/src/main/java/org/wpilib/math/geometry/Rectangle2d.java

@@ -0,0 +1,259 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import edu.wpi.first.math.geometry.proto.Rectangle2dProto;
+import edu.wpi.first.math.geometry.struct.Rectangle2dStruct;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Objects;
+
+/**
+ * Represents a 2d rectangular space containing translational, rotational, and scaling components.
+ */
+public class Rectangle2d implements ProtobufSerializable, StructSerializable {
+  private final Pose2d m_center;
+  private final double m_xWidth;
+  private final double m_yWidth;
+
+  /**
+   * Constructs a rectangle at the specified position with the specified width and height.
+   *
+   * @param center The position (translation and rotation) of the rectangle.
+   * @param xWidth The x size component of the rectangle, in unrotated coordinate frame.
+   * @param yWidth The y size component of the rectangle, in unrotated coordinate frame.
+   */
+  public Rectangle2d(Pose2d center, double xWidth, double yWidth) {
+    if (xWidth < 0 || yWidth < 0) {
+      throw new IllegalArgumentException("Rectangle2d dimensions cannot be less than 0!");
+    }
+
+    m_center = center;
+    m_xWidth = xWidth;
+    m_yWidth = yWidth;
+  }
+
+  /**
+   * Constructs a rectangle at the specified position with the specified width and height. The X and
+   * Y widths will be converted to and tracked as meters.
+   *
+   * @param center The position (translation and rotation) of the rectangle.
+   * @param xWidth The x size component of the rectangle, in unrotated coordinate frame.
+   * @param yWidth The y size component of the rectangle, in unrotated coordinate frame.
+   */
+  public Rectangle2d(Pose2d center, Distance xWidth, Distance yWidth) {
+    this(center, xWidth.in(Meters), yWidth.in(Meters));
+  }
+
+  /**
+   * Creates an unrotated rectangle from the given corners. The corners should be diagonally
+   * opposite of each other.
+   *
+   * @param cornerA The first corner of the rectangle.
+   * @param cornerB The second corner of the rectangle.
+   */
+  public Rectangle2d(Translation2d cornerA, Translation2d cornerB) {
+    this(
+        new Pose2d(cornerA.plus(cornerB).div(2.0), Rotation2d.kZero),
+        Math.abs(cornerA.getX() - cornerB.getX()),
+        Math.abs(cornerA.getY() - cornerB.getY()));
+  }
+
+  /**
+   * Returns the center of the rectangle.
+   *
+   * @return The center of the rectangle.
+   */
+  public Pose2d getCenter() {
+    return m_center;
+  }
+
+  /**
+   * Returns the rotational component of the rectangle.
+   *
+   * @return The rotational component of the rectangle.
+   */
+  public Rotation2d getRotation() {
+    return m_center.getRotation();
+  }
+
+  /**
+   * Returns the x size component of the rectangle.
+   *
+   * @return The x size component of the rectangle.
+   */
+  public double getXWidth() {
+    return m_xWidth;
+  }
+
+  /**
+   * Returns the y size component of the rectangle.
+   *
+   * @return The y size component of the rectangle.
+   */
+  public double getYWidth() {
+    return m_yWidth;
+  }
+
+  /**
+   * Returns the X size component of the rectangle in a measure.
+   *
+   * @return The x size component of the rectangle in a measure.
+   */
+  public Distance getMeasureXWidth() {
+    return Meters.of(m_xWidth);
+  }
+
+  /**
+   * Returns the Y size component of the rectangle in a measure.
+   *
+   * @return The y size component of the rectangle in a measure.
+   */
+  public Distance getMeasureYWidth() {
+    return Meters.of(m_yWidth);
+  }
+
+  /**
+   * Transforms the center of the rectangle and returns the new rectangle.
+   *
+   * @param other The transform to transform by.
+   * @return The transformed rectangle
+   */
+  public Rectangle2d transformBy(Transform2d other) {
+    return new Rectangle2d(m_center.transformBy(other), m_xWidth, m_yWidth);
+  }
+
+  /**
+   * Rotates the center of the rectangle and returns the new rectangle.
+   *
+   * @param other The rotation to transform by.
+   * @return The rotated rectangle.
+   */
+  public Rectangle2d rotateBy(Rotation2d other) {
+    return new Rectangle2d(m_center.rotateBy(other), m_xWidth, m_yWidth);
+  }
+
+  /**
+   * Checks if a point is intersected by the rectangle's perimeter.
+   *
+   * @param point The point to check.
+   * @return True, if the rectangle's perimeter intersects the point.
+   */
+  public boolean intersects(Translation2d point) {
+    // Move the point into the rectangle's coordinate frame
+    point = point.minus(m_center.getTranslation());
+    point = point.rotateBy(m_center.getRotation().unaryMinus());
+
+    if (Math.abs(Math.abs(point.getX()) - m_xWidth / 2.0) <= 1E-9) {
+      // Point rests on left/right perimeter
+      return Math.abs(point.getY()) <= m_yWidth / 2.0;
+    } else if (Math.abs(Math.abs(point.getY()) - m_yWidth / 2.0) <= 1E-9) {
+      // Point rests on top/bottom perimeter
+      return Math.abs(point.getX()) <= m_xWidth / 2.0;
+    }
+
+    return false;
+  }
+
+  /**
+   * Checks if a point is contained within the rectangle. This is inclusive, if the point lies on
+   * the perimeter it will return true.
+   *
+   * @param point The point to check.
+   * @return True, if the rectangle contains the point or the perimeter intersects the point.
+   */
+  public boolean contains(Translation2d point) {
+    // Rotate the point into the rectangle's coordinate frame
+    point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus());
+
+    // Check if within bounding box
+    return point.getX() >= (m_center.getX() - m_xWidth / 2.0)
+        && point.getX() <= (m_center.getX() + m_xWidth / 2.0)
+        && point.getY() >= (m_center.getY() - m_yWidth / 2.0)
+        && point.getY() <= (m_center.getY() + m_yWidth / 2.0);
+  }
+
+  /**
+   * Returns the distance between the perimeter of the rectangle and the point.
+   *
+   * @param point The point to check.
+   * @return The distance (0, if the point is contained by the rectangle)
+   */
+  public double getDistance(Translation2d point) {
+    return nearest(point).getDistance(point);
+  }
+
+  /**
+   * Returns the distance between the perimeter of the rectangle and the point in a measure.
+   *
+   * @param point The point to check.
+   * @return The distance (0, if the point is contained by the rectangle) in a measure.
+   */
+  public Distance getMeasureDistance(Translation2d point) {
+    return Meters.of(getDistance(point));
+  }
+
+  /**
+   * Returns the nearest point that is contained within the rectangle.
+   *
+   * @param point The point that this will find the nearest point to.
+   * @return A new point that is nearest to {@code point} and contained in the rectangle.
+   */
+  public Translation2d nearest(Translation2d point) {
+    // Check if already in rectangle
+    if (contains(point)) {
+      return point;
+    }
+
+    // Rotate the point by the inverse of the rectangle's rotation
+    point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus());
+
+    // Find nearest point
+    point =
+        new Translation2d(
+            Math.clamp(
+                point.getX(), m_center.getX() - m_xWidth / 2.0, m_center.getX() + m_xWidth / 2.0),
+            Math.clamp(
+                point.getY(), m_center.getY() - m_yWidth / 2.0, m_center.getY() + m_yWidth / 2.0));
+
+    // Undo rotation
+    return point.rotateAround(m_center.getTranslation(), m_center.getRotation());
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Rectangle2d(center: %s, x: %.2f, y: %.2f)", m_center, m_xWidth, m_yWidth);
+  }
+
+  /**
+   * Checks equality between this Rectangle2d 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 Rectangle2d) {
+      return ((Rectangle2d) obj).getCenter().equals(m_center)
+          && ((Rectangle2d) obj).getXWidth() == m_xWidth
+          && ((Rectangle2d) obj).getYWidth() == m_yWidth;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_center, m_xWidth, m_yWidth);
+  }
+
+  /** Rectangle2d protobuf for serialization. */
+  public static final Rectangle2dProto proto = new Rectangle2dProto();
+
+  /** Rectangle2d struct for serialization. */
+  public static final Rectangle2dStruct struct = new Rectangle2dStruct();
+}

wpimath/src/main/java/org/wpilib/math/geometry/Rotation2d.java

@@ -0,0 +1,374 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static edu.wpi.first.units.Units.Radians;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.geometry.proto.Rotation2dProto;
+import edu.wpi.first.math.geometry.struct.Rotation2dStruct;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Objects;
+
+/**
+ * A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
+ */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Rotation2d
+    implements Interpolatable<Rotation2d>, ProtobufSerializable, StructSerializable {
+  /**
+   * A preallocated Rotation2d representing no rotation.
+   *
+   * <p>This exists to avoid allocations for common rotations.
+   */
+  public static final Rotation2d kZero = new Rotation2d();
+
+  /**
+   * A preallocated Rotation2d representing a clockwise rotation by π/2 rad (90°).
+   *
+   * <p>This exists to avoid allocations for common rotations.
+   */
+  public static final Rotation2d kCW_Pi_2 = new Rotation2d(-Math.PI / 2);
+
+  /**
+   * A preallocated Rotation2d representing a clockwise rotation by 90° (π/2 rad).
+   *
+   * <p>This exists to avoid allocations for common rotations.
+   */
+  public static final Rotation2d kCW_90deg = kCW_Pi_2;
+
+  /**
+   * A preallocated Rotation2d representing a counterclockwise rotation by π/2 rad (90°).
+   *
+   * <p>This exists to avoid allocations for common rotations.
+   */
+  public static final Rotation2d kCCW_Pi_2 = new Rotation2d(Math.PI / 2);
+
+  /**
+   * A preallocated Rotation2d representing a counterclockwise rotation by 90° (π/2 rad).
+   *
+   * <p>This exists to avoid allocations for common rotations.
+   */
+  public static final Rotation2d kCCW_90deg = kCCW_Pi_2;
+
+  /**
+   * A preallocated Rotation2d representing a counterclockwise rotation by π rad (180°).
+   *
+   * <p>This exists to avoid allocations for common rotations.
+   */
+  public static final Rotation2d kPi = new Rotation2d(Math.PI);
+
+  /**
+   * A preallocated Rotation2d representing a counterclockwise rotation by 180° (π rad).
+   *
+   * <p>This exists to avoid allocations for common rotations.
+   */
+  public static final Rotation2d k180deg = kPi;
+
+  private final double m_cos;
+  private final double m_sin;
+
+  /** Constructs a Rotation2d with a default angle of 0 degrees. */
+  public Rotation2d() {
+    m_cos = 1.0;
+    m_sin = 0.0;
+  }
+
+  /**
+   * Constructs a Rotation2d with the given radian value.
+   *
+   * @param value The value of the angle in radians.
+   */
+  @JsonCreator
+  public Rotation2d(@JsonProperty(required = true, value = "radians") double 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_cos = x / magnitude;
+      m_sin = y / magnitude;
+    } else {
+      m_cos = 1.0;
+      m_sin = 0.0;
+      MathSharedStore.reportError(
+          "x and y components of Rotation2d are zero\n", Thread.currentThread().getStackTrace());
+    }
+  }
+
+  /**
+   * Constructs a Rotation2d with the given angle.
+   *
+   * @param angle The angle of the rotation.
+   */
+  public Rotation2d(Angle angle) {
+    this(angle.in(Radians));
+  }
+
+  /**
+   * Constructs a Rotation2d from a rotation matrix.
+   *
+   * @param rotationMatrix The rotation matrix.
+   * @throws IllegalArgumentException if the rotation matrix isn't special orthogonal.
+   */
+  public Rotation2d(Matrix<N2, N2> rotationMatrix) {
+    final var R = rotationMatrix;
+
+    // Require that the rotation matrix is special orthogonal. This is true if
+    // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
+    if (R.times(R.transpose()).minus(Matrix.eye(Nat.N2())).normF() > 1e-9) {
+      var msg = "Rotation matrix isn't orthogonal\n\nR =\n" + R.getStorage().toString() + '\n';
+      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
+      throw new IllegalArgumentException(msg);
+    }
+    if (Math.abs(R.det() - 1.0) > 1e-9) {
+      var msg =
+          "Rotation matrix is orthogonal but not special orthogonal\n\nR =\n"
+              + R.getStorage().toString()
+              + '\n';
+      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
+      throw new IllegalArgumentException(msg);
+    }
+
+    // R = [cosθ  −sinθ]
+    //     [sinθ   cosθ]
+    m_cos = R.get(0, 0);
+    m_sin = R.get(1, 0);
+  }
+
+  /**
+   * Constructs and returns a Rotation2d with the given radian value.
+   *
+   * @param radians The value of the angle in radians.
+   * @return The rotation object with the desired angle value.
+   */
+  public static Rotation2d fromRadians(double radians) {
+    return new Rotation2d(radians);
+  }
+
+  /**
+   * Constructs and returns a Rotation2d with the given degree value.
+   *
+   * @param degrees The value of the angle in degrees.
+   * @return The rotation object with the desired angle value.
+   */
+  public static Rotation2d fromDegrees(double degrees) {
+    return new Rotation2d(Math.toRadians(degrees));
+  }
+
+  /**
+   * Constructs and returns a Rotation2d with the given number of rotations.
+   *
+   * @param rotations The value of the angle in rotations.
+   * @return The rotation object with the desired angle value.
+   */
+  public static Rotation2d fromRotations(double rotations) {
+    return new Rotation2d(Units.rotationsToRadians(rotations));
+  }
+
+  /**
+   * Adds two rotations together, with the result being bounded between -π and π.
+   *
+   * <p>For example, <code>Rotation2d.fromDegrees(30).plus(Rotation2d.fromDegrees(60))</code> equals
+   * <code>Rotation2d(Math.PI/2.0)</code>
+   *
+   * @param other The rotation to add.
+   * @return The sum of the two rotations.
+   */
+  public Rotation2d plus(Rotation2d other) {
+    return rotateBy(other);
+  }
+
+  /**
+   * Subtracts the new rotation from the current rotation and returns the new rotation.
+   *
+   * <p>For example, <code>Rotation2d.fromDegrees(10).minus(Rotation2d.fromDegrees(100))</code>
+   * equals <code>Rotation2d(-Math.PI/2.0)</code>
+   *
+   * @param other The rotation to subtract.
+   * @return The difference between the two rotations.
+   */
+  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_cos, -m_sin);
+  }
+
+  /**
+   * Multiplies the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Rotation2d.
+   */
+  public Rotation2d times(double scalar) {
+    return new Rotation2d(getRadians() * scalar);
+  }
+
+  /**
+   * Divides the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Rotation2d.
+   */
+  public Rotation2d div(double scalar) {
+    return times(1.0 / scalar);
+  }
+
+  /**
+   * Adds the new rotation to the current rotation using a rotation matrix.
+   *
+   * <p>The matrix multiplication is as follows:
+   *
+   * <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 matrix representation of this rotation.
+   *
+   * @return Matrix representation of this rotation.
+   */
+  public Matrix<N2, N2> toMatrix() {
+    // R = [cosθ  −sinθ]
+    //     [sinθ   cosθ]
+    return MatBuilder.fill(Nat.N2(), Nat.N2(), m_cos, -m_sin, m_sin, m_cos);
+  }
+
+  /**
+   * Returns the measure of the Rotation2d.
+   *
+   * @return The measure of the Rotation2d.
+   */
+  public Angle getMeasure() {
+    return Radians.of(getRadians());
+  }
+
+  /**
+   * Returns the radian value of the Rotation2d constrained within [-π, π].
+   *
+   * @return The radian value of the Rotation2d constrained within [-π, π].
+   */
+  @JsonProperty
+  public double getRadians() {
+    return Math.atan2(m_sin, m_cos);
+  }
+
+  /**
+   * Returns the degree value of the Rotation2d constrained within [-180, 180].
+   *
+   * @return The degree value of the Rotation2d constrained within [-180, 180].
+   */
+  public double getDegrees() {
+    return Math.toDegrees(getRadians());
+  }
+
+  /**
+   * Returns the number of rotations of the Rotation2d.
+   *
+   * @return The number of rotations of the Rotation2d.
+   */
+  public double getRotations() {
+    return Units.radiansToRotations(getRadians());
+  }
+
+  /**
+   * Returns the cosine of the Rotation2d.
+   *
+   * @return The cosine of the Rotation2d.
+   */
+  public double getCos() {
+    return m_cos;
+  }
+
+  /**
+   * Returns the sine of the Rotation2d.
+   *
+   * @return The sine of the Rotation2d.
+   */
+  public double getSin() {
+    return m_sin;
+  }
+
+  /**
+   * Returns the tangent of the Rotation2d.
+   *
+   * @return The tangent of the Rotation2d.
+   */
+  public double getTan() {
+    return m_sin / m_cos;
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", getRadians(), getDegrees());
+  }
+
+  /**
+   * 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) {
+    return obj instanceof Rotation2d other
+        && Math.hypot(m_cos - other.m_cos, m_sin - other.m_sin) < 1E-9;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(getRadians());
+  }
+
+  @Override
+  public Rotation2d interpolate(Rotation2d endValue, double t) {
+    return plus(endValue.minus(this).times(Math.clamp(t, 0, 1)));
+  }
+
+  /** Rotation2d protobuf for serialization. */
+  public static final Rotation2dProto proto = new Rotation2dProto();
+
+  /** Rotation2d struct for serialization. */
+  public static final Rotation2dStruct struct = new Rotation2dStruct();
+}

wpimath/src/main/java/org/wpilib/math/geometry/Rotation3d.java

@@ -0,0 +1,572 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static edu.wpi.first.units.Units.Radians;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.proto.Rotation3dProto;
+import edu.wpi.first.math.geometry.struct.Rotation3dStruct;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Objects;
+
+/** A rotation in a 3D coordinate frame represented by a quaternion. */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Rotation3d
+    implements Interpolatable<Rotation3d>, ProtobufSerializable, StructSerializable {
+  /**
+   * A preallocated Rotation3d representing no rotation.
+   *
+   * <p>This exists to avoid allocations for common rotations.
+   */
+  public static final Rotation3d kZero = new Rotation3d();
+
+  private final Quaternion m_q;
+
+  /** Constructs a Rotation3d representing no rotation. */
+  public Rotation3d() {
+    m_q = new Quaternion();
+  }
+
+  /**
+   * Constructs a Rotation3d from a quaternion.
+   *
+   * @param q The quaternion.
+   */
+  @JsonCreator
+  public Rotation3d(@JsonProperty(required = true, value = "quaternion") Quaternion q) {
+    m_q = q.normalize();
+  }
+
+  /**
+   * Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
+   *
+   * <p>Extrinsic rotations occur in that order around the axes in the fixed global frame rather
+   * than the body frame.
+   *
+   * <p>Angles are measured counterclockwise with the rotation axis pointing "out of the page". If
+   * you point your right thumb along the positive axis direction, your fingers curl in the
+   * direction of positive rotation.
+   *
+   * @param roll The counterclockwise rotation angle around the X axis (roll) in radians.
+   * @param pitch The counterclockwise rotation angle around the Y axis (pitch) in radians.
+   * @param yaw The counterclockwise rotation angle around the Z axis (yaw) in radians.
+   */
+  public Rotation3d(double roll, double pitch, double yaw) {
+    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
+    double cr = Math.cos(roll * 0.5);
+    double sr = Math.sin(roll * 0.5);
+
+    double cp = Math.cos(pitch * 0.5);
+    double sp = Math.sin(pitch * 0.5);
+
+    double cy = Math.cos(yaw * 0.5);
+    double sy = Math.sin(yaw * 0.5);
+
+    m_q =
+        new Quaternion(
+            cr * cp * cy + sr * sp * sy,
+            sr * cp * cy - cr * sp * sy,
+            cr * sp * cy + sr * cp * sy,
+            cr * cp * sy - sr * sp * cy);
+  }
+
+  /**
+   * Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
+   *
+   * <p>Extrinsic rotations occur in that order around the axes in the fixed global frame rather
+   * than the body frame.
+   *
+   * <p>Angles are measured counterclockwise with the rotation axis pointing "out of the page". If
+   * you point your right thumb along the positive axis direction, your fingers curl in the
+   * direction of positive rotation.
+   *
+   * @param roll The counterclockwise rotation angle around the X axis (roll).
+   * @param pitch The counterclockwise rotation angle around the Y axis (pitch).
+   * @param yaw The counterclockwise rotation angle around the Z axis (yaw).
+   */
+  public Rotation3d(Angle roll, Angle pitch, Angle yaw) {
+    this(roll.in(Radians), pitch.in(Radians), yaw.in(Radians));
+  }
+
+  /**
+   * Constructs a Rotation3d with the given rotation vector representation. This representation is
+   * equivalent to axis-angle, where the normalized axis is multiplied by the rotation around the
+   * axis in radians.
+   *
+   * @param rvec The rotation vector.
+   */
+  public Rotation3d(Vector<N3> rvec) {
+    this(rvec, rvec.norm());
+  }
+
+  /**
+   * Constructs a Rotation3d with the given axis-angle representation. The axis doesn't have to be
+   * normalized.
+   *
+   * @param axis The rotation axis.
+   * @param angleRadians The rotation around the axis in radians.
+   */
+  public Rotation3d(Vector<N3> axis, double angleRadians) {
+    double norm = axis.norm();
+    if (norm == 0.0) {
+      m_q = new Quaternion();
+      return;
+    }
+
+    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
+    var v = axis.times(1.0 / norm).times(Math.sin(angleRadians / 2.0));
+    m_q = new Quaternion(Math.cos(angleRadians / 2.0), v.get(0, 0), v.get(1, 0), v.get(2, 0));
+  }
+
+  /**
+   * Constructs a Rotation3d with the given axis-angle representation. The axis doesn't have to be
+   * normalized.
+   *
+   * @param axis The rotation axis.
+   * @param angle The rotation around the axis.
+   */
+  public Rotation3d(Vector<N3> axis, Angle angle) {
+    this(axis, angle.in(Radians));
+  }
+
+  /**
+   * Constructs a Rotation3d from a rotation matrix.
+   *
+   * @param rotationMatrix The rotation matrix.
+   * @throws IllegalArgumentException if the rotation matrix isn't special orthogonal.
+   */
+  public Rotation3d(Matrix<N3, N3> rotationMatrix) {
+    final var R = rotationMatrix;
+
+    // Require that the rotation matrix is special orthogonal. This is true if
+    // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
+    if (R.times(R.transpose()).minus(Matrix.eye(Nat.N3())).normF() > 1e-9) {
+      var msg = "Rotation matrix isn't orthogonal\n\nR =\n" + R.getStorage().toString() + '\n';
+      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
+      throw new IllegalArgumentException(msg);
+    }
+    if (Math.abs(R.det() - 1.0) > 1e-9) {
+      var msg =
+          "Rotation matrix is orthogonal but not special orthogonal\n\nR =\n"
+              + R.getStorage().toString()
+              + '\n';
+      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
+      throw new IllegalArgumentException(msg);
+    }
+
+    // Turn rotation matrix into a quaternion
+    // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+    double trace = R.get(0, 0) + R.get(1, 1) + R.get(2, 2);
+    double w;
+    double x;
+    double y;
+    double z;
+
+    if (trace > 0.0) {
+      double s = 0.5 / Math.sqrt(trace + 1.0);
+      w = 0.25 / s;
+      x = (R.get(2, 1) - R.get(1, 2)) * s;
+      y = (R.get(0, 2) - R.get(2, 0)) * s;
+      z = (R.get(1, 0) - R.get(0, 1)) * s;
+    } else {
+      if (R.get(0, 0) > R.get(1, 1) && R.get(0, 0) > R.get(2, 2)) {
+        double s = 2.0 * Math.sqrt(1.0 + R.get(0, 0) - R.get(1, 1) - R.get(2, 2));
+        w = (R.get(2, 1) - R.get(1, 2)) / s;
+        x = 0.25 * s;
+        y = (R.get(0, 1) + R.get(1, 0)) / s;
+        z = (R.get(0, 2) + R.get(2, 0)) / s;
+      } else if (R.get(1, 1) > R.get(2, 2)) {
+        double s = 2.0 * Math.sqrt(1.0 + R.get(1, 1) - R.get(0, 0) - R.get(2, 2));
+        w = (R.get(0, 2) - R.get(2, 0)) / s;
+        x = (R.get(0, 1) + R.get(1, 0)) / s;
+        y = 0.25 * s;
+        z = (R.get(1, 2) + R.get(2, 1)) / s;
+      } else {
+        double s = 2.0 * Math.sqrt(1.0 + R.get(2, 2) - R.get(0, 0) - R.get(1, 1));
+        w = (R.get(1, 0) - R.get(0, 1)) / s;
+        x = (R.get(0, 2) + R.get(2, 0)) / s;
+        y = (R.get(1, 2) + R.get(2, 1)) / s;
+        z = 0.25 * s;
+      }
+    }
+
+    m_q = new Quaternion(w, x, y, z);
+  }
+
+  /**
+   * Constructs a Rotation3d that rotates the initial vector onto the final vector.
+   *
+   * <p>This is useful for turning a 3D vector (final) into an orientation relative to a coordinate
+   * system vector (initial).
+   *
+   * @param initial The initial vector.
+   * @param last The final vector.
+   */
+  public Rotation3d(Vector<N3> initial, Vector<N3> last) {
+    double dot = initial.dot(last);
+    double normProduct = initial.norm() * last.norm();
+    double dotNorm = dot / normProduct;
+
+    if (dotNorm > 1.0 - 1E-9) {
+      // If the dot product is 1, the two vectors point in the same direction so
+      // there's no rotation. The default initialization of m_q will work.
+      m_q = new Quaternion();
+    } else if (dotNorm < -1.0 + 1E-9) {
+      // If the dot product is -1, the two vectors are antiparallel, so a 180°
+      // rotation is required. Any other vector can be used to generate an
+      // orthogonal one.
+
+      double x = Math.abs(initial.get(0, 0));
+      double y = Math.abs(initial.get(1, 0));
+      double z = Math.abs(initial.get(2, 0));
+
+      // Find vector that is most orthogonal to initial vector
+      Vector<N3> other;
+      if (x < y) {
+        if (x < z) {
+          // Use x-axis
+          other = VecBuilder.fill(1, 0, 0);
+        } else {
+          // Use z-axis
+          other = VecBuilder.fill(0, 0, 1);
+        }
+      } else {
+        if (y < z) {
+          // Use y-axis
+          other = VecBuilder.fill(0, 1, 0);
+        } else {
+          // Use z-axis
+          other = VecBuilder.fill(0, 0, 1);
+        }
+      }
+
+      var axis = Vector.cross(initial, other);
+
+      double axisNorm = axis.norm();
+      m_q =
+          new Quaternion(
+              0.0, axis.get(0, 0) / axisNorm, axis.get(1, 0) / axisNorm, axis.get(2, 0) / axisNorm);
+    } else {
+      var axis = Vector.cross(initial, last);
+
+      // https://stackoverflow.com/a/11741520
+      m_q =
+          new Quaternion(normProduct + dot, axis.get(0, 0), axis.get(1, 0), axis.get(2, 0))
+              .normalize();
+    }
+  }
+
+  /**
+   * Constructs a 3D rotation from a 2D rotation in the X-Y plane.
+   *
+   * @param rotation The 2D rotation.
+   * @see Pose3d#Pose3d(Pose2d)
+   * @see Transform3d#Transform3d(Transform2d)
+   */
+  public Rotation3d(Rotation2d rotation) {
+    this(0.0, 0.0, rotation.getRadians());
+  }
+
+  /**
+   * Adds two rotations together.
+   *
+   * @param other The rotation to add.
+   * @return The sum of the two rotations.
+   */
+  public Rotation3d plus(Rotation3d other) {
+    return rotateBy(other);
+  }
+
+  /**
+   * Subtracts the new rotation from the current rotation and returns the new rotation.
+   *
+   * @param other The rotation to subtract.
+   * @return The difference between the two rotations.
+   */
+  public Rotation3d minus(Rotation3d other) {
+    return rotateBy(other.unaryMinus());
+  }
+
+  /**
+   * Takes the inverse of the current rotation.
+   *
+   * @return The inverse of the current rotation.
+   */
+  public Rotation3d unaryMinus() {
+    return new Rotation3d(m_q.inverse());
+  }
+
+  /**
+   * Multiplies the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Rotation3d.
+   */
+  public Rotation3d times(double scalar) {
+    // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
+    if (m_q.getW() >= 0.0) {
+      return new Rotation3d(
+          VecBuilder.fill(m_q.getX(), m_q.getY(), m_q.getZ()),
+          2.0 * scalar * Math.acos(m_q.getW()));
+    } else {
+      return new Rotation3d(
+          VecBuilder.fill(-m_q.getX(), -m_q.getY(), -m_q.getZ()),
+          2.0 * scalar * Math.acos(-m_q.getW()));
+    }
+  }
+
+  /**
+   * Divides the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Rotation3d.
+   */
+  public Rotation3d div(double scalar) {
+    return times(1.0 / scalar);
+  }
+
+  /**
+   * Adds the new rotation to the current rotation. The other rotation is applied extrinsically,
+   * which means that it rotates around the global axes. For example, {@code new
+   * Rotation3d(Units.degreesToRadians(90), 0, 0).rotateBy(new Rotation3d(0,
+   * Units.degreesToRadians(45), 0))} rotates by 90 degrees around the +X axis and then by 45
+   * degrees around the global +Y axis. (This is equivalent to {@code new
+   * Rotation3d(Units.degreesToRadians(90), Units.degreesToRadians(45), 0)})
+   *
+   * @param other The extrinsic rotation to rotate by.
+   * @return The new rotated Rotation3d.
+   */
+  public Rotation3d rotateBy(Rotation3d other) {
+    return new Rotation3d(other.m_q.times(m_q));
+  }
+
+  /**
+   * Returns the quaternion representation of the Rotation3d.
+   *
+   * @return The quaternion representation of the Rotation3d.
+   */
+  @JsonProperty(value = "quaternion")
+  public Quaternion getQuaternion() {
+    return m_q;
+  }
+
+  /**
+   * Returns the counterclockwise rotation angle around the X axis (roll) in radians.
+   *
+   * @return The counterclockwise rotation angle around the X axis (roll) in radians.
+   */
+  public double getX() {
+    final var w = m_q.getW();
+    final var x = m_q.getX();
+    final var y = m_q.getY();
+    final var z = m_q.getZ();
+
+    // wpimath/algorithms.md
+    final var cxcy = 1.0 - 2.0 * (x * x + y * y);
+    final var sxcy = 2.0 * (w * x + y * z);
+    final var cy_sq = cxcy * cxcy + sxcy * sxcy;
+    if (cy_sq > 1e-20) {
+      return Math.atan2(sxcy, cxcy);
+    } else {
+      return 0.0;
+    }
+  }
+
+  /**
+   * Returns the counterclockwise rotation angle around the Y axis (pitch) in radians.
+   *
+   * @return The counterclockwise rotation angle around the Y axis (pitch) in radians.
+   */
+  public double getY() {
+    final var w = m_q.getW();
+    final var x = m_q.getX();
+    final var y = m_q.getY();
+    final var z = m_q.getZ();
+
+    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
+    double ratio = 2.0 * (w * y - z * x);
+    if (Math.abs(ratio) >= 1.0) {
+      return Math.copySign(Math.PI / 2.0, ratio);
+    } else {
+      return Math.asin(ratio);
+    }
+  }
+
+  /**
+   * Returns the counterclockwise rotation angle around the Z axis (yaw) in radians.
+   *
+   * @return The counterclockwise rotation angle around the Z axis (yaw) in radians.
+   */
+  public double getZ() {
+    final var w = m_q.getW();
+    final var x = m_q.getX();
+    final var y = m_q.getY();
+    final var z = m_q.getZ();
+
+    // wpimath/algorithms.md
+    final var cycz = 1.0 - 2.0 * (y * y + z * z);
+    final var cysz = 2.0 * (w * z + x * y);
+    final var cy_sq = cycz * cycz + cysz * cysz;
+    if (cy_sq > 1e-20) {
+      return Math.atan2(cysz, cycz);
+    } else {
+      return Math.atan2(2.0 * w * z, w * w - z * z);
+    }
+  }
+
+  /**
+   * Returns the counterclockwise rotation angle around the X axis (roll) in a measure.
+   *
+   * @return The counterclockwise rotation angle around the x axis (roll) in a measure.
+   */
+  public Angle getMeasureX() {
+    return Radians.of(getX());
+  }
+
+  /**
+   * Returns the counterclockwise rotation angle around the Y axis (pitch) in a measure.
+   *
+   * @return The counterclockwise rotation angle around the y axis (pitch) in a measure.
+   */
+  public Angle getMeasureY() {
+    return Radians.of(getY());
+  }
+
+  /**
+   * Returns the counterclockwise rotation angle around the Z axis (yaw) in a measure.
+   *
+   * @return The counterclockwise rotation angle around the z axis (yaw) in a measure.
+   */
+  public Angle getMeasureZ() {
+    return Radians.of(getZ());
+  }
+
+  /**
+   * Returns the axis in the axis-angle representation of this rotation.
+   *
+   * @return The axis in the axis-angle representation.
+   */
+  public Vector<N3> getAxis() {
+    double norm =
+        Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
+    if (norm == 0.0) {
+      return VecBuilder.fill(0.0, 0.0, 0.0);
+    } else {
+      return VecBuilder.fill(m_q.getX() / norm, m_q.getY() / norm, m_q.getZ() / norm);
+    }
+  }
+
+  /**
+   * Returns the angle in radians in the axis-angle representation of this rotation.
+   *
+   * @return The angle in radians in the axis-angle representation of this rotation.
+   */
+  public double getAngle() {
+    double norm =
+        Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
+    return 2.0 * Math.atan2(norm, m_q.getW());
+  }
+
+  /**
+   * Returns rotation matrix representation of this rotation.
+   *
+   * @return Rotation matrix representation of this rotation.
+   */
+  public Matrix<N3, N3> toMatrix() {
+    double w = m_q.getW();
+    double x = m_q.getX();
+    double y = m_q.getY();
+    double z = m_q.getZ();
+
+    // https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix
+    return MatBuilder.fill(
+        Nat.N3(),
+        Nat.N3(),
+        1.0 - 2.0 * (y * y + z * z),
+        2.0 * (x * y - w * z),
+        2.0 * (x * z + w * y),
+        2.0 * (x * y + w * z),
+        1.0 - 2.0 * (x * x + z * z),
+        2.0 * (y * z - w * x),
+        2.0 * (x * z - w * y),
+        2.0 * (y * z + w * x),
+        1.0 - 2.0 * (x * x + y * y));
+  }
+
+  /**
+   * Returns rotation vector representation of this rotation.
+   *
+   * @return Rotation vector representation of this rotation.
+   */
+  public Vector<N3> toVector() {
+    return m_q.toRotationVector();
+  }
+
+  /**
+   * Returns the angle in a measure in the axis-angle representation of this rotation.
+   *
+   * @return The angle in a measure in the axis-angle representation of this rotation.
+   */
+  public Angle getMeasureAngle() {
+    return Radians.of(getAngle());
+  }
+
+  /**
+   * Returns a Rotation2d representing this Rotation3d projected into the X-Y plane.
+   *
+   * @return A Rotation2d representing this Rotation3d projected into the X-Y plane.
+   */
+  public Rotation2d toRotation2d() {
+    return new Rotation2d(getZ());
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Rotation3d(%s)", m_q);
+  }
+
+  /**
+   * Checks equality between this Rotation3d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    return obj instanceof Rotation3d other
+        && Math.abs(Math.abs(m_q.dot(other.m_q)) - m_q.norm() * other.m_q.norm()) < 1e-9;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_q);
+  }
+
+  @Override
+  public Rotation3d interpolate(Rotation3d endValue, double t) {
+    return plus(endValue.minus(this).times(Math.clamp(t, 0, 1)));
+  }
+
+  /** Rotation3d protobuf for serialization. */
+  public static final Rotation3dProto proto = new Rotation3dProto();
+
+  /** Rotation3d struct for serialization. */
+  public static final Rotation3dStruct struct = new Rotation3dStruct();
+}

wpimath/src/main/java/org/wpilib/math/geometry/Transform2d.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.geometry;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.geometry.proto.Transform2dProto;
+import edu.wpi.first.math.geometry.struct.Transform2dStruct;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Objects;
+
+/** Represents a transformation for a Pose2d in the pose's frame. */
+public class Transform2d implements ProtobufSerializable, StructSerializable {
+  /**
+   * A preallocated Transform2d representing no transformation.
+   *
+   * <p>This exists to avoid allocations for common transformations.
+   */
+  public static final Transform2d kZero = new 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 a transform with x and y translations instead of a separate Translation2d.
+   *
+   * @param x The x component of the translational component of the transform.
+   * @param y The y component of the translational component of the transform.
+   * @param rotation The rotational component of the transform.
+   */
+  public Transform2d(double x, double y, Rotation2d rotation) {
+    m_translation = new Translation2d(x, y);
+    m_rotation = rotation;
+  }
+
+  /**
+   * Constructs a transform with x and y translations instead of a separate Translation2d. The X and
+   * Y translations will be converted to and tracked as meters.
+   *
+   * @param x The x component of the translational component of the transform.
+   * @param y The y component of the translational component of the transform.
+   * @param rotation The rotational component of the transform.
+   */
+  public Transform2d(Distance x, Distance y, Rotation2d rotation) {
+    this(x.in(Meters), y.in(Meters), rotation);
+  }
+
+  /**
+   * Constructs a transform with the specified affine transformation matrix.
+   *
+   * @param matrix The affine transformation matrix.
+   * @throws IllegalArgumentException if the affine transformation matrix is invalid.
+   */
+  public Transform2d(Matrix<N3, N3> matrix) {
+    m_translation = new Translation2d(matrix.get(0, 2), matrix.get(1, 2));
+    m_rotation = new Rotation2d(matrix.block(2, 2, 0, 0));
+    if (matrix.get(2, 0) != 0.0 || matrix.get(2, 1) != 0.0 || matrix.get(2, 2) != 1.0) {
+      throw new IllegalArgumentException("Affine transformation matrix is invalid");
+    }
+  }
+
+  /** Constructs the identity transform -- maps an initial pose to itself. */
+  public Transform2d() {
+    m_translation = Translation2d.kZero;
+    m_rotation = Rotation2d.kZero;
+  }
+
+  /**
+   * Multiplies 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));
+  }
+
+  /**
+   * Divides the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform2d.
+   */
+  public Transform2d div(double scalar) {
+    return times(1.0 / scalar);
+  }
+
+  /**
+   * Composes two transformations. The second transform is applied relative to the orientation of
+   * the first.
+   *
+   * @param other The transform to compose with this one.
+   * @return The composition of the two transformations.
+   */
+  public Transform2d plus(Transform2d other) {
+    return new Transform2d(Pose2d.kZero, Pose2d.kZero.transformBy(this).transformBy(other));
+  }
+
+  /**
+   * 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 X component of the transformation's translation in a measure.
+   *
+   * @return The x component of the transformation's translation in a measure.
+   */
+  public Distance getMeasureX() {
+    return m_translation.getMeasureX();
+  }
+
+  /**
+   * Returns the Y component of the transformation's translation in a measure.
+   *
+   * @return The y component of the transformation's translation in a measure.
+   */
+  public Distance getMeasureY() {
+    return m_translation.getMeasureY();
+  }
+
+  /**
+   * Returns an affine transformation matrix representation of this transformation.
+   *
+   * @return An affine transformation matrix representation of this transformation.
+   */
+  public Matrix<N3, N3> toMatrix() {
+    var vec = m_translation.toVector();
+    var mat = m_rotation.toMatrix();
+    return MatBuilder.fill(
+        Nat.N3(),
+        Nat.N3(),
+        mat.get(0, 0),
+        mat.get(0, 1),
+        vec.get(0),
+        mat.get(1, 0),
+        mat.get(1, 1),
+        vec.get(1),
+        0.0,
+        0.0,
+        1.0);
+  }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return Reference to the rotational component of the transform.
+   */
+  public Rotation2d getRotation() {
+    return m_rotation;
+  }
+
+  /**
+   * Returns a Twist2d of the current transform (pose delta). If b is the output of {@code a.log()},
+   * then {@code b.exp()} would yield a.
+   *
+   * @return The twist that maps the current transform.
+   */
+  public Twist2d log() {
+    final double dtheta = m_rotation.getRadians();
+    final double halfDtheta = dtheta / 2.0;
+
+    final double cosMinusOne = m_rotation.getCos() - 1;
+
+    double halfThetaByTanOfHalfDtheta;
+    if (Math.abs(cosMinusOne) < 1E-9) {
+      halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
+    } else {
+      halfThetaByTanOfHalfDtheta = -(halfDtheta * m_rotation.getSin()) / cosMinusOne;
+    }
+
+    Translation2d translationPart =
+        m_translation
+            .rotateBy(new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta))
+            .times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
+
+    return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
+  }
+
+  /**
+   * 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) {
+    return obj instanceof Transform2d other
+        && other.m_translation.equals(m_translation)
+        && other.m_rotation.equals(m_rotation);
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_translation, m_rotation);
+  }
+
+  /** Transform2d protobuf for serialization. */
+  public static final Transform2dProto proto = new Transform2dProto();
+
+  /** Transform2d struct for serialization. */
+  public static final Transform2dStruct struct = new Transform2dStruct();
+}

wpimath/src/main/java/org/wpilib/math/geometry/Transform3d.java

@@ -0,0 +1,323 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.geometry.proto.Transform3dProto;
+import edu.wpi.first.math.geometry.struct.Transform3dStruct;
+import edu.wpi.first.math.jni.Transform3dJNI;
+import edu.wpi.first.math.numbers.N4;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Objects;
+
+/** Represents a transformation for a Pose3d in the pose's frame. */
+public class Transform3d implements ProtobufSerializable, StructSerializable {
+  /**
+   * A preallocated Transform3d representing no transformation.
+   *
+   * <p>This exists to avoid allocations for common transformations.
+   */
+  public static final Transform3d kZero = new Transform3d();
+
+  private final Translation3d m_translation;
+  private final Rotation3d m_rotation;
+
+  /**
+   * Constructs the transform that maps the initial pose to the final pose.
+   *
+   * @param initial The initial pose for the transformation.
+   * @param last The final pose for the transformation.
+   */
+  public Transform3d(Pose3d initial, Pose3d last) {
+    // We are rotating the difference between the translations
+    // using a clockwise rotation matrix. This transforms the global
+    // delta into a local delta (relative to the initial pose).
+    m_translation =
+        last.getTranslation()
+            .minus(initial.getTranslation())
+            .rotateBy(initial.getRotation().unaryMinus());
+
+    m_rotation = last.getRotation().minus(initial.getRotation());
+  }
+
+  /**
+   * Constructs a transform with the given translation and rotation components.
+   *
+   * @param translation Translational component of the transform.
+   * @param rotation Rotational component of the transform.
+   */
+  public Transform3d(Translation3d translation, Rotation3d rotation) {
+    m_translation = translation;
+    m_rotation = rotation;
+  }
+
+  /**
+   * Constructs a transform with x, y, and z translations instead of a separate Translation3d.
+   *
+   * @param x The x component of the translational component of the transform.
+   * @param y The y component of the translational component of the transform.
+   * @param z The z component of the translational component of the transform.
+   * @param rotation The rotational component of the transform.
+   */
+  public Transform3d(double x, double y, double z, Rotation3d rotation) {
+    m_translation = new Translation3d(x, y, z);
+    m_rotation = rotation;
+  }
+
+  /**
+   * Constructs a transform with x, y, and z translations instead of a separate Translation3d. The
+   * X, Y, and Z translations will be converted to and tracked as meters.
+   *
+   * @param x The x component of the translational component of the transform.
+   * @param y The y component of the translational component of the transform.
+   * @param z The z component of the translational component of the transform.
+   * @param rotation The rotational component of the transform.
+   */
+  public Transform3d(Distance x, Distance y, Distance z, Rotation3d rotation) {
+    this(x.in(Meters), y.in(Meters), z.in(Meters), rotation);
+  }
+
+  /**
+   * Constructs a transform with the specified affine transformation matrix.
+   *
+   * @param matrix The affine transformation matrix.
+   * @throws IllegalArgumentException if the affine transformation matrix is invalid.
+   */
+  public Transform3d(Matrix<N4, N4> matrix) {
+    m_translation = new Translation3d(matrix.get(0, 3), matrix.get(1, 3), matrix.get(2, 3));
+    m_rotation = new Rotation3d(matrix.block(3, 3, 0, 0));
+    if (matrix.get(3, 0) != 0.0
+        || matrix.get(3, 1) != 0.0
+        || matrix.get(3, 2) != 0.0
+        || matrix.get(3, 3) != 1.0) {
+      throw new IllegalArgumentException("Affine transformation matrix is invalid");
+    }
+  }
+
+  /** Constructs the identity transform -- maps an initial pose to itself. */
+  public Transform3d() {
+    m_translation = Translation3d.kZero;
+    m_rotation = Rotation3d.kZero;
+  }
+
+  /**
+   * Constructs a 3D transform from a 2D transform in the X-Y plane.
+   *
+   * @param transform The 2D transform.
+   * @see Rotation3d#Rotation3d(Rotation2d)
+   * @see Translation3d#Translation3d(Translation2d)
+   */
+  public Transform3d(Transform2d transform) {
+    m_translation = new Translation3d(transform.getTranslation());
+    m_rotation = new Rotation3d(transform.getRotation());
+  }
+
+  /**
+   * Multiplies the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform3d.
+   */
+  public Transform3d times(double scalar) {
+    return new Transform3d(m_translation.times(scalar), m_rotation.times(scalar));
+  }
+
+  /**
+   * Divides the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform3d.
+   */
+  public Transform3d div(double scalar) {
+    return times(1.0 / scalar);
+  }
+
+  /**
+   * Composes two transformations. The second transform is applied relative to the orientation of
+   * the first.
+   *
+   * @param other The transform to compose with this one.
+   * @return The composition of the two transformations.
+   */
+  public Transform3d plus(Transform3d other) {
+    return new Transform3d(Pose3d.kZero, Pose3d.kZero.transformBy(this).transformBy(other));
+  }
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return The translational component of the transform.
+   */
+  public Translation3d getTranslation() {
+    return m_translation;
+  }
+
+  /**
+   * Returns the X component of the transformation's translation.
+   *
+   * @return The x component of the transformation's translation.
+   */
+  public double getX() {
+    return m_translation.getX();
+  }
+
+  /**
+   * Returns the Y component of the transformation's translation.
+   *
+   * @return The y component of the transformation's translation.
+   */
+  public double getY() {
+    return m_translation.getY();
+  }
+
+  /**
+   * Returns the Z component of the transformation's translation.
+   *
+   * @return The z component of the transformation's translation.
+   */
+  public double getZ() {
+    return m_translation.getZ();
+  }
+
+  /**
+   * Returns the X component of the transformation's translation in a measure.
+   *
+   * @return The x component of the transformation's translation in a measure.
+   */
+  public Distance getMeasureX() {
+    return m_translation.getMeasureX();
+  }
+
+  /**
+   * Returns the Y component of the transformation's translation in a measure.
+   *
+   * @return The y component of the transformation's translation in a measure.
+   */
+  public Distance getMeasureY() {
+    return m_translation.getMeasureY();
+  }
+
+  /**
+   * Returns the Z component of the transformation's translation in a measure.
+   *
+   * @return The z component of the transformation's translation in a measure.
+   */
+  public Distance getMeasureZ() {
+    return m_translation.getMeasureZ();
+  }
+
+  /**
+   * Returns an affine transformation matrix representation of this transformation.
+   *
+   * @return An affine transformation matrix representation of this transformation.
+   */
+  public Matrix<N4, N4> toMatrix() {
+    var vec = m_translation.toVector();
+    var mat = m_rotation.toMatrix();
+    return MatBuilder.fill(
+        Nat.N4(),
+        Nat.N4(),
+        mat.get(0, 0),
+        mat.get(0, 1),
+        mat.get(0, 2),
+        vec.get(0),
+        mat.get(1, 0),
+        mat.get(1, 1),
+        mat.get(1, 2),
+        vec.get(1),
+        mat.get(2, 0),
+        mat.get(2, 1),
+        mat.get(2, 2),
+        vec.get(2),
+        0.0,
+        0.0,
+        0.0,
+        1.0);
+  }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return Reference to the rotational component of the transform.
+   */
+  public Rotation3d getRotation() {
+    return m_rotation;
+  }
+
+  /**
+   * Returns a Twist3d of the current transform (pose delta). If b is the output of {@code a.log()},
+   * then {@code b.exp()} would yield a.
+   *
+   * @return The twist that maps the current transform.
+   */
+  public Twist3d log() {
+    var thisQuaternion = m_rotation.getQuaternion();
+    double[] resultArray =
+        Transform3dJNI.log(
+            this.getX(),
+            this.getY(),
+            this.getZ(),
+            thisQuaternion.getW(),
+            thisQuaternion.getX(),
+            thisQuaternion.getY(),
+            thisQuaternion.getZ());
+    return new Twist3d(
+        resultArray[0],
+        resultArray[1],
+        resultArray[2],
+        resultArray[3],
+        resultArray[4],
+        resultArray[5]);
+  }
+
+  /**
+   * Invert the transformation. This is useful for undoing a transformation.
+   *
+   * @return The inverted transformation.
+   */
+  public Transform3d inverse() {
+    // We are rotating the difference between the translations
+    // using a clockwise rotation matrix. This transforms the global
+    // delta into a local delta (relative to the initial pose).
+    return new Transform3d(
+        getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
+        getRotation().unaryMinus());
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Transform3d(%s, %s)", m_translation, m_rotation);
+  }
+
+  /**
+   * Checks equality between this Transform3d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    return obj instanceof Transform3d other
+        && other.m_translation.equals(m_translation)
+        && other.m_rotation.equals(m_rotation);
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_translation, m_rotation);
+  }
+
+  /** Transform3d protobuf for serialization. */
+  public static final Transform3dProto proto = new Transform3dProto();
+
+  /** Transform3d struct for serialization. */
+  public static final Transform3dStruct struct = new Transform3dStruct();
+}

wpimath/src/main/java/org/wpilib/math/geometry/Translation2d.java

@@ -0,0 +1,370 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.proto.Translation2dProto;
+import edu.wpi.first.math.geometry.struct.Translation2dStruct;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Collection;
+import java.util.Collections;
+import java.util.Comparator;
+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 at the
+ * origin facing in the positive X direction, forward is positive X and left is positive Y.
+ */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Translation2d
+    implements Interpolatable<Translation2d>, ProtobufSerializable, StructSerializable {
+  /**
+   * A preallocated Translation2d representing the origin.
+   *
+   * <p>This exists to avoid allocations for common translations.
+   */
+  public static final Translation2d kZero = new 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();
+  }
+
+  /**
+   * Constructs a Translation2d with the X and Y components equal to the provided values. The X and
+   * Y components will be converted to and tracked as meters.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   */
+  public Translation2d(Distance x, Distance y) {
+    this(x.in(Meters), y.in(Meters));
+  }
+
+  /**
+   * Constructs a Translation2d from a 2D translation vector. The values are assumed to be in
+   * meters.
+   *
+   * @param vector The translation vector.
+   */
+  public Translation2d(Vector<N2> vector) {
+    this(vector.get(0), vector.get(1));
+  }
+
+  /**
+   * Calculates the distance between two translations in 2D space.
+   *
+   * <p>The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²).
+   *
+   * @param other The translation to compute the distance to.
+   * @return The distance between the two translations.
+   */
+  public double getDistance(Translation2d other) {
+    return Math.hypot(other.m_x - m_x, other.m_y - m_y);
+  }
+
+  /**
+   * Calculates the square of the distance between two translations in 2D space. This is equivalent
+   * to squaring the result of {@link #getDistance(Translation2d)}, but avoids computing a square
+   * root.
+   *
+   * <p>The square of the distance between translations is defined as (x₂−x₁)²+(y₂−y₁)².
+   *
+   * @param other The translation to compute the squared distance to.
+   * @return The square of the distance between the two translations, in square meters.
+   */
+  public double getSquaredDistance(Translation2d other) {
+    double dx = other.m_x - m_x;
+    double dy = other.m_y - m_y;
+    return dx * dx + dy * dy;
+  }
+
+  /**
+   * 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 X component of the translation in a measure.
+   *
+   * @return The x component of the translation in a measure.
+   */
+  public Distance getMeasureX() {
+    return Meters.of(m_x);
+  }
+
+  /**
+   * Returns the Y component of the translation in a measure.
+   *
+   * @return The y component of the translation in a measure.
+   */
+  public Distance getMeasureY() {
+    return Meters.of(m_y);
+  }
+
+  /**
+   * Returns a 2D translation vector representation of this translation.
+   *
+   * @return A 2D translation vector representation of this translation.
+   */
+  public Vector<N2> toVector() {
+    return VecBuilder.fill(m_x, 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);
+  }
+
+  /**
+   * Returns the squared norm, or squared distance from the origin to the translation. This is
+   * equivalent to squaring the result of {@link #getNorm()}, but avoids computing a square root.
+   *
+   * @return The squared norm of the translation, in square meters.
+   */
+  public double getSquaredNorm() {
+    return m_x * m_x + m_y * m_y;
+  }
+
+  /**
+   * Returns the angle this translation forms with the positive X axis.
+   *
+   * @return The angle of the translation
+   */
+  public Rotation2d getAngle() {
+    return new Rotation2d(m_x, m_y);
+  }
+
+  /**
+   * Applies a rotation to the translation in 2D space.
+   *
+   * <p>This multiplies the translation vector by a counterclockwise rotation matrix of the given
+   * angle.
+   *
+   * <pre>
+   * [x_new]   [other.cos, -other.sin][x]
+   * [y_new] = [other.sin,  other.cos][y]
+   * </pre>
+   *
+   * <p>For example, rotating a Translation2d of &lt;2, 0&gt; by 90 degrees will return a
+   * Translation2d of &lt;0, 2&gt;.
+   *
+   * @param other The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  public Translation2d rotateBy(Rotation2d other) {
+    return new Translation2d(
+        m_x * other.getCos() - m_y * other.getSin(), m_x * other.getSin() + m_y * other.getCos());
+  }
+
+  /**
+   * Rotates this translation around another translation in 2D space.
+   *
+   * <pre>
+   * [x_new]   [rot.cos, -rot.sin][x - other.x]   [other.x]
+   * [y_new] = [rot.sin,  rot.cos][y - other.y] + [other.y]
+   * </pre>
+   *
+   * @param other The other translation to rotate around.
+   * @param rot The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  public Translation2d rotateAround(Translation2d other, Rotation2d rot) {
+    return new Translation2d(
+        (m_x - other.getX()) * rot.getCos() - (m_y - other.getY()) * rot.getSin() + other.getX(),
+        (m_x - other.getX()) * rot.getSin() + (m_y - other.getY()) * rot.getCos() + other.getY());
+  }
+
+  /**
+   * Computes the dot product between this translation and another translation in 2D space.
+   *
+   * <p>The dot product between two translations is defined as x₁x₂+y₁y₂.
+   *
+   * @param other The translation to compute the dot product with.
+   * @return The dot product between the two translations, in square meters.
+   */
+  public double dot(Translation2d other) {
+    return m_x * other.m_x + m_y * other.m_y;
+  }
+
+  /**
+   * Computes the cross product between this translation and another translation in 2D space.
+   *
+   * <p>The 2D cross product between two translations is defined as x₁y₂-x₂y₁.
+   *
+   * @param other The translation to compute the cross product with.
+   * @return The cross product between the two translations, in square meters.
+   */
+  public double cross(Translation2d other) {
+    return m_x * other.m_y - m_y * other.m_x;
+  }
+
+  /**
+   * Returns the sum of two translations in 2D space.
+   *
+   * <p>For example, Translation3d(1.0, 2.5) + Translation3d(2.0, 5.5) = Translation3d{3.0, 8.0).
+   *
+   * @param other The translation to add.
+   * @return The sum of the translations.
+   */
+  public Translation2d plus(Translation2d other) {
+    return new Translation2d(m_x + other.m_x, m_y + other.m_y);
+  }
+
+  /**
+   * Returns the difference between two translations.
+   *
+   * <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 negating all components of the translation.
+   *
+   * @return The inverse of the current translation.
+   */
+  public Translation2d unaryMinus() {
+    return new Translation2d(-m_x, -m_y);
+  }
+
+  /**
+   * Returns the translation multiplied by a scalar.
+   *
+   * <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);
+  }
+
+  /**
+   * Returns the translation divided by a scalar.
+   *
+   * <p>For example, Translation3d(2.0, 2.5) / 2 = Translation3d(1.0, 1.25).
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The reference to the new mutated object.
+   */
+  public Translation2d div(double scalar) {
+    return new Translation2d(m_x / scalar, m_y / scalar);
+  }
+
+  /**
+   * Returns the nearest Translation2d from a collection of translations.
+   *
+   * @param translations The collection of translations.
+   * @return The nearest Translation2d from the collection.
+   */
+  public Translation2d nearest(Collection<Translation2d> translations) {
+    return Collections.min(translations, Comparator.comparing(this::getDistance));
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
+  }
+
+  /**
+   * 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) {
+    return obj instanceof Translation2d other
+        && Math.abs(other.m_x - m_x) < 1E-9
+        && Math.abs(other.m_y - m_y) < 1E-9;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_x, m_y);
+  }
+
+  @Override
+  public Translation2d interpolate(Translation2d endValue, double t) {
+    return new Translation2d(
+        MathUtil.lerp(this.getX(), endValue.getX(), t),
+        MathUtil.lerp(this.getY(), endValue.getY(), t));
+  }
+
+  /** Translation2d protobuf for serialization. */
+  public static final Translation2dProto proto = new Translation2dProto();
+
+  /** Translation2d struct for serialization. */
+  public static final Translation2dStruct struct = new Translation2dStruct();
+}

wpimath/src/main/java/org/wpilib/math/geometry/Translation3d.java

@@ -0,0 +1,406 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.proto.Translation3dProto;
+import edu.wpi.first.math.geometry.struct.Translation3dStruct;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Collection;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.Objects;
+
+/**
+ * Represents a translation in 3D space. This object can be used to represent a point or a vector.
+ *
+ * <p>This assumes that you are using conventional mathematical axes. When the robot is at the
+ * origin facing in the positive X direction, forward is positive X, left is positive Y, and up is
+ * positive Z.
+ */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Translation3d
+    implements Interpolatable<Translation3d>, ProtobufSerializable, StructSerializable {
+  /**
+   * A preallocated Translation3d representing the origin.
+   *
+   * <p>This exists to avoid allocations for common translations.
+   */
+  public static final Translation3d kZero = new Translation3d();
+
+  private final double m_x;
+  private final double m_y;
+  private final double m_z;
+
+  /** Constructs a Translation3d with X, Y, and Z components equal to zero. */
+  public Translation3d() {
+    this(0.0, 0.0, 0.0);
+  }
+
+  /**
+   * Constructs a Translation3d with the X, Y, and Z components equal to the provided values.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   * @param z The z component of the translation.
+   */
+  @JsonCreator
+  public Translation3d(
+      @JsonProperty(required = true, value = "x") double x,
+      @JsonProperty(required = true, value = "y") double y,
+      @JsonProperty(required = true, value = "z") double z) {
+    m_x = x;
+    m_y = y;
+    m_z = z;
+  }
+
+  /**
+   * Constructs a Translation3d with the provided distance and angle. This is essentially converting
+   * from polar coordinates to Cartesian coordinates.
+   *
+   * @param distance The distance from the origin to the end of the translation.
+   * @param angle The angle between the x-axis and the translation vector.
+   */
+  public Translation3d(double distance, Rotation3d angle) {
+    final var rectangular = new Translation3d(distance, 0.0, 0.0).rotateBy(angle);
+    m_x = rectangular.getX();
+    m_y = rectangular.getY();
+    m_z = rectangular.getZ();
+  }
+
+  /**
+   * Constructs a Translation3d with the X, Y, and Z components equal to the provided values. The
+   * components will be converted to and tracked as meters.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   * @param z The z component of the translation.
+   */
+  public Translation3d(Distance x, Distance y, Distance z) {
+    this(x.in(Meters), y.in(Meters), z.in(Meters));
+  }
+
+  /**
+   * Constructs a 3D translation from a 2D translation in the X-Y plane.
+   *
+   * @param translation The 2D translation.
+   * @see Pose3d#Pose3d(Pose2d)
+   * @see Transform3d#Transform3d(Transform2d)
+   */
+  public Translation3d(Translation2d translation) {
+    this(translation.getX(), translation.getY(), 0.0);
+  }
+
+  /**
+   * Constructs a Translation3d from a 3D translation vector. The values are assumed to be in
+   * meters.
+   *
+   * @param vector The translation vector.
+   */
+  public Translation3d(Vector<N3> vector) {
+    this(vector.get(0), vector.get(1), vector.get(2));
+  }
+
+  /**
+   * Calculates the distance between two translations in 3D space.
+   *
+   * <p>The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²).
+   *
+   * @param other The translation to compute the distance to.
+   * @return The distance between the two translations.
+   */
+  public double getDistance(Translation3d other) {
+    double dx = other.m_x - m_x;
+    double dy = other.m_y - m_y;
+    double dz = other.m_z - m_z;
+    return Math.sqrt(dx * dx + dy * dy + dz * dz);
+  }
+
+  /**
+   * Calculates the squared distance between two translations in 3D space. This is equivalent to
+   * squaring the result of {@link #getDistance(Translation3d)}, but avoids computing a square root.
+   *
+   * <p>The squared distance between translations is defined as (x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)².
+   *
+   * @param other The translation to compute the squared distance to.
+   * @return The squared distance between the two translations.
+   */
+  public double getSquaredDistance(Translation3d other) {
+    double dx = other.m_x - m_x;
+    double dy = other.m_y - m_y;
+    double dz = other.m_z - m_z;
+    return dx * dx + dy * dy + dz * dz;
+  }
+
+  /**
+   * Returns the X component of the translation.
+   *
+   * @return The X component of the translation.
+   */
+  @JsonProperty
+  public double getX() {
+    return m_x;
+  }
+
+  /**
+   * Returns the Y component of the translation.
+   *
+   * @return The Y component of the translation.
+   */
+  @JsonProperty
+  public double getY() {
+    return m_y;
+  }
+
+  /**
+   * Returns the Z component of the translation.
+   *
+   * @return The Z component of the translation.
+   */
+  @JsonProperty
+  public double getZ() {
+    return m_z;
+  }
+
+  /**
+   * Returns the X component of the translation in a measure.
+   *
+   * @return The x component of the translation in a measure.
+   */
+  public Distance getMeasureX() {
+    return Meters.of(m_x);
+  }
+
+  /**
+   * Returns the Y component of the translation in a measure.
+   *
+   * @return The y component of the translation in a measure.
+   */
+  public Distance getMeasureY() {
+    return Meters.of(m_y);
+  }
+
+  /**
+   * Returns the Z component of the translation in a measure.
+   *
+   * @return The z component of the translation in a measure.
+   */
+  public Distance getMeasureZ() {
+    return Meters.of(m_z);
+  }
+
+  /**
+   * Returns a 2D translation vector representation of this translation.
+   *
+   * @return A 2D translation vector representation of this translation.
+   */
+  public Vector<N3> toVector() {
+    return VecBuilder.fill(m_x, m_y, m_z);
+  }
+
+  /**
+   * Returns the norm, or distance from the origin to the translation.
+   *
+   * @return The norm of the translation.
+   */
+  public double getNorm() {
+    return Math.sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
+  }
+
+  /**
+   * Returns the squared norm, or squared distance from the origin to the translation. This is
+   * equivalent to squaring the result of {@link #getNorm()}, but avoids computing a square root.
+   *
+   * @return The squared norm of the translation.
+   */
+  public double getSquaredNorm() {
+    return m_x * m_x + m_y * m_y + m_z * m_z;
+  }
+
+  /**
+   * Applies a rotation to the translation in 3D space.
+   *
+   * <p>For example, rotating a Translation3d of &lt;2, 0, 0&gt; by 90 degrees around the Z axis
+   * will return a Translation3d of &lt;0, 2, 0&gt;.
+   *
+   * @param other The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  public Translation3d rotateBy(Rotation3d other) {
+    final var p = new Quaternion(0.0, m_x, m_y, m_z);
+    final var qprime = other.getQuaternion().times(p).times(other.getQuaternion().inverse());
+    return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ());
+  }
+
+  /**
+   * Rotates this translation around another translation in 3D space.
+   *
+   * @param other The other translation to rotate around.
+   * @param rot The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  public Translation3d rotateAround(Translation3d other, Rotation3d rot) {
+    return this.minus(other).rotateBy(rot).plus(other);
+  }
+
+  /**
+   * Computes the dot product between this translation and another translation in 3D space.
+   *
+   * <p>The dot product between two translations is defined as x₁x₂+y₁y₂+z₁z₂.
+   *
+   * @param other The translation to compute the dot product with.
+   * @return The dot product between the two translations, in square meters.
+   */
+  public double dot(Translation3d other) {
+    return m_x * other.m_x + m_y * other.m_y + m_z * other.m_z;
+  }
+
+  /**
+   * Computes the cross product between this translation and another translation in 3D space. The
+   * resulting translation will be perpendicular to both translations.
+   *
+   * <p>The 3D cross product between two translations is defined as &lt;y₁z₂-y₂z₁, z₁x₂-z₂x₁,
+   * x₁y₂-x₂y₁&gt;.
+   *
+   * @param other The translation to compute the cross product with.
+   * @return The cross product between the two translations.
+   */
+  public Vector<N3> cross(Translation3d other) {
+    return VecBuilder.fill(
+        m_y * other.m_z - other.m_y * m_z,
+        m_z * other.m_x - other.m_z * m_x,
+        m_x * other.m_y - other.m_x * m_y);
+  }
+
+  /**
+   * Returns a Translation2d representing this Translation3d projected into the X-Y plane.
+   *
+   * @return A Translation2d representing this Translation3d projected into the X-Y plane.
+   */
+  public Translation2d toTranslation2d() {
+    return new Translation2d(m_x, m_y);
+  }
+
+  /**
+   * Returns the sum of two translations in 3D space.
+   *
+   * <p>For example, Translation3d(1.0, 2.5, 3.5) + Translation3d(2.0, 5.5, 7.5) =
+   * Translation3d{3.0, 8.0, 11.0).
+   *
+   * @param other The translation to add.
+   * @return The sum of the translations.
+   */
+  public Translation3d plus(Translation3d other) {
+    return new Translation3d(m_x + other.m_x, m_y + other.m_y, m_z + other.m_z);
+  }
+
+  /**
+   * Returns the difference between two translations.
+   *
+   * <p>For example, Translation3d(5.0, 4.0, 3.0) - Translation3d(1.0, 2.0, 3.0) =
+   * Translation3d(4.0, 2.0, 0.0).
+   *
+   * @param other The translation to subtract.
+   * @return The difference between the two translations.
+   */
+  public Translation3d minus(Translation3d other) {
+    return new Translation3d(m_x - other.m_x, m_y - other.m_y, m_z - other.m_z);
+  }
+
+  /**
+   * Returns the inverse of the current translation. This is equivalent to negating all components
+   * of the translation.
+   *
+   * @return The inverse of the current translation.
+   */
+  public Translation3d unaryMinus() {
+    return new Translation3d(-m_x, -m_y, -m_z);
+  }
+
+  /**
+   * Returns the translation multiplied by a scalar.
+   *
+   * <p>For example, Translation3d(2.0, 2.5, 4.5) * 2 = Translation3d(4.0, 5.0, 9.0).
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled translation.
+   */
+  public Translation3d times(double scalar) {
+    return new Translation3d(m_x * scalar, m_y * scalar, m_z * scalar);
+  }
+
+  /**
+   * Returns the translation divided by a scalar.
+   *
+   * <p>For example, Translation3d(2.0, 2.5, 4.5) / 2 = Translation3d(1.0, 1.25, 2.25).
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The reference to the new mutated object.
+   */
+  public Translation3d div(double scalar) {
+    return new Translation3d(m_x / scalar, m_y / scalar, m_z / scalar);
+  }
+
+  /**
+   * Returns the nearest Translation3d from a collection of translations.
+   *
+   * @param translations The collection of translations to find the nearest.
+   * @return The nearest Translation3d from the collection.
+   */
+  public Translation3d nearest(Collection<Translation3d> translations) {
+    return Collections.min(translations, Comparator.comparing(this::getDistance));
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Translation3d(X: %.2f, Y: %.2f, Z: %.2f)", m_x, m_y, m_z);
+  }
+
+  /**
+   * Checks equality between this Translation3d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    return obj instanceof Translation3d other
+        && Math.abs(other.m_x - m_x) < 1E-9
+        && Math.abs(other.m_y - m_y) < 1E-9
+        && Math.abs(other.m_z - m_z) < 1E-9;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_x, m_y, m_z);
+  }
+
+  @Override
+  public Translation3d interpolate(Translation3d endValue, double t) {
+    return new Translation3d(
+        MathUtil.lerp(this.getX(), endValue.getX(), t),
+        MathUtil.lerp(this.getY(), endValue.getY(), t),
+        MathUtil.lerp(this.getZ(), endValue.getZ(), t));
+  }
+
+  /** Translation3d protobuf for serialization. */
+  public static final Translation3dProto proto = new Translation3dProto();
+
+  /** Translation3d struct for serialization. */
+  public static final Translation3dStruct struct = new Translation3dStruct();
+}

wpimath/src/main/java/org/wpilib/math/geometry/Twist2d.java

@@ -0,0 +1,107 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import edu.wpi.first.math.geometry.proto.Twist2dProto;
+import edu.wpi.first.math.geometry.struct.Twist2dStruct;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Objects;
+
+/**
+ * A change in distance along a 2D arc since the last pose update. We can use ideas from
+ * differential calculus to create new Pose2d objects from a Twist2d and vice versa.
+ *
+ * <p>A Twist can be used to represent a difference between two poses.
+ */
+public class Twist2d implements ProtobufSerializable, StructSerializable {
+  /** Linear "dx" component. */
+  public double dx;
+
+  /** Linear "dy" component. */
+  public double dy;
+
+  /** Angular "dtheta" component (radians). */
+  public double dtheta;
+
+  /** Default constructor. */
+  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;
+  }
+
+  /**
+   * Obtain a new Transform2d 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 twist, the user will receive the pose delta.
+   *
+   * <p>"Exp" represents the pose exponential, which is solving a differential equation moving the
+   * pose forward in time.
+   *
+   * @return The pose delta of the robot.
+   */
+  public Transform2d exp() {
+    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;
+    }
+
+    return new Transform2d(
+        new Translation2d(dx * s - dy * c, dx * c + dy * s), new Rotation2d(cosTheta, sinTheta));
+  }
+
+  @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) {
+    return obj instanceof Twist2d other
+        && Math.abs(other.dx - dx) < 1E-9
+        && Math.abs(other.dy - dy) < 1E-9
+        && Math.abs(other.dtheta - dtheta) < 1E-9;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(dx, dy, dtheta);
+  }
+
+  /** Twist2d protobuf for serialization. */
+  public static final Twist2dProto proto = new Twist2dProto();
+
+  /** Twist2d struct for serialization. */
+  public static final Twist2dStruct struct = new Twist2dStruct();
+}

wpimath/src/main/java/org/wpilib/math/geometry/Twist3d.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.geometry;
+
+import edu.wpi.first.math.geometry.proto.Twist3dProto;
+import edu.wpi.first.math.geometry.struct.Twist3dStruct;
+import edu.wpi.first.math.jni.Twist3dJNI;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.util.Objects;
+
+/**
+ * A change in distance along a 3D arc since the last pose update. We can use ideas from
+ * differential calculus to create new Pose3d objects from a Twist3d and vice versa.
+ *
+ * <p>A Twist can be used to represent a difference between two poses.
+ */
+public class Twist3d implements ProtobufSerializable, StructSerializable {
+  /** Linear "dx" component. */
+  public double dx;
+
+  /** Linear "dy" component. */
+  public double dy;
+
+  /** Linear "dz" component. */
+  public double dz;
+
+  /** Rotation vector x component (radians). */
+  public double rx;
+
+  /** Rotation vector y component (radians). */
+  public double ry;
+
+  /** Rotation vector z component (radians). */
+  public double rz;
+
+  /** Default constructor. */
+  public Twist3d() {}
+
+  /**
+   * Constructs a Twist3d with the given values.
+   *
+   * @param dx Change in x direction relative to robot.
+   * @param dy Change in y direction relative to robot.
+   * @param dz Change in z direction relative to robot.
+   * @param rx Rotation vector x component.
+   * @param ry Rotation vector y component.
+   * @param rz Rotation vector z component.
+   */
+  public Twist3d(double dx, double dy, double dz, double rx, double ry, double rz) {
+    this.dx = dx;
+    this.dy = dy;
+    this.dz = dz;
+    this.rx = rx;
+    this.ry = ry;
+    this.rz = rz;
+  }
+
+  /**
+   * Obtain a new Transform3d 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 twist, the user will receive the pose delta.
+   *
+   * <p>"Exp" represents the pose exponential, which is solving a differential equation moving the
+   * pose forward in time.
+   *
+   * @return The pose delta of the robot.
+   */
+  public Transform3d exp() {
+    double[] resultArray = Twist3dJNI.exp(dx, dy, dz, rx, ry, rz);
+    return new Transform3d(
+        resultArray[0],
+        resultArray[1],
+        resultArray[2],
+        new Rotation3d(
+            new Quaternion(resultArray[3], resultArray[4], resultArray[5], resultArray[6])));
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "Twist3d(dX: %.2f, dY: %.2f, dZ: %.2f, rX: %.2f, rY: %.2f, rZ: %.2f)",
+        dx, dy, dz, rx, ry, rz);
+  }
+
+  /**
+   * Checks equality between this Twist3d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    return obj instanceof Twist3d other
+        && Math.abs(other.dx - dx) < 1E-9
+        && Math.abs(other.dy - dy) < 1E-9
+        && Math.abs(other.dz - dz) < 1E-9
+        && Math.abs(other.rx - rx) < 1E-9
+        && Math.abs(other.ry - ry) < 1E-9
+        && Math.abs(other.rz - rz) < 1E-9;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(dx, dy, dz, rx, ry, rz);
+  }
+
+  /** Twist3d protobuf for serialization. */
+  public static final Twist3dProto proto = new Twist3dProto();
+
+  /** Twist3d struct for serialization. */
+  public static final Twist3dStruct struct = new Twist3dStruct();
+}

wpimath/src/main/java/org/wpilib/math/geometry/proto/Ellipse2dProto.java

@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Ellipse2d;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufEllipse2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Ellipse2dProto implements Protobuf<Ellipse2d, ProtobufEllipse2d> {
+  @Override
+  public Class<Ellipse2d> getTypeClass() {
+    return Ellipse2d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufEllipse2d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufEllipse2d createMessage() {
+    return ProtobufEllipse2d.newInstance();
+  }
+
+  @Override
+  public Ellipse2d unpack(ProtobufEllipse2d msg) {
+    return new Ellipse2d(
+        Pose2d.proto.unpack(msg.getCenter()), msg.getXSemiAxis(), msg.getYSemiAxis());
+  }
+
+  @Override
+  public void pack(ProtobufEllipse2d msg, Ellipse2d value) {
+    Pose2d.proto.pack(msg.getMutableCenter(), value.getCenter());
+    msg.setXSemiAxis(value.getXSemiAxis());
+    msg.setYSemiAxis(value.getYSemiAxis());
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/proto/Pose2dProto.java

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+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.proto.Geometry2D.ProtobufPose2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Pose2dProto implements Protobuf<Pose2d, ProtobufPose2d> {
+  @Override
+  public Class<Pose2d> getTypeClass() {
+    return Pose2d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufPose2d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufPose2d createMessage() {
+    return ProtobufPose2d.newInstance();
+  }
+
+  @Override
+  public Pose2d unpack(ProtobufPose2d msg) {
+    return new Pose2d(
+        Translation2d.proto.unpack(msg.getTranslation()),
+        Rotation2d.proto.unpack(msg.getRotation()));
+  }
+
+  @Override
+  public void pack(ProtobufPose2d msg, Pose2d value) {
+    Translation2d.proto.pack(msg.getMutableTranslation(), value.getTranslation());
+    Rotation2d.proto.pack(msg.getMutableRotation(), value.getRotation());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/proto/Pose3dProto.java

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Pose3dProto implements Protobuf<Pose3d, ProtobufPose3d> {
+  @Override
+  public Class<Pose3d> getTypeClass() {
+    return Pose3d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufPose3d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufPose3d createMessage() {
+    return ProtobufPose3d.newInstance();
+  }
+
+  @Override
+  public Pose3d unpack(ProtobufPose3d msg) {
+    return new Pose3d(
+        Translation3d.proto.unpack(msg.getTranslation()),
+        Rotation3d.proto.unpack(msg.getRotation()));
+  }
+
+  @Override
+  public void pack(ProtobufPose3d msg, Pose3d value) {
+    Translation3d.proto.pack(msg.getMutableTranslation(), value.getTranslation());
+    Rotation3d.proto.pack(msg.getMutableRotation(), value.getRotation());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/proto/QuaternionProto.java

@@ -0,0 +1,45 @@
+// 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.proto;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class QuaternionProto implements Protobuf<Quaternion, ProtobufQuaternion> {
+  @Override
+  public Class<Quaternion> getTypeClass() {
+    return Quaternion.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufQuaternion.getDescriptor();
+  }
+
+  @Override
+  public ProtobufQuaternion createMessage() {
+    return ProtobufQuaternion.newInstance();
+  }
+
+  @Override
+  public Quaternion unpack(ProtobufQuaternion msg) {
+    return new Quaternion(msg.getW(), msg.getX(), msg.getY(), msg.getZ());
+  }
+
+  @Override
+  public void pack(ProtobufQuaternion msg, Quaternion value) {
+    msg.setW(value.getW());
+    msg.setX(value.getX());
+    msg.setY(value.getY());
+    msg.setZ(value.getZ());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/proto/Rectangle2dProto.java

@@ -0,0 +1,40 @@
+// 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.proto;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rectangle2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufRectangle2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Rectangle2dProto implements Protobuf<Rectangle2d, ProtobufRectangle2d> {
+  @Override
+  public Class<Rectangle2d> getTypeClass() {
+    return Rectangle2d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufRectangle2d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufRectangle2d createMessage() {
+    return ProtobufRectangle2d.newInstance();
+  }
+
+  @Override
+  public Rectangle2d unpack(ProtobufRectangle2d msg) {
+    return new Rectangle2d(Pose2d.proto.unpack(msg.getCenter()), msg.getXWidth(), msg.getYWidth());
+  }
+
+  @Override
+  public void pack(ProtobufRectangle2d msg, Rectangle2d value) {
+    Pose2d.proto.pack(msg.getMutableCenter(), value.getCenter());
+    msg.setXWidth(value.getXWidth());
+    msg.setYWidth(value.getYWidth());
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/proto/Rotation2dProto.java

@@ -0,0 +1,42 @@
+// 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.proto;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Rotation2dProto implements Protobuf<Rotation2d, ProtobufRotation2d> {
+  @Override
+  public Class<Rotation2d> getTypeClass() {
+    return Rotation2d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufRotation2d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufRotation2d createMessage() {
+    return ProtobufRotation2d.newInstance();
+  }
+
+  @Override
+  public Rotation2d unpack(ProtobufRotation2d msg) {
+    return new Rotation2d(msg.getValue());
+  }
+
+  @Override
+  public void pack(ProtobufRotation2d msg, Rotation2d value) {
+    msg.setValue(value.getRadians());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/proto/Rotation3dProto.java

@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufRotation3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Rotation3dProto implements Protobuf<Rotation3d, ProtobufRotation3d> {
+  @Override
+  public Class<Rotation3d> getTypeClass() {
+    return Rotation3d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufRotation3d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufRotation3d createMessage() {
+    return ProtobufRotation3d.newInstance();
+  }
+
+  @Override
+  public Rotation3d unpack(ProtobufRotation3d msg) {
+    return new Rotation3d(Quaternion.proto.unpack(msg.getQ()));
+  }
+
+  @Override
+  public void pack(ProtobufRotation3d msg, Rotation3d value) {
+    Quaternion.proto.pack(msg.getMutableQ(), value.getQuaternion());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/proto/Transform2dProto.java

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+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.proto.Geometry2D.ProtobufTransform2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Transform2dProto implements Protobuf<Transform2d, ProtobufTransform2d> {
+  @Override
+  public Class<Transform2d> getTypeClass() {
+    return Transform2d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufTransform2d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufTransform2d createMessage() {
+    return ProtobufTransform2d.newInstance();
+  }
+
+  @Override
+  public Transform2d unpack(ProtobufTransform2d msg) {
+    return new Transform2d(
+        Translation2d.proto.unpack(msg.getTranslation()),
+        Rotation2d.proto.unpack(msg.getRotation()));
+  }
+
+  @Override
+  public void pack(ProtobufTransform2d msg, Transform2d value) {
+    Translation2d.proto.pack(msg.getMutableTranslation(), value.getTranslation());
+    Rotation2d.proto.pack(msg.getMutableRotation(), value.getRotation());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/proto/Transform3dProto.java

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Transform3dProto implements Protobuf<Transform3d, ProtobufTransform3d> {
+  @Override
+  public Class<Transform3d> getTypeClass() {
+    return Transform3d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufTransform3d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufTransform3d createMessage() {
+    return ProtobufTransform3d.newInstance();
+  }
+
+  @Override
+  public Transform3d unpack(ProtobufTransform3d msg) {
+    return new Transform3d(
+        Translation3d.proto.unpack(msg.getTranslation()),
+        Rotation3d.proto.unpack(msg.getRotation()));
+  }
+
+  @Override
+  public void pack(ProtobufTransform3d msg, Transform3d value) {
+    Translation3d.proto.pack(msg.getMutableTranslation(), value.getTranslation());
+    Rotation3d.proto.pack(msg.getMutableRotation(), value.getRotation());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/proto/Translation2dProto.java

@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Translation2dProto implements Protobuf<Translation2d, ProtobufTranslation2d> {
+  @Override
+  public Class<Translation2d> getTypeClass() {
+    return Translation2d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufTranslation2d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufTranslation2d createMessage() {
+    return ProtobufTranslation2d.newInstance();
+  }
+
+  @Override
+  public Translation2d unpack(ProtobufTranslation2d msg) {
+    return new Translation2d(msg.getX(), msg.getY());
+  }
+
+  @Override
+  public void pack(ProtobufTranslation2d msg, Translation2d value) {
+    msg.setX(value.getX());
+    msg.setY(value.getY());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/proto/Translation3dProto.java

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Translation3dProto implements Protobuf<Translation3d, ProtobufTranslation3d> {
+  @Override
+  public Class<Translation3d> getTypeClass() {
+    return Translation3d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufTranslation3d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufTranslation3d createMessage() {
+    return ProtobufTranslation3d.newInstance();
+  }
+
+  @Override
+  public Translation3d unpack(ProtobufTranslation3d msg) {
+    return new Translation3d(msg.getX(), msg.getY(), msg.getZ());
+  }
+
+  @Override
+  public void pack(ProtobufTranslation3d msg, Translation3d value) {
+    msg.setX(value.getX());
+    msg.setY(value.getY());
+    msg.setZ(value.getZ());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/proto/Twist2dProto.java

@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTwist2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Twist2dProto implements Protobuf<Twist2d, ProtobufTwist2d> {
+  @Override
+  public Class<Twist2d> getTypeClass() {
+    return Twist2d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufTwist2d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufTwist2d createMessage() {
+    return ProtobufTwist2d.newInstance();
+  }
+
+  @Override
+  public Twist2d unpack(ProtobufTwist2d msg) {
+    return new Twist2d(msg.getDx(), msg.getDy(), msg.getDtheta());
+  }
+
+  @Override
+  public void pack(ProtobufTwist2d msg, Twist2d value) {
+    msg.setDx(value.dx);
+    msg.setDy(value.dy);
+    msg.setDtheta(value.dtheta);
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/proto/Twist3dProto.java

@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import edu.wpi.first.math.geometry.Twist3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTwist3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Twist3dProto implements Protobuf<Twist3d, ProtobufTwist3d> {
+  @Override
+  public Class<Twist3d> getTypeClass() {
+    return Twist3d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufTwist3d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufTwist3d createMessage() {
+    return ProtobufTwist3d.newInstance();
+  }
+
+  @Override
+  public Twist3d unpack(ProtobufTwist3d msg) {
+    return new Twist3d(
+        msg.getDx(), msg.getDy(), msg.getDz(), msg.getRx(), msg.getRy(), msg.getRz());
+  }
+
+  @Override
+  public void pack(ProtobufTwist3d msg, Twist3d value) {
+    msg.setDx(value.dx);
+    msg.setDy(value.dy);
+    msg.setDz(value.dz);
+    msg.setRx(value.rx);
+    msg.setRy(value.ry);
+    msg.setRz(value.rz);
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/struct/Ellipse2dStruct.java

@@ -0,0 +1,52 @@
+// 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.struct;
+
+import edu.wpi.first.math.geometry.Ellipse2d;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Ellipse2dStruct implements Struct<Ellipse2d> {
+  @Override
+  public Class<Ellipse2d> getTypeClass() {
+    return Ellipse2d.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "Ellipse2d";
+  }
+
+  @Override
+  public int getSize() {
+    return Pose2d.struct.getSize() + kSizeDouble + kSizeDouble;
+  }
+
+  @Override
+  public String getSchema() {
+    return "Pose2d center;double xSemiAxis; double ySemiAxis";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Pose2d.struct};
+  }
+
+  @Override
+  public Ellipse2d unpack(ByteBuffer bb) {
+    Pose2d center = Pose2d.struct.unpack(bb);
+    double xSemiAxis = bb.getDouble();
+    double ySemiAxis = bb.getDouble();
+    return new Ellipse2d(center, xSemiAxis, ySemiAxis);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Ellipse2d value) {
+    Pose2d.struct.pack(bb, value.getCenter());
+    bb.putDouble(value.getXSemiAxis());
+    bb.putDouble(value.getYSemiAxis());
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/struct/Pose2dStruct.java

@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+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.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Pose2dStruct implements Struct<Pose2d> {
+  @Override
+  public Class<Pose2d> getTypeClass() {
+    return Pose2d.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "Pose2d";
+  }
+
+  @Override
+  public int getSize() {
+    return Translation2d.struct.getSize() + Rotation2d.struct.getSize();
+  }
+
+  @Override
+  public String getSchema() {
+    return "Translation2d translation;Rotation2d rotation";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Translation2d.struct, Rotation2d.struct};
+  }
+
+  @Override
+  public Pose2d unpack(ByteBuffer bb) {
+    Translation2d translation = Translation2d.struct.unpack(bb);
+    Rotation2d rotation = Rotation2d.struct.unpack(bb);
+    return new Pose2d(translation, rotation);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Pose2d value) {
+    Translation2d.struct.pack(bb, value.getTranslation());
+    Rotation2d.struct.pack(bb, value.getRotation());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/struct/Pose3dStruct.java

@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Pose3dStruct implements Struct<Pose3d> {
+  @Override
+  public Class<Pose3d> getTypeClass() {
+    return Pose3d.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "Pose3d";
+  }
+
+  @Override
+  public int getSize() {
+    return Translation3d.struct.getSize() + Rotation3d.struct.getSize();
+  }
+
+  @Override
+  public String getSchema() {
+    return "Translation3d translation;Rotation3d rotation";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Translation3d.struct, Rotation3d.struct};
+  }
+
+  @Override
+  public Pose3d unpack(ByteBuffer bb) {
+    Translation3d translation = Translation3d.struct.unpack(bb);
+    Rotation3d rotation = Rotation3d.struct.unpack(bb);
+    return new Pose3d(translation, rotation);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Pose3d value) {
+    Translation3d.struct.pack(bb, value.getTranslation());
+    Rotation3d.struct.pack(bb, value.getRotation());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/struct/QuaternionStruct.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.geometry.struct;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class QuaternionStruct implements Struct<Quaternion> {
+  @Override
+  public Class<Quaternion> getTypeClass() {
+    return Quaternion.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "Quaternion";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 4;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double w;double x;double y;double z";
+  }
+
+  @Override
+  public Quaternion unpack(ByteBuffer bb) {
+    double w = bb.getDouble();
+    double x = bb.getDouble();
+    double y = bb.getDouble();
+    double z = bb.getDouble();
+    return new Quaternion(w, x, y, z);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Quaternion value) {
+    bb.putDouble(value.getW());
+    bb.putDouble(value.getX());
+    bb.putDouble(value.getY());
+    bb.putDouble(value.getZ());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/struct/Rectangle2dStruct.java

@@ -0,0 +1,52 @@
+// 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.struct;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rectangle2d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Rectangle2dStruct implements Struct<Rectangle2d> {
+  @Override
+  public Class<Rectangle2d> getTypeClass() {
+    return Rectangle2d.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "Rectangle2d";
+  }
+
+  @Override
+  public int getSize() {
+    return Pose2d.struct.getSize() + kSizeDouble + kSizeDouble;
+  }
+
+  @Override
+  public String getSchema() {
+    return "Pose2d center;double xWidth; double yWidth";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Pose2d.struct};
+  }
+
+  @Override
+  public Rectangle2d unpack(ByteBuffer bb) {
+    Pose2d center = Pose2d.struct.unpack(bb);
+    double width = bb.getDouble();
+    double height = bb.getDouble();
+    return new Rectangle2d(center, width, height);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Rectangle2d value) {
+    Pose2d.struct.pack(bb, value.getCenter());
+    bb.putDouble(value.getXWidth());
+    bb.putDouble(value.getYWidth());
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/struct/Rotation2dStruct.java

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Rotation2dStruct implements Struct<Rotation2d> {
+  @Override
+  public Class<Rotation2d> getTypeClass() {
+    return Rotation2d.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "Rotation2d";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double value";
+  }
+
+  @Override
+  public Rotation2d unpack(ByteBuffer bb) {
+    double value = bb.getDouble();
+    return new Rotation2d(value);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Rotation2d value) {
+    bb.putDouble(value.getRadians());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/struct/Rotation3dStruct.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.geometry.struct;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Rotation3dStruct implements Struct<Rotation3d> {
+  @Override
+  public Class<Rotation3d> getTypeClass() {
+    return Rotation3d.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "Rotation3d";
+  }
+
+  @Override
+  public int getSize() {
+    return Quaternion.struct.getSize();
+  }
+
+  @Override
+  public String getSchema() {
+    return "Quaternion q";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Quaternion.struct};
+  }
+
+  @Override
+  public Rotation3d unpack(ByteBuffer bb) {
+    Quaternion q = Quaternion.struct.unpack(bb);
+    return new Rotation3d(q);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Rotation3d value) {
+    Quaternion.struct.pack(bb, value.getQuaternion());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/struct/Transform2dStruct.java

@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+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.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Transform2dStruct implements Struct<Transform2d> {
+  @Override
+  public Class<Transform2d> getTypeClass() {
+    return Transform2d.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "Transform2d";
+  }
+
+  @Override
+  public int getSize() {
+    return Translation2d.struct.getSize() + Rotation2d.struct.getSize();
+  }
+
+  @Override
+  public String getSchema() {
+    return "Translation2d translation;Rotation2d rotation";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Translation2d.struct, Rotation2d.struct};
+  }
+
+  @Override
+  public Transform2d unpack(ByteBuffer bb) {
+    Translation2d translation = Translation2d.struct.unpack(bb);
+    Rotation2d rotation = Rotation2d.struct.unpack(bb);
+    return new Transform2d(translation, rotation);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Transform2d value) {
+    Translation2d.struct.pack(bb, value.getTranslation());
+    Rotation2d.struct.pack(bb, value.getRotation());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/struct/Transform3dStruct.java

@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Transform3dStruct implements Struct<Transform3d> {
+  @Override
+  public Class<Transform3d> getTypeClass() {
+    return Transform3d.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "Transform3d";
+  }
+
+  @Override
+  public int getSize() {
+    return Translation3d.struct.getSize() + Rotation3d.struct.getSize();
+  }
+
+  @Override
+  public String getSchema() {
+    return "Translation3d translation;Rotation3d rotation";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Translation3d.struct, Rotation3d.struct};
+  }
+
+  @Override
+  public Transform3d unpack(ByteBuffer bb) {
+    Translation3d translation = Translation3d.struct.unpack(bb);
+    Rotation3d rotation = Rotation3d.struct.unpack(bb);
+    return new Transform3d(translation, rotation);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Transform3d value) {
+    Translation3d.struct.pack(bb, value.getTranslation());
+    Rotation3d.struct.pack(bb, value.getRotation());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/struct/Translation2dStruct.java

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Translation2dStruct implements Struct<Translation2d> {
+  @Override
+  public Class<Translation2d> getTypeClass() {
+    return Translation2d.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "Translation2d";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 2;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double x;double y";
+  }
+
+  @Override
+  public Translation2d unpack(ByteBuffer bb) {
+    double x = bb.getDouble();
+    double y = bb.getDouble();
+    return new Translation2d(x, y);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Translation2d value) {
+    bb.putDouble(value.getX());
+    bb.putDouble(value.getY());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/struct/Translation3dStruct.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.geometry.struct;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Translation3dStruct implements Struct<Translation3d> {
+  @Override
+  public Class<Translation3d> getTypeClass() {
+    return Translation3d.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "Translation3d";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 3;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double x;double y;double z";
+  }
+
+  @Override
+  public Translation3d unpack(ByteBuffer bb) {
+    double x = bb.getDouble();
+    double y = bb.getDouble();
+    double z = bb.getDouble();
+    return new Translation3d(x, y, z);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Translation3d value) {
+    bb.putDouble(value.getX());
+    bb.putDouble(value.getY());
+    bb.putDouble(value.getZ());
+  }
+
+  @Override
+  public boolean isImmutable() {
+    return true;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/struct/Twist2dStruct.java

@@ -0,0 +1,46 @@
+// 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.struct;
+
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Twist2dStruct implements Struct<Twist2d> {
+  @Override
+  public Class<Twist2d> getTypeClass() {
+    return Twist2d.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "Twist2d";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 3;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double dx;double dy;double dtheta";
+  }
+
+  @Override
+  public Twist2d unpack(ByteBuffer bb) {
+    double dx = bb.getDouble();
+    double dy = bb.getDouble();
+    double dtheta = bb.getDouble();
+    return new Twist2d(dx, dy, dtheta);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Twist2d value) {
+    bb.putDouble(value.dx);
+    bb.putDouble(value.dy);
+    bb.putDouble(value.dtheta);
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/struct/Twist3dStruct.java

@@ -0,0 +1,52 @@
+// 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.struct;
+
+import edu.wpi.first.math.geometry.Twist3d;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class Twist3dStruct implements Struct<Twist3d> {
+  @Override
+  public Class<Twist3d> getTypeClass() {
+    return Twist3d.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "Twist3d";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 6;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double dx;double dy;double dz;double rx;double ry;double rz";
+  }
+
+  @Override
+  public Twist3d unpack(ByteBuffer bb) {
+    double dx = bb.getDouble();
+    double dy = bb.getDouble();
+    double dz = bb.getDouble();
+    double rx = bb.getDouble();
+    double ry = bb.getDouble();
+    double rz = bb.getDouble();
+    return new Twist3d(dx, dy, dz, rx, ry, rz);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, Twist3d value) {
+    bb.putDouble(value.dx);
+    bb.putDouble(value.dy);
+    bb.putDouble(value.dz);
+    bb.putDouble(value.rx);
+    bb.putDouble(value.ry);
+    bb.putDouble(value.rz);
+  }
+}

wpimath/src/main/java/org/wpilib/math/interpolation/Interpolatable.java

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+/**
+ * An object should extend interpolatable if you wish to interpolate between a lower and upper
+ * bound, such as a robot position on the field between timesteps. This behavior can be linear or
+ * nonlinear.
+ *
+ * @param <T> The class that is interpolatable.
+ */
+@SuppressWarnings("PMD.ImplicitFunctionalInterface")
+public interface Interpolatable<T> {
+  /**
+   * Return the interpolated value. This object is assumed to be the starting position, or lower
+   * bound.
+   *
+   * @param endValue The upper bound, or end.
+   * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
+   * @return The interpolated value.
+   */
+  T interpolate(T endValue, double t);
+}

wpimath/src/main/java/org/wpilib/math/interpolation/InterpolatingDoubleTreeMap.java

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import java.util.Map;
+
+/**
+ * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
+ * from points that are defined. This uses linear interpolation.
+ *
+ * <p>Example of use:
+ *
+ * <pre><code>
+ * InterpolatingDoubleTreeMap table = new InterpolatingDoubleTreeMap();
+ * table.put(0.0, 0.0);
+ * table.put(1.0, 10.0);
+ * table.put(2.0, 30.0);
+ * // ...
+ * double result = table.get(1.5); // Returns 20.0
+ * </code></pre>
+ */
+public class InterpolatingDoubleTreeMap extends InterpolatingTreeMap<Double, Double> {
+  /** Default constructor. */
+  public InterpolatingDoubleTreeMap() {
+    super(InverseInterpolator.forDouble(), Interpolator.forDouble());
+  }
+
+  /**
+   * Creates an {@link InterpolatingDoubleTreeMap} from the given entries.
+   *
+   * @param entries The entries to add to the map.
+   * @return The map filled with the {@code entries}.
+   */
+  @SafeVarargs
+  public static InterpolatingDoubleTreeMap ofEntries(Map.Entry<Double, Double>... entries) {
+    InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap();
+    for (var entry : entries) {
+      map.put(entry.getKey(), entry.getValue());
+    }
+    return map;
+  }
+}

wpimath/src/main/java/org/wpilib/math/interpolation/InterpolatingMatrixTreeMap.java

@@ -0,0 +1,98 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import java.util.TreeMap;
+
+/**
+ * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
+ * from points that are defined. This uses linear interpolation.
+ *
+ * @param <K> Key type.
+ * @param <R> Number of matrix rows.
+ * @param <C> Number of matrix columns.
+ */
+public class InterpolatingMatrixTreeMap<K extends Number, R extends Num, C extends Num> {
+  private final TreeMap<K, Matrix<R, C>> m_map = new TreeMap<>();
+
+  /** Default constructor. */
+  public InterpolatingMatrixTreeMap() {}
+
+  /**
+   * Inserts a key-value pair.
+   *
+   * @param key The key.
+   * @param value The value.
+   */
+  public void put(K key, Matrix<R, C> value) {
+    m_map.put(key, value);
+  }
+
+  /**
+   * Returns the value associated with a given key.
+   *
+   * <p>If there's no matching key, the value returned will be a linear interpolation between the
+   * keys before and after the provided one.
+   *
+   * @param key The key.
+   * @return The value associated with the given key.
+   */
+  public Matrix<R, C> get(K key) {
+    Matrix<R, C> val = m_map.get(key);
+    if (val == null) {
+      K ceilingKey = m_map.ceilingKey(key);
+      K floorKey = m_map.floorKey(key);
+
+      if (ceilingKey == null && floorKey == null) {
+        return null;
+      }
+      if (ceilingKey == null) {
+        return m_map.get(floorKey);
+      }
+      if (floorKey == null) {
+        return m_map.get(ceilingKey);
+      }
+      Matrix<R, C> floor = m_map.get(floorKey);
+      Matrix<R, C> ceiling = m_map.get(ceilingKey);
+
+      return interpolate(floor, ceiling, inverseInterpolate(ceilingKey, key, floorKey));
+    } else {
+      return val;
+    }
+  }
+
+  /**
+   * Return the value interpolated between val1 and val2 by the interpolant d.
+   *
+   * @param val1 The lower part of the interpolation range.
+   * @param val2 The upper part of the interpolation range.
+   * @param d The interpolant in the range [0, 1].
+   * @return The interpolated value.
+   */
+  public Matrix<R, C> interpolate(Matrix<R, C> val1, Matrix<R, C> val2, double d) {
+    var dydx = val2.minus(val1);
+    return dydx.times(d).plus(val1);
+  }
+
+  /**
+   * Return where within interpolation range [0, 1] q is between down and up.
+   *
+   * @param up Upper part of interpolation range.
+   * @param q Query.
+   * @param down Lower part of interpolation range.
+   * @return Interpolant in range [0, 1].
+   */
+  public double inverseInterpolate(K up, K q, K down) {
+    double upperToLower = up.doubleValue() - down.doubleValue();
+    if (upperToLower <= 0) {
+      return 0.0;
+    }
+    double queryToLower = q.doubleValue() - down.doubleValue();
+    if (queryToLower <= 0) {
+      return 0.0;
+    }
+    return queryToLower / upperToLower;
+  }
+}

wpimath/src/main/java/org/wpilib/math/interpolation/InterpolatingTreeMap.java

@@ -0,0 +1,104 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import java.util.Comparator;
+import java.util.TreeMap;
+
+/**
+ * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
+ * from points that are defined. This uses linear interpolation.
+ *
+ * <p>{@code K} must implement {@link Comparable}, or a {@link Comparator} on {@code K} can be
+ * provided.
+ *
+ * @param <K> The type of keys held in this map.
+ * @param <V> The type of values held in this map.
+ */
+public class InterpolatingTreeMap<K, V> {
+  private final TreeMap<K, V> m_map;
+
+  private final InverseInterpolator<K> m_inverseInterpolator;
+  private final Interpolator<V> m_interpolator;
+
+  /**
+   * Constructs an InterpolatingTreeMap.
+   *
+   * @param inverseInterpolator Function to use for inverse interpolation of the keys.
+   * @param interpolator Function to use for interpolation of the values.
+   */
+  public InterpolatingTreeMap(
+      InverseInterpolator<K> inverseInterpolator, Interpolator<V> interpolator) {
+    m_map = new TreeMap<>();
+    m_inverseInterpolator = inverseInterpolator;
+    m_interpolator = interpolator;
+  }
+
+  /**
+   * Constructs an InterpolatingTreeMap using {@code comparator}.
+   *
+   * @param inverseInterpolator Function to use for inverse interpolation of the keys.
+   * @param interpolator Function to use for interpolation of the values.
+   * @param comparator Comparator to use on keys.
+   */
+  public InterpolatingTreeMap(
+      InverseInterpolator<K> inverseInterpolator,
+      Interpolator<V> interpolator,
+      Comparator<K> comparator) {
+    m_inverseInterpolator = inverseInterpolator;
+    m_interpolator = interpolator;
+    m_map = new TreeMap<>(comparator);
+  }
+
+  /**
+   * Inserts a key-value pair.
+   *
+   * @param key The key.
+   * @param value The value.
+   */
+  public void put(K key, V value) {
+    m_map.put(key, value);
+  }
+
+  /**
+   * Returns the value associated with a given key.
+   *
+   * <p>If there's no matching key, the value returned will be an interpolation between the keys
+   * before and after the provided one, using the {@link Interpolator} and {@link
+   * InverseInterpolator} provided.
+   *
+   * @param key The key.
+   * @return The value associated with the given key.
+   */
+  public V get(K key) {
+    V val = m_map.get(key);
+    if (val == null) {
+      K ceilingKey = m_map.ceilingKey(key);
+      K floorKey = m_map.floorKey(key);
+
+      if (ceilingKey == null && floorKey == null) {
+        return null;
+      }
+      if (ceilingKey == null) {
+        return m_map.get(floorKey);
+      }
+      if (floorKey == null) {
+        return m_map.get(ceilingKey);
+      }
+      V floor = m_map.get(floorKey);
+      V ceiling = m_map.get(ceilingKey);
+
+      return m_interpolator.interpolate(
+          floor, ceiling, m_inverseInterpolator.inverseInterpolate(floorKey, ceilingKey, key));
+    } else {
+      return val;
+    }
+  }
+
+  /** Clears the contents. */
+  public void clear() {
+    m_map.clear();
+  }
+}

wpimath/src/main/java/org/wpilib/math/interpolation/Interpolator.java

@@ -0,0 +1,35 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import edu.wpi.first.math.MathUtil;
+
+/**
+ * An interpolation function that returns a value interpolated between an upper and lower bound.
+ * This behavior can be linear or nonlinear.
+ *
+ * @param <T> The type that the {@link Interpolator} will operate on.
+ */
+@FunctionalInterface
+public interface Interpolator<T> {
+  /**
+   * Perform interpolation between two values.
+   *
+   * @param startValue The value to start at.
+   * @param endValue The value to end at.
+   * @param t How far between the two values to interpolate. This should be bounded to [0, 1].
+   * @return The interpolated value.
+   */
+  T interpolate(T startValue, T endValue, double t);
+
+  /**
+   * Returns interpolator for Double.
+   *
+   * @return Interpolator for Double.
+   */
+  static Interpolator<Double> forDouble() {
+    return MathUtil::lerp;
+  }
+}

wpimath/src/main/java/org/wpilib/math/interpolation/InverseInterpolator.java

@@ -0,0 +1,35 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import edu.wpi.first.math.MathUtil;
+
+/**
+ * An inverse interpolation function which determines where within an interpolation range an object
+ * lies. This behavior can be linear or nonlinear.
+ *
+ * @param <T> The type that the {@link InverseInterpolator} will operate on.
+ */
+@FunctionalInterface
+public interface InverseInterpolator<T> {
+  /**
+   * Return where within interpolation range [0, 1] q is between startValue and endValue.
+   *
+   * @param startValue Lower part of interpolation range.
+   * @param endValue Upper part of interpolation range.
+   * @param q Query.
+   * @return Interpolant in range [0, 1].
+   */
+  double inverseInterpolate(T startValue, T endValue, T q);
+
+  /**
+   * Returns inverse interpolator for Double.
+   *
+   * @return Inverse interpolator for Double.
+   */
+  static InverseInterpolator<Double> forDouble() {
+    return MathUtil::inverseLerp;
+  }
+}

wpimath/src/main/java/org/wpilib/math/interpolation/TimeInterpolatableBuffer.java

@@ -0,0 +1,151 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import edu.wpi.first.math.MathUtil;
+import java.util.NavigableMap;
+import java.util.Optional;
+import java.util.TreeMap;
+
+/**
+ * The TimeInterpolatableBuffer provides an easy way to estimate past measurements. One application
+ * might be in conjunction with the DifferentialDrivePoseEstimator, where knowledge of the robot
+ * pose at the time when vision or other global measurement were recorded is necessary, or for
+ * recording the past angles of mechanisms as measured by encoders.
+ *
+ * @param <T> The type stored in this buffer.
+ */
+public final class TimeInterpolatableBuffer<T> {
+  private final double m_historySize;
+  private final Interpolator<T> m_interpolatingFunc;
+  private final NavigableMap<Double, T> m_pastSnapshots = new TreeMap<>();
+
+  /**
+   * Constructs a TimeInterpolatableBuffer.
+   *
+   * @param interpolateFunction Interpolation function.
+   * @param historySize The history size of the buffer in seconds.
+   */
+  private TimeInterpolatableBuffer(Interpolator<T> interpolateFunction, double historySize) {
+    this.m_historySize = historySize;
+    this.m_interpolatingFunc = interpolateFunction;
+  }
+
+  /**
+   * Create a new TimeInterpolatableBuffer.
+   *
+   * @param interpolateFunction The function used to interpolate between values.
+   * @param historySize The history size of the buffer in seconds.
+   * @param <T> The type of data to store in the buffer.
+   * @return The new TimeInterpolatableBuffer.
+   */
+  public static <T> TimeInterpolatableBuffer<T> createBuffer(
+      Interpolator<T> interpolateFunction, double historySize) {
+    return new TimeInterpolatableBuffer<>(interpolateFunction, historySize);
+  }
+
+  /**
+   * Create a new TimeInterpolatableBuffer that stores a given subclass of {@link Interpolatable}.
+   *
+   * @param historySize The history size of the buffer in seconds.
+   * @param <T> The type of {@link Interpolatable} to store in the buffer.
+   * @return The new TimeInterpolatableBuffer.
+   */
+  public static <T extends Interpolatable<T>> TimeInterpolatableBuffer<T> createBuffer(
+      double historySize) {
+    return new TimeInterpolatableBuffer<>(Interpolatable::interpolate, historySize);
+  }
+
+  /**
+   * Create a new TimeInterpolatableBuffer to store Double values.
+   *
+   * @param historySize The history size of the buffer in seconds.
+   * @return The new TimeInterpolatableBuffer.
+   */
+  public static TimeInterpolatableBuffer<Double> createDoubleBuffer(double historySize) {
+    return new TimeInterpolatableBuffer<>(MathUtil::lerp, historySize);
+  }
+
+  /**
+   * Add a sample to the buffer.
+   *
+   * @param time The timestamp of the sample in seconds.
+   * @param sample The sample object.
+   */
+  public void addSample(double time, T sample) {
+    cleanUp(time);
+    m_pastSnapshots.put(time, sample);
+  }
+
+  /**
+   * Removes samples older than our current history size.
+   *
+   * @param time The current timestamp in seconds.
+   */
+  private void cleanUp(double time) {
+    while (!m_pastSnapshots.isEmpty()) {
+      var entry = m_pastSnapshots.firstEntry();
+      if (time - entry.getKey() >= m_historySize) {
+        m_pastSnapshots.remove(entry.getKey());
+      } else {
+        return;
+      }
+    }
+  }
+
+  /** Clear all old samples. */
+  public void clear() {
+    m_pastSnapshots.clear();
+  }
+
+  /**
+   * Sample the buffer at the given time. If the buffer is empty, an empty Optional is returned.
+   *
+   * @param time The time at which to sample in seconds.
+   * @return The interpolated value at that timestamp or an empty Optional.
+   */
+  public Optional<T> getSample(double time) {
+    if (m_pastSnapshots.isEmpty()) {
+      return Optional.empty();
+    }
+
+    // Special case for when the requested time is the same as a sample
+    var nowEntry = m_pastSnapshots.get(time);
+    if (nowEntry != null) {
+      return Optional.of(nowEntry);
+    }
+
+    var topBound = m_pastSnapshots.ceilingEntry(time);
+    var bottomBound = m_pastSnapshots.floorEntry(time);
+
+    // Return null if neither sample exists, and the opposite bound if the other is null
+    if (topBound == null && bottomBound == null) {
+      return Optional.empty();
+    } else if (topBound == null) {
+      return Optional.of(bottomBound.getValue());
+    } else if (bottomBound == null) {
+      return Optional.of(topBound.getValue());
+    } else {
+      // Otherwise, interpolate. Because T is between [0, 1], we want the ratio of (the difference
+      // between the current time and bottom bound) and (the difference between top and bottom
+      // bounds).
+      return Optional.of(
+          m_interpolatingFunc.interpolate(
+              bottomBound.getValue(),
+              topBound.getValue(),
+              (time - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey())));
+    }
+  }
+
+  /**
+   * Grant access to the internal sample buffer. Used in Pose Estimation to replay odometry inputs
+   * stored within this buffer.
+   *
+   * @return The internal sample buffer.
+   */
+  public NavigableMap<Double, T> getInternalBuffer() {
+    return m_pastSnapshots;
+  }
+}

wpimath/src/main/java/org/wpilib/math/jni/ArmFeedforwardJNI.java

@@ -0,0 +1,36 @@
+// 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.jni;
+
+/** ArmFeedforward JNI. */
+public final class ArmFeedforwardJNI extends WPIMathJNI {
+  /**
+   * Obtain a feedforward voltage from a single jointed arm feedforward object.
+   *
+   * <p>Constructs an ArmFeedforward object and runs its currentVelocity and nextVelocity overload
+   *
+   * @param ks The ArmFeedforward's static gain in volts.
+   * @param kv The ArmFeedforward's velocity gain in volt seconds per radian.
+   * @param ka The ArmFeedforward's acceleration gain in volt seconds² per radian.
+   * @param kg The ArmFeedforward's gravity gain in volts.
+   * @param currentAngle The current angle in the calculation in radians.
+   * @param currentVelocity The current velocity in the calculation in radians per second.
+   * @param nextVelocity The next velocity in the calculation in radians per second.
+   * @param dt The time between velocity setpoints in seconds.
+   * @return The calculated feedforward in volts.
+   */
+  public static native double calculate(
+      double ks,
+      double kv,
+      double ka,
+      double kg,
+      double currentAngle,
+      double currentVelocity,
+      double nextVelocity,
+      double dt);
+
+  /** Utility class. */
+  private ArmFeedforwardJNI() {}
+}

wpimath/src/main/java/org/wpilib/math/jni/DAREJNI.java

@@ -0,0 +1,176 @@
+// 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.jni;
+
+/** DARE JNI. */
+public final class DAREJNI extends WPIMathJNI {
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+   *
+   * <p>This internal function skips expensive precondition checks for increased performance. The
+   * solver may hang if any of the following occur:
+   *
+   * <ul>
+   *   <li>Q isn't symmetric positive semidefinite
+   *   <li>R isn't symmetric positive definite
+   *   <li>The (A, B) pair isn't stabilizable
+   *   <li>The (A, C) pair where Q = CᵀC isn't detectable
+   * </ul>
+   *
+   * <p>Only use this function if you're sure the preconditions are met. Solves the discrete
+   * algebraic Riccati equation.
+   *
+   * @param A Array containing elements of A in row-major order.
+   * @param B Array containing elements of B in row-major order.
+   * @param Q Array containing elements of Q in row-major order.
+   * @param R Array containing elements of R in row-major order.
+   * @param states Number of states in A matrix.
+   * @param inputs Number of inputs in B matrix.
+   * @param S Array storage for DARE solution.
+   */
+  public static native void dareNoPrecondABQR(
+      double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+   *
+   * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
+   * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q  N][xₖ]
+   * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
+   * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
+   *
+   * <pre>
+   *     ∞
+   * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This can be refactored as:
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q 0][xₖ]
+   * J = Σ [uₖ] [0 R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This internal function skips expensive precondition checks for increased performance. The
+   * solver may hang if any of the following occur:
+   *
+   * <ul>
+   *   <li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
+   *   <li>R isn't symmetric positive definite
+   *   <li>The (A − BR⁻¹Nᵀ, B) pair isn't stabilizable
+   *   <li>The (A, C) pair where Q = CᵀC isn't detectable
+   * </ul>
+   *
+   * <p>Only use this function if you're sure the preconditions are met.
+   *
+   * @param A Array containing elements of A in row-major order.
+   * @param B Array containing elements of B in row-major order.
+   * @param Q Array containing elements of Q in row-major order.
+   * @param R Array containing elements of R in row-major order.
+   * @param N Array containing elements of N in row-major order.
+   * @param states Number of states in A matrix.
+   * @param inputs Number of inputs in B matrix.
+   * @param S Array storage for DARE solution.
+   */
+  public static native void dareNoPrecondABQRN(
+      double[] A,
+      double[] B,
+      double[] Q,
+      double[] R,
+      double[] N,
+      int states,
+      int inputs,
+      double[] S);
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+   *
+   * @param A Array containing elements of A in row-major order.
+   * @param B Array containing elements of B in row-major order.
+   * @param Q Array containing elements of Q in row-major order.
+   * @param R Array containing elements of R in row-major order.
+   * @param states Number of states in A matrix.
+   * @param inputs Number of inputs in B matrix.
+   * @param S Array storage for DARE solution.
+   * @throws IllegalArgumentException if Q isn't symmetric positive semidefinite.
+   * @throws IllegalArgumentException if R isn't symmetric positive definite.
+   * @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
+   * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
+   */
+  public static native void dareABQR(
+      double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+   *
+   * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
+   * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q  N][xₖ]
+   * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
+   * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
+   *
+   * <pre>
+   *     ∞
+   * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This can be refactored as:
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q 0][xₖ]
+   * J = Σ [uₖ] [0 R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * @param A Array containing elements of A in row-major order.
+   * @param B Array containing elements of B in row-major order.
+   * @param Q Array containing elements of Q in row-major order.
+   * @param R Array containing elements of R in row-major order.
+   * @param N Array containing elements of N in row-major order.
+   * @param states Number of states in A matrix.
+   * @param inputs Number of inputs in B matrix.
+   * @param S Array storage for DARE solution.
+   * @throws IllegalArgumentException if Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite.
+   * @throws IllegalArgumentException if R isn't symmetric positive definite.
+   * @throws IllegalArgumentException if the (A − BR⁻¹Nᵀ, B) pair isn't stabilizable.
+   * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
+   */
+  public static native void dareABQRN(
+      double[] A,
+      double[] B,
+      double[] Q,
+      double[] R,
+      double[] N,
+      int states,
+      int inputs,
+      double[] S);
+
+  /** Utility class. */
+  private DAREJNI() {}
+}

wpimath/src/main/java/org/wpilib/math/jni/EigenJNI.java

@@ -0,0 +1,57 @@
+// 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.jni;
+
+/** Eigen JNI. */
+public final class EigenJNI extends WPIMathJNI {
+  /**
+   * Computes the matrix exp.
+   *
+   * @param src Array of elements of the matrix to be exponentiated.
+   * @param rows How many rows there are.
+   * @param dst Array where the result will be stored.
+   */
+  public static native void exp(double[] src, int rows, double[] dst);
+
+  /**
+   * Computes the matrix pow.
+   *
+   * @param src Array of elements of the matrix to be raised to a power.
+   * @param rows How many rows there are.
+   * @param exponent The exponent.
+   * @param dst Array where the result will be stored.
+   */
+  public static native void pow(double[] src, int rows, double exponent, double[] dst);
+
+  /**
+   * Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition
+   * matrix.
+   *
+   * @param mat Array of elements of the matrix to be updated.
+   * @param lowerTriangular Whether mat is lower triangular.
+   * @param rows How many rows there are.
+   * @param vec Vector to use for the rank update.
+   * @param sigma Sigma value to use for the rank update.
+   */
+  public static native void rankUpdate(
+      double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular);
+
+  /**
+   * Solves the least-squares problem Ax=B using a QR decomposition with full pivoting.
+   *
+   * @param A Array of elements of the A matrix.
+   * @param Arows Number of rows of the A matrix.
+   * @param Acols Number of rows of the A matrix.
+   * @param B Array of elements of the B matrix.
+   * @param Brows Number of rows of the B matrix.
+   * @param Bcols Number of rows of the B matrix.
+   * @param dst Array to store solution in. If A is m-n and B is m-p, dst is n-p.
+   */
+  public static native void solveFullPivHouseholderQr(
+      double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst);
+
+  /** Utility class. */
+  private EigenJNI() {}
+}