SCRIPT Move java files

wpimath/src/test/java/org/wpilib/ProtoTestBase.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.wpilibj;
+
+import edu.wpi.first.util.protobuf.Protobuf;
+import org.junit.jupiter.api.Test;
+import us.hebi.quickbuf.ProtoMessage;
+
+public abstract class ProtoTestBase<T, MessageType extends ProtoMessage<MessageType>> {
+  private final T m_testData;
+  private final Protobuf<T, MessageType> m_proto;
+
+  protected ProtoTestBase(T testData, Protobuf<T, MessageType> proto) {
+    m_testData = testData;
+    m_proto = proto;
+  }
+
+  public abstract void checkEquals(T testData, T data);
+
+  @Test
+  void testRoundTrip() {
+    final MessageType msg = m_proto.createMessage();
+    m_proto.pack(msg, m_testData);
+
+    final T data = m_proto.unpack(msg);
+    checkEquals(m_testData, data);
+  }
+
+  @Test
+  void testDoublePack() {
+    final MessageType msg = m_proto.createMessage();
+    m_proto.pack(msg, m_testData);
+    m_proto.pack(msg, m_testData);
+
+    final T data = m_proto.unpack(msg);
+    checkEquals(m_testData, data);
+  }
+
+  @Test
+  void testDoubleUnpack() {
+    final MessageType msg = m_proto.createMessage();
+    m_proto.pack(msg, m_testData);
+
+    T data = m_proto.unpack(msg);
+    checkEquals(m_testData, data);
+
+    data = m_proto.unpack(msg);
+    checkEquals(m_testData, data);
+  }
+}

wpimath/src/test/java/org/wpilib/StructTestBase.java

@@ -0,0 +1,61 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+public abstract class StructTestBase<T> {
+  private final T m_testData;
+  private final Struct<T> m_struct;
+
+  protected StructTestBase(T testData, Struct<T> struct) {
+    m_testData = testData;
+    m_struct = struct;
+  }
+
+  public abstract void checkEquals(T testData, T data);
+
+  @Test
+  void testRoundTrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(m_struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    m_struct.pack(buffer, m_testData);
+    buffer.rewind();
+
+    final T data = m_struct.unpack(buffer);
+    checkEquals(m_testData, data);
+  }
+
+  @Test
+  void testDoublePack() {
+    ByteBuffer buffer = ByteBuffer.allocate(m_struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    m_struct.pack(buffer, m_testData);
+    buffer.rewind();
+    m_struct.pack(buffer, m_testData);
+    buffer.rewind();
+
+    final T data = m_struct.unpack(buffer);
+    checkEquals(m_testData, data);
+  }
+
+  @Test
+  void testDoubleUnpack() {
+    ByteBuffer buffer = ByteBuffer.allocate(m_struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    m_struct.pack(buffer, m_testData);
+    buffer.rewind();
+
+    T data = m_struct.unpack(buffer);
+    checkEquals(m_testData, data);
+    buffer.rewind();
+
+    data = m_struct.unpack(buffer);
+    checkEquals(m_testData, data);
+  }
+}

wpimath/src/test/java/org/wpilib/UtilityClassTest.java

@@ -0,0 +1,65 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.DynamicTest.dynamicTest;
+
+import java.lang.reflect.Constructor;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Modifier;
+import java.util.Arrays;
+import java.util.stream.Stream;
+import org.junit.jupiter.api.DynamicTest;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.TestFactory;
+
+// Declaring this class abstract prevents UtilityClassTest from running on itself and throwing the
+// following exception:
+//
+// org.junit.jupiter.api.extension.ParameterResolutionException: No ParameterResolver registered
+// for parameter [java.lang.Class<T> arg0] in constructor [protected
+// edu.wpi.first.wpilibj.UtilityClassTest(java.lang.Class<T>)].
+@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
+public abstract class UtilityClassTest<T> {
+  private final Class<T> m_clazz;
+
+  protected UtilityClassTest(Class<T> clazz) {
+    m_clazz = clazz;
+  }
+
+  @Test
+  void singleConstructorTest() {
+    assertEquals(1, m_clazz.getDeclaredConstructors().length, "More than one constructor defined");
+  }
+
+  @Test
+  void constructorPrivateTest() {
+    Constructor<?> constructor = m_clazz.getDeclaredConstructors()[0];
+
+    assertFalse(constructor.canAccess(null), "Constructor is not private");
+  }
+
+  @Test
+  @SuppressWarnings("PMD.AvoidAccessibilityAlteration")
+  void constructorReflectionTest() {
+    Constructor<?> constructor = m_clazz.getDeclaredConstructors()[0];
+    constructor.setAccessible(true);
+    assertThrows(InvocationTargetException.class, constructor::newInstance);
+  }
+
+  @TestFactory
+  Stream<DynamicTest> publicMethodsStaticTestFactory() {
+    return Arrays.stream(m_clazz.getDeclaredMethods())
+        .filter(method -> Modifier.isPublic(method.getModifiers()))
+        .map(
+            method ->
+                dynamicTest(
+                    method.getName(), () -> assertTrue(Modifier.isStatic(method.getModifiers()))));
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/ArmFeedforwardTest.java

@@ -0,0 +1,185 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.NumericalIntegration;
+import java.util.function.BiFunction;
+import org.junit.jupiter.api.Test;
+
+class ArmFeedforwardTest {
+  /**
+   * Simulates a single-jointed arm and returns the final state.
+   *
+   * @param ks The static gain, in volts.
+   * @param kv The velocity gain, in volt seconds per radian.
+   * @param ka The acceleration gain, in volt seconds² per radian.
+   * @param kg The gravity gain, in volts.
+   * @param currentAngle The starting angle in radians.
+   * @param currentVelocity The starting angular velocity in radians per second.
+   * @param input The input voltage.
+   * @param dt The simulation time in seconds.
+   * @return The final state as a 2-vector of angle and angular velocity.
+   */
+  private Matrix<N2, N1> simulate(
+      double ks,
+      double kv,
+      double ka,
+      double kg,
+      double currentAngle,
+      double currentVelocity,
+      double input,
+      double dt) {
+    final Matrix<N2, N2> A =
+        new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.0, 1.0, 0.0, -kv / ka});
+    final Matrix<N2, N1> B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0.0, 1.0 / ka});
+
+    final BiFunction<Matrix<N2, N1>, Matrix<N1, N1>, Matrix<N2, N1>> f =
+        (x, u) -> {
+          Matrix<N2, N1> c =
+              MatBuilder.fill(
+                  Nat.N2(),
+                  Nat.N1(),
+                  0.0,
+                  Math.signum(x.get(1, 0)) * (-ks / ka) - (kg / ka) * Math.cos(x.get(0, 0)));
+          return A.times(x).plus(B.times(u)).plus(c);
+        };
+
+    return NumericalIntegration.rk4(
+        f,
+        MatBuilder.fill(Nat.N2(), Nat.N1(), currentAngle, currentVelocity),
+        MatBuilder.fill(Nat.N1(), Nat.N1(), input),
+        dt);
+  }
+
+  /**
+   * Calculates a feedforward voltage using overload taking angle, two angular velocities, and dt;
+   * then simulates an arm using that voltage to verify correctness.
+   *
+   * @param armFF The feedforward object.
+   * @param ks The static gain, in volts.
+   * @param kv The velocity gain, in volt seconds per radian.
+   * @param ka The acceleration gain, in volt seconds² per radian.
+   * @param kg The gravity gain, in volts.
+   * @param currentAngle The starting angle in radians.
+   * @param currentVelocity The starting angular velocity in radians per second.
+   * @param input The input voltage.
+   * @param dt The simulation time in seconds.
+   */
+  private void calculateAndSimulate(
+      ArmFeedforward armFF,
+      double ks,
+      double kv,
+      double ka,
+      double kg,
+      double currentAngle,
+      double currentVelocity,
+      double nextVelocity,
+      double dt) {
+    final double input = armFF.calculate(currentAngle, currentVelocity, nextVelocity);
+    assertEquals(
+        nextVelocity,
+        simulate(ks, kv, ka, kg, currentAngle, currentVelocity, input, dt).get(1, 0),
+        1e-4);
+  }
+
+  @Test
+  void testCalculate() {
+    final double ks = 0.5;
+    final double kv = 1.5;
+    final double ka = 2;
+    final double kg = 1;
+    final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
+
+    // calculate(angle, angular velocity)
+    assertEquals(0.5, armFF.calculate(Math.PI / 3, 0.0), 0.002);
+    assertEquals(2.5, armFF.calculate(Math.PI / 3, 1.0), 0.002);
+
+    // calculate(currentAngle, currentVelocity, nextAngle, dt)
+    calculateAndSimulate(armFF, ks, kv, ka, kg, Math.PI / 3, 1.0, 1.05, 0.020);
+    calculateAndSimulate(armFF, ks, kv, ka, kg, Math.PI / 3, 1.0, 0.95, 0.020);
+    calculateAndSimulate(armFF, ks, kv, ka, kg, -Math.PI / 3, 1.0, 1.05, 0.020);
+    calculateAndSimulate(armFF, ks, kv, ka, kg, -Math.PI / 3, 1.0, 0.95, 0.020);
+  }
+
+  @Test
+  void testCalculateIllConditionedModel() {
+    final double ks = 0.39671;
+    final double kv = 2.7167;
+    final double ka = 1e-2;
+    final double kg = 0.2708;
+    final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
+
+    final double currentAngle = 1.0;
+    final double currentVelocity = 0.02;
+    final double nextVelocity = 0.0;
+
+    var averageAccel = (nextVelocity - currentVelocity) / 0.02;
+
+    assertEquals(
+        armFF.calculate(currentAngle, currentVelocity, nextVelocity),
+        ks + kv * currentVelocity + ka * averageAccel + kg * Math.cos(currentAngle));
+  }
+
+  @Test
+  void testCalculateIllConditionedGradient() {
+    final double ks = 0.39671;
+    final double kv = 2.7167;
+    final double ka = 0.50799;
+    final double kg = 0.2708;
+    final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
+
+    calculateAndSimulate(armFF, ks, kv, ka, kg, 1.0, 0.02, 0.0, 0.02);
+  }
+
+  @Test
+  void testAchievableVelocity() {
+    final double ks = 0.5;
+    final double kv = 1.5;
+    final double ka = 2;
+    final double kg = 1;
+    final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
+
+    assertEquals(6, armFF.maxAchievableVelocity(12, Math.PI / 3, 1), 0.002);
+    assertEquals(-9, armFF.minAchievableVelocity(11.5, Math.PI / 3, 1), 0.002);
+  }
+
+  @Test
+  void testAchievableAcceleration() {
+    final double ks = 0.5;
+    final double kv = 1.5;
+    final double ka = 2;
+    final double kg = 1;
+    final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
+
+    assertEquals(4.75, armFF.maxAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
+    assertEquals(6.75, armFF.maxAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
+    assertEquals(-7.25, armFF.minAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
+    assertEquals(-5.25, armFF.minAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
+  }
+
+  @Test
+  void testNegativeGains() {
+    final double ks = 0.5;
+    final double kv = 1.5;
+    final double ka = 2;
+    final double kg = 1;
+
+    assertAll(
+        () ->
+            assertThrows(IllegalArgumentException.class, () -> new ArmFeedforward(ks, kg, -kv, ka)),
+        () ->
+            assertThrows(
+                IllegalArgumentException.class, () -> new ArmFeedforward(ks, kg, kv, -ka)));
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/BangBangInputOutputTest.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.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class BangBangInputOutputTest {
+  @Test
+  void shouldOutput() {
+    var controller = new BangBangController();
+
+    assertEquals(controller.calculate(0, 1), 1);
+  }
+
+  @Test
+  void shouldNotOutput() {
+    var controller = new BangBangController();
+
+    assertEquals(controller.calculate(1, 0), 0);
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/BangBangToleranceTest.java

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+
+class BangBangToleranceTest {
+  @Test
+  void inTolerance() {
+    var controller = new BangBangController(0.1);
+
+    controller.setSetpoint(1);
+    controller.calculate(1);
+    assertTrue(controller.atSetpoint());
+  }
+
+  @Test
+  void outOfTolerance() {
+    var controller = new BangBangController(0.1);
+
+    controller.setSetpoint(1);
+    controller.calculate(0);
+    assertFalse(controller.atSetpoint());
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/ControlAffinePlantInversionFeedforwardTest.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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import org.junit.jupiter.api.Test;
+
+class ControlAffinePlantInversionFeedforwardTest {
+  @Test
+  void testCalculate() {
+    ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
+        new ControlAffinePlantInversionFeedforward<>(Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
+
+    assertEquals(
+        48.0, feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0), 1e-6);
+  }
+
+  @Test
+  void testCalculateState() {
+    ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
+        new ControlAffinePlantInversionFeedforward<>(Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
+
+    assertEquals(
+        48.0, feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0), 1e-6);
+  }
+
+  protected Matrix<N2, N1> getDynamics(Matrix<N2, N1> x, Matrix<N1, N1> u) {
+    return MatBuilder.fill(Nat.N2(), Nat.N2(), 1.000, 0, 0, 1.000)
+        .times(x)
+        .plus(VecBuilder.fill(0, 1).times(u));
+  }
+
+  protected Matrix<N2, N1> getStateDynamics(Matrix<N2, N1> x) {
+    return MatBuilder.fill(Nat.N2(), Nat.N2(), 1.000, 0, 0, 1.000).times(x);
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/DifferentialDriveAccelerationLimiterTest.java

@@ -0,0 +1,246 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveAccelerationLimiterTest {
+  @Test
+  void testLowLimits() {
+    final double trackwidth = 0.9;
+    final double dt = 0.005;
+    final double maxA = 2.0;
+    final double maxAlpha = 2.0;
+
+    var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+
+    var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, maxA, maxAlpha);
+
+    Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
+    Matrix<N2, N1> xAccelLimiter = VecBuilder.fill(0.0, 0.0);
+
+    // Ensure voltage exceeds acceleration before limiting
+    {
+      final var accels =
+          plant.getA().times(xAccelLimiter).plus(plant.getB().times(VecBuilder.fill(12.0, 12.0)));
+      final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+      assertTrue(Math.abs(a) > maxA);
+    }
+    {
+      final var accels =
+          plant.getA().times(xAccelLimiter).plus(plant.getB().times(VecBuilder.fill(-12.0, 12.0)));
+      final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
+      assertTrue(Math.abs(alpha) > maxAlpha);
+    }
+
+    // Forward
+    var u = VecBuilder.fill(12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      final var voltages =
+          accelLimiter.calculate(
+              xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+      xAccelLimiter =
+          plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
+
+      final var accels =
+          plant
+              .getA()
+              .times(xAccelLimiter)
+              .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right)));
+      final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+      final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
+      assertTrue(Math.abs(a) <= maxA);
+      assertTrue(Math.abs(alpha) <= maxAlpha);
+    }
+
+    // Backward
+    u = VecBuilder.fill(-12.0, -12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      final var voltages =
+          accelLimiter.calculate(
+              xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+      xAccelLimiter =
+          plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
+
+      final var accels =
+          plant
+              .getA()
+              .times(xAccelLimiter)
+              .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right)));
+      final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+      final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
+      assertTrue(Math.abs(a) <= maxA);
+      assertTrue(Math.abs(alpha) <= maxAlpha);
+    }
+
+    // Rotate CCW
+    u = VecBuilder.fill(-12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      final var voltages =
+          accelLimiter.calculate(
+              xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+      xAccelLimiter =
+          plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
+
+      final var accels =
+          plant
+              .getA()
+              .times(xAccelLimiter)
+              .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right)));
+      final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+      final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
+      assertTrue(Math.abs(a) <= maxA);
+      assertTrue(Math.abs(alpha) <= maxAlpha);
+    }
+  }
+
+  @Test
+  void testHighLimits() {
+    final double trackwidth = 0.9;
+    final double dt = 0.005;
+
+    var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+
+    // Limits are so high, they don't get hit, so states of constrained and
+    // unconstrained systems should match
+    var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, 1e3, 1e3);
+
+    Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
+    Matrix<N2, N1> xAccelLimiter = VecBuilder.fill(0.0, 0.0);
+
+    // Forward
+    var u = VecBuilder.fill(12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      final var voltages =
+          accelLimiter.calculate(
+              xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+      xAccelLimiter =
+          plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
+
+      assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5);
+      assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5);
+    }
+
+    // Backward
+    x.fill(0.0);
+    xAccelLimiter.fill(0.0);
+    u = VecBuilder.fill(-12.0, -12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      final var voltages =
+          accelLimiter.calculate(
+              xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+      xAccelLimiter =
+          plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
+
+      assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5);
+      assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5);
+    }
+
+    // Rotate CCW
+    x.fill(0.0);
+    xAccelLimiter.fill(0.0);
+    u = VecBuilder.fill(-12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      final var voltages =
+          accelLimiter.calculate(
+              xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+      xAccelLimiter =
+          plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
+
+      assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5);
+      assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5);
+    }
+  }
+
+  @Test
+  void testSeparateMinMaxLowLimits() {
+    final double trackwidth = 0.9;
+    final double dt = 0.005;
+    final double minA = -1.0;
+    final double maxA = 2.0;
+    final double maxAlpha = 2.0;
+
+    var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+
+    var accelLimiter =
+        new DifferentialDriveAccelerationLimiter(plant, trackwidth, minA, maxA, maxAlpha);
+
+    Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
+    Matrix<N2, N1> xAccelLimiter = VecBuilder.fill(0.0, 0.0);
+
+    // Ensure voltage exceeds acceleration before limiting
+    {
+      final var accels =
+          plant.getA().times(xAccelLimiter).plus(plant.getB().times(VecBuilder.fill(12.0, 12.0)));
+      final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+      assertTrue(Math.abs(a) > maxA);
+      assertTrue(Math.abs(a) > -minA);
+    }
+
+    // a should always be within [minA, maxA]
+    // Forward
+    var u = VecBuilder.fill(12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      final var voltages =
+          accelLimiter.calculate(
+              xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+      xAccelLimiter =
+          plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
+
+      final var accels =
+          plant
+              .getA()
+              .times(xAccelLimiter)
+              .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right)));
+      final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+      assertTrue(minA <= a && a <= maxA);
+    }
+
+    // Backward
+    u = VecBuilder.fill(-12.0, -12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      final var voltages =
+          accelLimiter.calculate(
+              xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+      xAccelLimiter =
+          plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
+
+      final var accels =
+          plant
+              .getA()
+              .times(xAccelLimiter)
+              .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right)));
+      final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+      assertTrue(minA <= a && a <= maxA);
+    }
+  }
+
+  @Test
+  void testMinAccelGreaterThanMaxAccel() {
+    var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+    assertDoesNotThrow(() -> new DifferentialDriveAccelerationLimiter(plant, 1, -1, 1, 1e3));
+    assertThrows(
+        IllegalArgumentException.class,
+        () -> new DifferentialDriveAccelerationLimiter(plant, 1, 1, -1, 1e3));
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/DifferentialDriveFeedforwardTest.java

@@ -0,0 +1,85 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveFeedforwardTest {
+  private static final double kVLinear = 1.0;
+  private static final double kALinear = 1.0;
+  private static final double kVAngular = 1.0;
+  private static final double kAAngular = 1.0;
+  private static final double trackwidth = 1.0;
+  private static final double dt = 0.02;
+
+  @Test
+  void testCalculateWithTrackwidth() {
+    DifferentialDriveFeedforward differentialDriveFeedforward =
+        new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
+    LinearSystem<N2, N2, N2> plant =
+        LinearSystemId.identifyDrivetrainSystem(
+            kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
+    for (int currentLeftVelocity = -4; currentLeftVelocity <= 4; currentLeftVelocity += 2) {
+      for (int currentRightVelocity = -4; currentRightVelocity <= 4; currentRightVelocity += 2) {
+        for (int nextLeftVelocity = -4; nextLeftVelocity <= 4; nextLeftVelocity += 2) {
+          for (int nextRightVelocity = -4; nextRightVelocity <= 4; nextRightVelocity += 2) {
+            DifferentialDriveWheelVoltages u =
+                differentialDriveFeedforward.calculate(
+                    currentLeftVelocity,
+                    nextLeftVelocity,
+                    currentRightVelocity,
+                    nextRightVelocity,
+                    dt);
+            Matrix<N2, N1> nextX =
+                plant.calculateX(
+                    VecBuilder.fill(currentLeftVelocity, currentRightVelocity),
+                    VecBuilder.fill(u.left, u.right),
+                    dt);
+            assertEquals(nextX.get(0, 0), nextLeftVelocity, 1e-6);
+            assertEquals(nextX.get(1, 0), nextRightVelocity, 1e-6);
+          }
+        }
+      }
+    }
+  }
+
+  @Test
+  void testCalculateWithoutTrackwidth() {
+    DifferentialDriveFeedforward differentialDriveFeedforward =
+        new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular);
+    LinearSystem<N2, N2, N2> plant =
+        LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular);
+    for (int currentLeftVelocity = -4; currentLeftVelocity <= 4; currentLeftVelocity += 2) {
+      for (int currentRightVelocity = -4; currentRightVelocity <= 4; currentRightVelocity += 2) {
+        for (int nextLeftVelocity = -4; nextLeftVelocity <= 4; nextLeftVelocity += 2) {
+          for (int nextRightVelocity = -4; nextRightVelocity <= 4; nextRightVelocity += 2) {
+            DifferentialDriveWheelVoltages u =
+                differentialDriveFeedforward.calculate(
+                    currentLeftVelocity,
+                    nextLeftVelocity,
+                    currentRightVelocity,
+                    nextRightVelocity,
+                    dt);
+            Matrix<N2, N1> nextX =
+                plant.calculateX(
+                    VecBuilder.fill(currentLeftVelocity, currentRightVelocity),
+                    VecBuilder.fill(u.left, u.right),
+                    dt);
+            assertEquals(nextX.get(0, 0), nextLeftVelocity, 1e-6);
+            assertEquals(nextX.get(1, 0), nextRightVelocity, 1e-6);
+          }
+        }
+      }
+    }
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/ElevatorFeedforwardTest.java

@@ -0,0 +1,67 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import org.junit.jupiter.api.Test;
+
+class ElevatorFeedforwardTest {
+  private static final double ks = 0.5;
+  private static final double kg = 1;
+  private static final double kv = 1.5;
+  private static final double ka = 2;
+
+  private final ElevatorFeedforward m_elevatorFF = new ElevatorFeedforward(ks, kg, kv, ka);
+
+  @Test
+  void testCalculate() {
+    assertEquals(1, m_elevatorFF.calculate(0.0), 0.002);
+    assertEquals(4.5, m_elevatorFF.calculate(2.0), 0.002);
+
+    var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -kv / ka);
+    var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / ka);
+    final double dt = 0.02;
+    var plantInversion = new LinearPlantInversionFeedforward<N1, N1, N1>(A, B, dt);
+
+    var r = VecBuilder.fill(2.0);
+    var nextR = VecBuilder.fill(3.0);
+    assertEquals(
+        plantInversion.calculate(r, nextR).get(0, 0) + ks + kg,
+        m_elevatorFF.calculate(2.0, 3.0),
+        0.002);
+  }
+
+  @Test
+  void testAchievableVelocity() {
+    assertEquals(5, m_elevatorFF.maxAchievableVelocity(11, 1), 0.002);
+    assertEquals(-9, m_elevatorFF.minAchievableVelocity(11, 1), 0.002);
+  }
+
+  @Test
+  void testAchievableAcceleration() {
+    assertEquals(3.75, m_elevatorFF.maxAchievableAcceleration(12, 2), 0.002);
+    assertEquals(7.25, m_elevatorFF.maxAchievableAcceleration(12, -2), 0.002);
+    assertEquals(-8.25, m_elevatorFF.minAchievableAcceleration(12, 2), 0.002);
+    assertEquals(-4.75, m_elevatorFF.minAchievableAcceleration(12, -2), 0.002);
+  }
+
+  @Test
+  void testNegativeGains() {
+    assertAll(
+        () ->
+            assertThrows(
+                IllegalArgumentException.class, () -> new ElevatorFeedforward(ks, kg, -kv, ka)),
+        () ->
+            assertThrows(
+                IllegalArgumentException.class, () -> new ElevatorFeedforward(ks, kg, kv, -ka)));
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/ImplicitModelFollowerTest.java

@@ -0,0 +1,108 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import org.junit.jupiter.api.Test;
+
+class ImplicitModelFollowerTest {
+  @Test
+  void testSameModel() {
+    final double dt = 0.005;
+
+    var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+
+    var imf = new ImplicitModelFollower<>(plant, plant);
+
+    Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
+    Matrix<N2, N1> xImf = VecBuilder.fill(0.0, 0.0);
+
+    // Forward
+    var u = VecBuilder.fill(12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertEquals(x.get(0, 0), xImf.get(0, 0));
+      assertEquals(x.get(1, 0), xImf.get(1, 0));
+    }
+
+    // Backward
+    u = VecBuilder.fill(-12.0, -12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertEquals(x.get(0, 0), xImf.get(0, 0));
+      assertEquals(x.get(1, 0), xImf.get(1, 0));
+    }
+
+    // Rotate CCW
+    u = VecBuilder.fill(-12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertEquals(x.get(0, 0), xImf.get(0, 0));
+      assertEquals(x.get(1, 0), xImf.get(1, 0));
+    }
+  }
+
+  @Test
+  void testSlowerRefModel() {
+    final double dt = 0.005;
+
+    var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+
+    // Linear acceleration is slower, but angular acceleration is the same
+    var plantRef = LinearSystemId.identifyDrivetrainSystem(1.0, 2.0, 1.0, 1.0);
+
+    var imf = new ImplicitModelFollower<>(plant, plantRef);
+
+    Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
+    Matrix<N2, N1> xImf = VecBuilder.fill(0.0, 0.0);
+
+    // Forward
+    var u = VecBuilder.fill(12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertTrue(x.get(0, 0) >= xImf.get(0, 0));
+      assertTrue(x.get(1, 0) >= xImf.get(1, 0));
+    }
+
+    // Backward
+    x.fill(0.0);
+    xImf.fill(0.0);
+    u = VecBuilder.fill(-12.0, -12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertTrue(x.get(0, 0) <= xImf.get(0, 0));
+      assertTrue(x.get(1, 0) <= xImf.get(1, 0));
+    }
+
+    // Rotate CCW
+    x.fill(0.0);
+    xImf.fill(0.0);
+    u = VecBuilder.fill(-12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertEquals(x.get(0, 0), xImf.get(0, 0), 1e-5);
+      assertEquals(x.get(1, 0), xImf.get(1, 0), 1e-5);
+    }
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/LTVDifferentialDriveControllerTest.java

@@ -0,0 +1,133 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.ArrayList;
+import org.junit.jupiter.api.Test;
+
+class LTVDifferentialDriveControllerTest {
+  private static final double kTolerance = 1 / 12.0;
+  private static final double kAngularTolerance = Math.toRadians(2);
+
+  /** States of the drivetrain system. */
+  static class State {
+    /// X position in global coordinate frame.
+    public static final int kX = 0;
+
+    /// Y position in global coordinate frame.
+    public static final int kY = 1;
+
+    /// Heading in global coordinate frame.
+    public static final int kHeading = 2;
+
+    /// Left encoder velocity.
+    public static final int kLeftVelocity = 3;
+
+    /// Right encoder velocity.
+    public static final int kRightVelocity = 4;
+  }
+
+  private static final double kLinearV = 3.02; // V/(m/s)
+  private static final double kLinearA = 0.642; // V/(m/s²)
+  private static final double kAngularV = 1.382; // V/(m/s)
+  private static final double kAngularA = 0.08495; // V/(m/s²)
+  private static final LinearSystem<N2, N2, N2> plant =
+      LinearSystemId.identifyDrivetrainSystem(kLinearV, kLinearA, kAngularV, kAngularA);
+  private static final double kTrackwidth = 0.9;
+
+  private static Matrix<N5, N1> dynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+    double v = (x.get(State.kLeftVelocity, 0) + x.get(State.kRightVelocity, 0)) / 2.0;
+
+    var xdot = new Matrix<>(Nat.N5(), Nat.N1());
+    xdot.set(0, 0, v * Math.cos(x.get(State.kHeading, 0)));
+    xdot.set(1, 0, v * Math.sin(x.get(State.kHeading, 0)));
+    xdot.set(2, 0, (x.get(State.kRightVelocity, 0) - x.get(State.kLeftVelocity, 0)) / kTrackwidth);
+    xdot.assignBlock(
+        3, 0, plant.getA().times(x.block(Nat.N2(), Nat.N1(), 3, 0)).plus(plant.getB().times(u)));
+    return xdot;
+  }
+
+  @Test
+  void testReachesReference() {
+    final double kDt = 0.02;
+
+    final var controller =
+        new LTVDifferentialDriveController(
+            plant,
+            kTrackwidth,
+            VecBuilder.fill(0.0625, 0.125, 2.5, 0.95, 0.95),
+            VecBuilder.fill(12.0, 12.0),
+            kDt);
+    var robotPose = new Pose2d(2.7, 23.0, Rotation2d.kZero);
+
+    final var waypoints = new ArrayList<Pose2d>();
+    waypoints.add(new Pose2d(2.75, 22.521, Rotation2d.kZero));
+    waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
+    var config = new TrajectoryConfig(8.8, 0.1);
+    final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+    var x =
+        MatBuilder.fill(
+            Nat.N5(),
+            Nat.N1(),
+            robotPose.getX(),
+            robotPose.getY(),
+            robotPose.getRotation().getRadians(),
+            0.0,
+            0.0);
+
+    final var totalTime = trajectory.getTotalTime();
+    for (int i = 0; i < (totalTime / kDt); ++i) {
+      var state = trajectory.sample(kDt * i);
+      robotPose =
+          new Pose2d(
+              x.get(State.kX, 0), x.get(State.kY, 0), new Rotation2d(x.get(State.kHeading, 0)));
+      final var output =
+          controller.calculate(
+              robotPose, x.get(State.kLeftVelocity, 0), x.get(State.kRightVelocity, 0), state);
+
+      x =
+          NumericalIntegration.rkdp(
+              LTVDifferentialDriveControllerTest::dynamics,
+              x,
+              VecBuilder.fill(output.left, output.right),
+              kDt);
+    }
+
+    final var states = trajectory.getStates();
+    final var endPose = states.get(states.size() - 1).pose;
+
+    // Java lambdas require local variables referenced from a lambda expression
+    // must be final or effectively final.
+    final var finalRobotPose = robotPose;
+    assertAll(
+        () -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
+        () -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
+        () ->
+            assertEquals(
+                0.0,
+                MathUtil.angleModulus(
+                    endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()),
+                kAngularTolerance));
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/LTVUnicycleControllerTest.java

@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.ArrayList;
+import org.junit.jupiter.api.Test;
+
+class LTVUnicycleControllerTest {
+  private static final double kTolerance = 1 / 12.0;
+  private static final double kAngularTolerance = Math.toRadians(2);
+
+  @Test
+  void testReachesReference() {
+    final double kDt = 0.02;
+
+    final var controller =
+        new LTVUnicycleController(
+            VecBuilder.fill(0.0625, 0.125, 2.5), VecBuilder.fill(4.0, 4.0), kDt);
+    var robotPose = new Pose2d(2.7, 23.0, Rotation2d.kZero);
+
+    final var waypoints = new ArrayList<Pose2d>();
+    waypoints.add(new Pose2d(2.75, 22.521, Rotation2d.kZero));
+    waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
+    var config = new TrajectoryConfig(8.8, 0.1);
+    final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+    final var totalTime = trajectory.getTotalTime();
+    for (int i = 0; i < (totalTime / kDt); ++i) {
+      var state = trajectory.sample(kDt * i);
+
+      var output = controller.calculate(robotPose, state);
+      robotPose = robotPose.plus(new Twist2d(output.vx * kDt, 0, output.omega * kDt).exp());
+    }
+
+    final var states = trajectory.getStates();
+    final var endPose = states.get(states.size() - 1).pose;
+
+    // Java lambdas require local variables referenced from a lambda expression
+    // must be final or effectively final.
+    final var finalRobotPose = robotPose;
+    assertAll(
+        () -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
+        () -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
+        () ->
+            assertEquals(
+                0.0,
+                MathUtil.angleModulus(
+                    endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()),
+                kAngularTolerance));
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/LinearPlantInversionFeedforwardTest.java

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.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 org.junit.jupiter.api.Test;
+
+class LinearPlantInversionFeedforwardTest {
+  @Test
+  void testCalculate() {
+    Matrix<N2, N2> A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1);
+    Matrix<N2, N1> B = VecBuilder.fill(0, 1);
+
+    LinearPlantInversionFeedforward<N2, N1, N1> feedforward =
+        new LinearPlantInversionFeedforward<>(A, B, 0.02);
+
+    assertEquals(
+        47.502599,
+        feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0),
+        0.002);
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/LinearQuadraticRegulatorTest.java

@@ -0,0 +1,174 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.system.Discretization;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import org.junit.jupiter.api.Test;
+
+class LinearQuadraticRegulatorTest {
+  @Test
+  void testLQROnElevator() {
+    var motors = DCMotor.getVex775Pro(2);
+
+    var m = 5.0;
+    var r = 0.0181864;
+    var G = 1.0;
+
+    var plant = LinearSystemId.createElevatorSystem(motors, m, r, G);
+
+    var qElms = VecBuilder.fill(0.02, 0.4);
+    var rElms = VecBuilder.fill(12.0);
+    var dt = 0.005;
+
+    var K = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt).getK();
+
+    assertEquals(522.87006795347486, K.get(0, 0), 1e-6);
+    assertEquals(38.239878385020411, K.get(0, 1), 1e-6);
+  }
+
+  @Test
+  void testFourMotorElevator() {
+    var dt = 0.020;
+
+    var plant =
+        LinearSystemId.createElevatorSystem(
+            DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67);
+
+    var K =
+        new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt)
+            .getK();
+
+    assertEquals(10.381, K.get(0, 0), 1e-2);
+    assertEquals(0.6929, K.get(0, 1), 1e-2);
+  }
+
+  @Test
+  void testLQROnArm() {
+    var motors = DCMotor.getVex775Pro(2);
+
+    var m = 4.0;
+    var r = 0.4;
+    var G = 100.0;
+
+    var plant = LinearSystemId.createSingleJointedArmSystem(motors, 1d / 3d * m * r * r, G);
+
+    var qElms = VecBuilder.fill(0.01745, 0.08726);
+    var rElms = VecBuilder.fill(12.0);
+    var dt = 0.005;
+
+    var K = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt).getK();
+
+    assertEquals(19.339349883583761, K.get(0, 0), 1e-6);
+    assertEquals(3.3542559517421582, K.get(0, 1), 1e-6);
+  }
+
+  /**
+   * Returns feedback control gain for implicit model following.
+   *
+   * <p>This is used to test the QRN overload of LQR.
+   *
+   * @param <States> Number of states.
+   * @param <Inputs> Number of inputs.
+   * @param A State matrix.
+   * @param B Input matrix.
+   * @param Q State cost matrix.
+   * @param R Input cost matrix.
+   * @param Aref Desired state matrix.
+   * @param dt Discretization timestep in seconds.
+   */
+  <States extends Num, Inputs extends Num> Matrix<Inputs, States> getImplicitModelFollowingK(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R,
+      Matrix<States, States> Aref,
+      double dt) {
+    // Discretize real dynamics
+    var discABPair = Discretization.discretizeAB(A, B, dt);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    // Discretize desired dynamics
+    var discAref = Discretization.discretizeA(Aref, dt);
+
+    Matrix<States, States> Qimf =
+        (discA.minus(discAref)).transpose().times(Q).times(discA.minus(discAref));
+    Matrix<Inputs, Inputs> Rimf = discB.transpose().times(Q).times(discB).plus(R);
+    Matrix<States, Inputs> Nimf = (discA.minus(discAref)).transpose().times(Q).times(discB);
+
+    return new LinearQuadraticRegulator<>(A, B, Qimf, Rimf, Nimf, dt).getK();
+  }
+
+  @Test
+  void testMatrixOverloadsWithSingleIntegrator() {
+    var A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 0, 0, 0);
+    var B = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1);
+    var Q = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1);
+    var R = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1);
+
+    // QR overload
+    var K = new LinearQuadraticRegulator<>(A, B, Q, R, 0.005).getK();
+    assertEquals(0.9975031249951226, K.get(0, 0), 1e-10);
+    assertEquals(0.0, K.get(0, 1), 1e-10);
+    assertEquals(0.0, K.get(1, 0), 1e-10);
+    assertEquals(0.9975031249951226, K.get(1, 1), 1e-10);
+
+    // QRN overload
+    var N = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1);
+    var Kimf = new LinearQuadraticRegulator<>(A, B, Q, R, N, 0.005).getK();
+    assertEquals(1.0, Kimf.get(0, 0), 1e-10);
+    assertEquals(0.0, Kimf.get(0, 1), 1e-10);
+    assertEquals(0.0, Kimf.get(1, 0), 1e-10);
+    assertEquals(1.0, Kimf.get(1, 1), 1e-10);
+  }
+
+  @Test
+  void testMatrixOverloadsWithDoubleIntegrator() {
+    double Kv = 3.02;
+    double Ka = 0.642;
+
+    var A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -Kv / Ka);
+    var B = MatBuilder.fill(Nat.N2(), Nat.N1(), 0, 1.0 / Ka);
+    var Q = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.2);
+    var R = MatBuilder.fill(Nat.N1(), Nat.N1(), 0.25);
+
+    // QR overload
+    var K = new LinearQuadraticRegulator<>(A, B, Q, R, 0.005).getK();
+    assertEquals(1.9960017786537287, K.get(0, 0), 1e-10);
+    assertEquals(0.5118212835109273, K.get(0, 1), 1e-10);
+
+    // QRN overload
+    var Aref = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -Kv / (Ka * 5.0));
+    var Kimf = getImplicitModelFollowingK(A, B, Q, R, Aref, 0.005);
+    assertEquals(0.0, Kimf.get(0, 0), 1e-10);
+    assertEquals(-6.919050011675146e-05, Kimf.get(0, 1), 1e-10);
+  }
+
+  @Test
+  void testLatencyCompensate() {
+    var dt = 0.02;
+
+    var plant =
+        LinearSystemId.createElevatorSystem(
+            DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67);
+
+    var regulator =
+        new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt);
+
+    regulator.latencyCompensate(plant, dt, 0.01);
+
+    assertEquals(8.97115941, regulator.getK().get(0, 0), 1e-3);
+    assertEquals(0.07904881, regulator.getK().get(0, 1), 1e-3);
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/LinearSystemLoopTest.java

@@ -0,0 +1,141 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.estimator.KalmanFilter;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.LinearSystemLoop;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import java.util.Random;
+import org.junit.jupiter.api.Test;
+
+class LinearSystemLoopTest {
+  private static final double kDt = 0.005;
+  private static final double kPositionStddev = 0.0001;
+  private static final Random random = new Random();
+
+  LinearSystem<N2, N1, N2> m_plant =
+      LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5, 0.0181864, 1.0);
+
+  @SuppressWarnings("unchecked")
+  KalmanFilter<N2, N1, N1> m_observer =
+      new KalmanFilter<>(
+          Nat.N2(),
+          Nat.N1(),
+          (LinearSystem<N2, N1, N1>) m_plant.slice(0),
+          VecBuilder.fill(0.05, 1.0),
+          VecBuilder.fill(0.0001),
+          kDt);
+
+  @SuppressWarnings("unchecked")
+  LinearQuadraticRegulator<N2, N1, N1> m_controller =
+      new LinearQuadraticRegulator<>(
+          (LinearSystem<N2, N1, N1>) m_plant.slice(0),
+          VecBuilder.fill(0.02, 0.4),
+          VecBuilder.fill(12.0),
+          0.005);
+
+  @SuppressWarnings("unchecked")
+  private final LinearSystemLoop<N2, N1, N1> m_loop =
+      new LinearSystemLoop<>(
+          (LinearSystem<N2, N1, N1>) m_plant.slice(0), m_controller, m_observer, 12, 0.005);
+
+  private static void updateTwoState(
+      LinearSystem<N2, N1, N1> plant, LinearSystemLoop<N2, N1, N1> loop, double noise) {
+    Matrix<N1, N1> y = plant.calculateY(loop.getXHat(), loop.getU()).plus(VecBuilder.fill(noise));
+
+    loop.correct(y);
+    loop.predict(kDt);
+  }
+
+  @Test
+  @SuppressWarnings("unchecked")
+  void testStateSpaceEnabled() {
+    m_loop.reset(VecBuilder.fill(0, 0));
+    Matrix<N2, N1> references = VecBuilder.fill(2.0, 0.0);
+    var constraints = new TrapezoidProfile.Constraints(4, 3);
+    m_loop.setNextR(references);
+
+    TrapezoidProfile profile;
+    TrapezoidProfile.State state;
+    for (int i = 0; i < 1000; i++) {
+      profile = new TrapezoidProfile(constraints);
+      state =
+          profile.calculate(
+              kDt,
+              new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)),
+              new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0)));
+      m_loop.setNextR(VecBuilder.fill(state.position, state.velocity));
+
+      updateTwoState(
+          (LinearSystem<N2, N1, N1>) m_plant.slice(0),
+          m_loop,
+          (random.nextGaussian()) * kPositionStddev);
+      var u = m_loop.getU(0);
+
+      assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u);
+    }
+
+    assertEquals(2.0, m_loop.getXHat(0), 0.05);
+    assertEquals(0.0, m_loop.getXHat(1), 0.5);
+  }
+
+  @Test
+  void testFlywheelEnabled() {
+    LinearSystem<N1, N1, N1> plant =
+        LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00289, 1.0);
+    KalmanFilter<N1, N1, N1> observer =
+        new KalmanFilter<>(
+            Nat.N1(), Nat.N1(), plant, VecBuilder.fill(1.0), VecBuilder.fill(kPositionStddev), kDt);
+
+    var qElms = VecBuilder.fill(9.0);
+    var rElms = VecBuilder.fill(12.0);
+
+    var controller = new LinearQuadraticRegulator<>(plant, qElms, rElms, kDt);
+
+    var feedforward = new LinearPlantInversionFeedforward<>(plant, kDt);
+
+    var loop = new LinearSystemLoop<>(controller, feedforward, observer, 12);
+
+    loop.reset(VecBuilder.fill(0.0));
+    var references = VecBuilder.fill(3000 / 60d * 2 * Math.PI);
+
+    loop.setNextR(references);
+
+    var time = 0.0;
+    while (time < 10) {
+      if (time > 10) {
+        break;
+      }
+
+      loop.setNextR(references);
+
+      Matrix<N1, N1> y =
+          plant
+              .calculateY(loop.getXHat(), loop.getU())
+              .plus(VecBuilder.fill(random.nextGaussian() * kPositionStddev));
+
+      loop.correct(y);
+      loop.predict(kDt);
+      var u = loop.getU(0);
+
+      assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u);
+
+      time += kDt;
+    }
+
+    assertEquals(0.0, loop.getError(0), 0.1);
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/PIDInputOutputTest.java

@@ -0,0 +1,82 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class PIDInputOutputTest {
+  @Test
+  void continuousInputTest() {
+    var controller = new PIDController(0.0, 0.0, 0.0);
+
+    controller.setP(1);
+    controller.enableContinuousInput(-180, 180);
+    assertEquals(controller.calculate(-179, 179), -2, 1e-5);
+
+    controller.enableContinuousInput(0, 360);
+    assertEquals(controller.calculate(1, 359), -2, 1e-5);
+  }
+
+  @Test
+  void proportionalGainOutputTest() {
+    var controller = new PIDController(0.0, 0.0, 0.0);
+
+    controller.setP(4);
+
+    assertEquals(-0.1, controller.calculate(0.025, 0), 1e-5);
+  }
+
+  @Test
+  void integralGainOutputTest() {
+    var controller = new PIDController(0.0, 0.0, 0.0);
+
+    controller.setI(4);
+
+    double out = 0;
+
+    for (int i = 0; i < 5; i++) {
+      out = controller.calculate(0.025, 0);
+    }
+
+    assertEquals(-0.5 * controller.getPeriod(), out, 1e-5);
+  }
+
+  @Test
+  void derivativeGainOutputTest() {
+    var controller = new PIDController(0.0, 0.0, 0.0);
+
+    controller.setD(4);
+
+    controller.calculate(0, 0);
+
+    assertEquals(-0.01 / controller.getPeriod(), controller.calculate(0.0025, 0), 1e-5);
+  }
+
+  @Test
+  void iZoneNoOutputTest() {
+    var controller = new PIDController(0.0, 0.0, 0.0);
+
+    controller.setI(1);
+    controller.setIZone(1);
+
+    double out = controller.calculate(2, 0);
+
+    assertEquals(0, out, 1e-5);
+  }
+
+  @Test
+  void iZoneOutputTest() {
+    var controller = new PIDController(0.0, 0.0, 0.0);
+
+    controller.setI(1);
+    controller.setIZone(1);
+
+    double out = controller.calculate(1, 0);
+
+    assertEquals(-1 * controller.getPeriod(), out, 1e-5);
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/PIDToleranceTest.java

@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+
+class PIDToleranceTest {
+  private static final double kSetpoint = 50.0;
+  private static final double kTolerance = 10.0;
+  private static final double kRange = 200;
+
+  @Test
+  void initialToleranceTest() {
+    try (var controller = new PIDController(0.05, 0.0, 0.0)) {
+      controller.enableContinuousInput(-kRange / 2, kRange / 2);
+
+      assertFalse(controller.atSetpoint());
+    }
+  }
+
+  @Test
+  void absoluteToleranceTest() {
+    try (var controller = new PIDController(0.05, 0.0, 0.0)) {
+      controller.enableContinuousInput(-kRange / 2, kRange / 2);
+
+      assertFalse(controller.atSetpoint());
+
+      controller.setTolerance(kTolerance);
+      controller.setSetpoint(kSetpoint);
+
+      assertFalse(
+          controller.atSetpoint(),
+          "Error was in tolerance when it should not have been. Error was "
+              + controller.getError());
+
+      controller.calculate(0.0);
+
+      assertFalse(
+          controller.atSetpoint(),
+          "Error was in tolerance when it should not have been. Error was "
+              + controller.getError());
+
+      controller.calculate(kSetpoint + kTolerance / 2);
+
+      assertTrue(
+          controller.atSetpoint(),
+          "Error was not in tolerance when it should have been. Error was "
+              + controller.getError());
+
+      controller.calculate(kSetpoint + 10 * kTolerance);
+
+      assertFalse(
+          controller.atSetpoint(),
+          "Error was in tolerance when it should not have been. Error was "
+              + controller.getError());
+    }
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/ProfiledPIDControllerTest.java

@@ -0,0 +1,22 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import org.junit.jupiter.api.Test;
+
+class ProfiledPIDControllerTest {
+  @Test
+  void testStartFromNonZeroPosition() {
+    ProfiledPIDController controller =
+        new ProfiledPIDController(1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1.0, 1.0));
+
+    controller.reset(20);
+
+    assertEquals(0.0, controller.calculate(20), 0.05);
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/ProfiledPIDInputOutputTest.java

@@ -0,0 +1,127 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import org.junit.jupiter.api.Test;
+
+class ProfiledPIDInputOutputTest {
+  @Test
+  void continuousInputTest1() {
+    var controller =
+        new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(360, 180));
+
+    controller.setP(1);
+    controller.enableContinuousInput(-180, 180);
+
+    final double kSetpoint = -179.0;
+    final double kMeasurement = -179.0;
+    final double kGoal = 179.0;
+
+    controller.reset(kSetpoint);
+    assertTrue(controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(controller.getSetpoint().position - kMeasurement) < 180.0);
+  }
+
+  @Test
+  void continuousInputTest2() {
+    var controller =
+        new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(360, 180));
+
+    controller.setP(1);
+    controller.enableContinuousInput(-Math.PI, Math.PI);
+
+    final double kSetpoint = -3.4826633343199735;
+    final double kMeasurement = -3.1352207333939606;
+    final double kGoal = -3.534162788601621;
+
+    controller.reset(kSetpoint);
+    assertTrue(controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(controller.getSetpoint().position - kMeasurement) < Math.PI);
+  }
+
+  @Test
+  void continuousInputTest3() {
+    var controller =
+        new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(360, 180));
+
+    controller.setP(1);
+    controller.enableContinuousInput(-Math.PI, Math.PI);
+
+    final double kSetpoint = -3.5176604690006377;
+    final double kMeasurement = 3.1191729343822456;
+    final double kGoal = 2.709680418117445;
+
+    controller.reset(kSetpoint);
+    assertTrue(controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(controller.getSetpoint().position - kMeasurement) < Math.PI);
+  }
+
+  @Test
+  void continuousInputTest4() {
+    var controller =
+        new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(360, 180));
+
+    controller.setP(1);
+    controller.enableContinuousInput(0, 2.0 * Math.PI);
+
+    final double kSetpoint = 2.78;
+    final double kMeasurement = 3.12;
+    final double kGoal = 2.71;
+
+    controller.reset(kSetpoint);
+    assertTrue(controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(controller.getSetpoint().position - kMeasurement) < Math.PI / 2.0);
+  }
+
+  @Test
+  void proportionalGainOutputTest() {
+    var controller =
+        new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(360, 180));
+
+    controller.setP(4);
+
+    assertEquals(-0.1, controller.calculate(0.025, 0), 1e-5);
+  }
+
+  @Test
+  void integralGainOutputTest() {
+    var controller =
+        new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(360, 180));
+
+    controller.setI(4);
+
+    double out = 0;
+
+    for (int i = 0; i < 5; i++) {
+      out = controller.calculate(0.025, 0);
+    }
+
+    assertEquals(-0.5 * controller.getPeriod(), out, 1e-5);
+  }
+
+  @Test
+  void derivativeGainOutputTest() {
+    var controller =
+        new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(360, 180));
+
+    controller.setD(4);
+
+    controller.calculate(0, 0);
+
+    assertEquals(-0.01 / controller.getPeriod(), controller.calculate(0.0025, 0), 1e-5);
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/SimpleMotorFeedforwardTest.java

@@ -0,0 +1,69 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import org.junit.jupiter.api.Test;
+
+class SimpleMotorFeedforwardTest {
+  @Test
+  void testCalculate() {
+    double Ks = 0.5;
+    double Kv = 3.0;
+    double Ka = 0.6;
+    double dt = 0.02;
+
+    var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -Kv / Ka);
+    var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / Ka);
+
+    var plantInversion = new LinearPlantInversionFeedforward<N1, N1, N1>(A, B, dt);
+    var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka, dt);
+
+    var r = VecBuilder.fill(2.0);
+    var nextR = VecBuilder.fill(3.0);
+    double currentVelocity = 2.0; // rad/s
+    double nextVelocity = 3.0; // rad/s
+
+    assertEquals(
+        37.52499583432516 + 0.5, simpleMotor.calculate(currentVelocity, nextVelocity), 0.002);
+    assertEquals(
+        plantInversion.calculate(r, nextR).get(0, 0) + Ks,
+        simpleMotor.calculate(currentVelocity, nextVelocity),
+        0.002);
+
+    // These won't match exactly. It's just an approximation to make sure they're
+    // in the same ballpark.
+    assertEquals(
+        plantInversion.calculate(r, nextR).get(0, 0) + Ks,
+        simpleMotor.calculate(currentVelocity, nextVelocity),
+        2.0);
+  }
+
+  @Test
+  void testNegativeGains() {
+    double Ks = 0.5;
+    double Kv = 3.5;
+    double Ka = 3.5;
+    double dt = 0.02;
+
+    assertAll(
+        () ->
+            assertThrows(
+                IllegalArgumentException.class, () -> new SimpleMotorFeedforward(Ks, -Kv, Ka, dt)),
+        () ->
+            assertThrows(
+                IllegalArgumentException.class, () -> new SimpleMotorFeedforward(Ks, Kv, -Ka, dt)),
+        () ->
+            assertThrows(
+                IllegalArgumentException.class, () -> new SimpleMotorFeedforward(Ks, Kv, Ka, 0)));
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/proto/ArmFeedforwardProtoTest.java

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufArmFeedforward;
+import org.junit.jupiter.api.Test;
+
+class ArmFeedforwardProtoTest {
+  private static final ArmFeedforward DATA = new ArmFeedforward(0.174, 0.229, 4.4, 4.4);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufArmFeedforward proto = ArmFeedforward.proto.createMessage();
+    ArmFeedforward.proto.pack(proto, DATA);
+
+    ArmFeedforward data = ArmFeedforward.proto.unpack(proto);
+    assertEquals(DATA.getKs(), data.getKs());
+    assertEquals(DATA.getKg(), data.getKg());
+    assertEquals(DATA.getKv(), data.getKv());
+    assertEquals(DATA.getKa(), data.getKa());
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/proto/DifferentialDriveFeedforwardProtoTest.java

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.DifferentialDriveFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufDifferentialDriveFeedforward;
+import edu.wpi.first.wpilibj.ProtoTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class DifferentialDriveFeedforwardProtoTest
+    extends ProtoTestBase<DifferentialDriveFeedforward, ProtobufDifferentialDriveFeedforward> {
+  DifferentialDriveFeedforwardProtoTest() {
+    super(
+        new DifferentialDriveFeedforward(0.174, 0.229, 4.4, 4.5),
+        DifferentialDriveFeedforward.proto);
+  }
+
+  @Override
+  public void checkEquals(
+      DifferentialDriveFeedforward testData, DifferentialDriveFeedforward data) {
+    assertEquals(testData.m_kVLinear, data.m_kVLinear);
+    assertEquals(testData.m_kALinear, data.m_kALinear);
+    assertEquals(testData.m_kVAngular, data.m_kVAngular);
+    assertEquals(testData.m_kAAngular, data.m_kAAngular);
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/proto/DifferentialDriveWheelVoltagesProtoTest.java

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages;
+import edu.wpi.first.math.proto.Controller.ProtobufDifferentialDriveWheelVoltages;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveWheelVoltagesProtoTest {
+  private static final DifferentialDriveWheelVoltages DATA =
+      new DifferentialDriveWheelVoltages(0.174, 0.191);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufDifferentialDriveWheelVoltages proto =
+        DifferentialDriveWheelVoltages.proto.createMessage();
+    DifferentialDriveWheelVoltages.proto.pack(proto, DATA);
+
+    DifferentialDriveWheelVoltages data = DifferentialDriveWheelVoltages.proto.unpack(proto);
+    assertEquals(DATA.left, data.left);
+    assertEquals(DATA.right, data.right);
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/proto/ElevatorFeedforwardProtoTest.java

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufElevatorFeedforward;
+import org.junit.jupiter.api.Test;
+
+class ElevatorFeedforwardProtoTest {
+  private static final ElevatorFeedforward DATA = new ElevatorFeedforward(1.91, 1.1, 1.1, 0.229);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufElevatorFeedforward proto = ElevatorFeedforward.proto.createMessage();
+    ElevatorFeedforward.proto.pack(proto, DATA);
+
+    ElevatorFeedforward data = ElevatorFeedforward.proto.unpack(proto);
+    assertEquals(DATA.getKs(), data.getKs());
+    assertEquals(DATA.getKg(), data.getKg());
+    assertEquals(DATA.getKv(), data.getKv());
+    assertEquals(DATA.getKa(), data.getKa());
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/proto/SimpleMotorFeedforwardProtoTest.java

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufSimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.ProtoTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class SimpleMotorFeedforwardProtoTest
+    extends ProtoTestBase<SimpleMotorFeedforward, ProtobufSimpleMotorFeedforward> {
+  SimpleMotorFeedforwardProtoTest() {
+    super(new SimpleMotorFeedforward(0.4, 4.0, 0.7, 0.025), SimpleMotorFeedforward.proto);
+  }
+
+  @Override
+  public void checkEquals(SimpleMotorFeedforward testData, SimpleMotorFeedforward data) {
+    assertEquals(testData.getKs(), data.getKs());
+    assertEquals(testData.getKv(), data.getKv());
+    assertEquals(testData.getKa(), data.getKa());
+    assertEquals(testData.getDt(), data.getDt());
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/struct/ArmFeedforwardStructTest.java

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class ArmFeedforwardStructTest {
+  private static final ArmFeedforward DATA = new ArmFeedforward(0.174, 0.229, 4.4, 4.4);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(ArmFeedforward.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    ArmFeedforward.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    ArmFeedforward data = ArmFeedforward.struct.unpack(buffer);
+    assertEquals(DATA.getKs(), data.getKs());
+    assertEquals(DATA.getKg(), data.getKg());
+    assertEquals(DATA.getKv(), data.getKv());
+    assertEquals(DATA.getKa(), data.getKa());
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/struct/DifferentialDriveFeedforwardStructTest.java

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.DifferentialDriveFeedforward;
+import edu.wpi.first.wpilibj.StructTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class DifferentialDriveFeedforwardStructTest extends StructTestBase<DifferentialDriveFeedforward> {
+  DifferentialDriveFeedforwardStructTest() {
+    super(
+        new DifferentialDriveFeedforward(0.174, 0.229, 4.4, 4.5),
+        DifferentialDriveFeedforward.struct);
+  }
+
+  @Override
+  public void checkEquals(
+      DifferentialDriveFeedforward testData, DifferentialDriveFeedforward data) {
+    assertEquals(testData.m_kVLinear, data.m_kVLinear);
+    assertEquals(testData.m_kALinear, data.m_kALinear);
+    assertEquals(testData.m_kVAngular, data.m_kVAngular);
+    assertEquals(testData.m_kAAngular, data.m_kAAngular);
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/struct/DifferentialDriveWheelVoltagesStructTest.java

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveWheelVoltagesStructTest {
+  private static final DifferentialDriveWheelVoltages DATA =
+      new DifferentialDriveWheelVoltages(0.174, 0.191);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveWheelVoltages.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    DifferentialDriveWheelVoltages.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    DifferentialDriveWheelVoltages data = DifferentialDriveWheelVoltages.struct.unpack(buffer);
+    assertEquals(DATA.left, data.left);
+    assertEquals(DATA.right, data.right);
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/struct/ElevatorFeedforwardStructTest.java

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class ElevatorFeedforwardStructTest {
+  private static final ElevatorFeedforward DATA = new ElevatorFeedforward(1.91, 1.1, 1.1, 0.229);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(ElevatorFeedforward.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    ElevatorFeedforward.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    ElevatorFeedforward data = ElevatorFeedforward.struct.unpack(buffer);
+    assertEquals(DATA.getKs(), data.getKs());
+    assertEquals(DATA.getKg(), data.getKg());
+    assertEquals(DATA.getKv(), data.getKv());
+    assertEquals(DATA.getKa(), data.getKa());
+  }
+}

wpimath/src/test/java/org/wpilib/math/controller/struct/SimpleMotorFeedforwardStructTest.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.controller.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.StructTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class SimpleMotorFeedforwardStructTest extends StructTestBase<SimpleMotorFeedforward> {
+  SimpleMotorFeedforwardStructTest() {
+    super(new SimpleMotorFeedforward(0.4, 4.0, 0.7, 0.025), SimpleMotorFeedforward.struct);
+  }
+
+  @Override
+  public void checkEquals(SimpleMotorFeedforward testData, SimpleMotorFeedforward data) {
+    assertEquals(testData.getKs(), data.getKs());
+    assertEquals(testData.getKv(), data.getKv());
+    assertEquals(testData.getKa(), data.getKa());
+    assertEquals(testData.getDt(), data.getDt());
+  }
+}

wpimath/src/test/java/org/wpilib/math/estimator/AngleStatisticsTest.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.estimator;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import org.junit.jupiter.api.Test;
+
+class AngleStatisticsTest {
+  @Test
+  void testMean() {
+    // 3 states, 2 sigmas
+    var sigmas =
+        MatBuilder.fill(Nat.N3(), Nat.N2(), 1, 1.2, Math.toRadians(359), Math.toRadians(3), 1, 2);
+    // Weights need to produce the mean of the sigmas
+    var weights = new Matrix<>(Nat.N2(), Nat.N1());
+    weights.fill(1.0 / sigmas.getNumCols());
+
+    assertTrue(
+        AngleStatistics.angleMean(sigmas, weights, 1)
+            .isEqual(VecBuilder.fill(1.1, Math.toRadians(1), 1.5), 1e-6));
+  }
+
+  @Test
+  void testResidual() {
+    var first = VecBuilder.fill(1, Math.toRadians(1), 2);
+    var second = VecBuilder.fill(1, Math.toRadians(359), 1);
+    assertTrue(
+        AngleStatistics.angleResidual(first, second, 1)
+            .isEqual(VecBuilder.fill(0, Math.toRadians(2), 1), 1e-6));
+  }
+
+  @Test
+  void testAdd() {
+    var first = VecBuilder.fill(1, Math.toRadians(1), 2);
+    var second = VecBuilder.fill(1, Math.toRadians(359), 1);
+    assertTrue(AngleStatistics.angleAdd(first, second, 1).isEqual(VecBuilder.fill(2, 0, 3), 1e-6));
+  }
+}

wpimath/src/test/java/org/wpilib/math/estimator/DifferentialDrivePoseEstimator3dTest.java

@@ -0,0 +1,462 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+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.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.List;
+import java.util.Optional;
+import java.util.Random;
+import java.util.TreeMap;
+import java.util.function.Function;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDrivePoseEstimator3dTest {
+  private static final double kEpsilon = 1e-9;
+
+  @Test
+  void testAccuracy() {
+    var kinematics = new DifferentialDriveKinematics(1);
+
+    var estimator =
+        new DifferentialDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            0,
+            0,
+            Pose3d.kZero,
+            VecBuilder.fill(0.02, 0.02, 0.02, 0.01),
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1));
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(2, 2));
+
+    testFollowTrajectory(
+        kinematics,
+        estimator,
+        trajectory,
+        state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
+        state -> state.pose,
+        trajectory.getInitialPose(),
+        new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+        0.02,
+        0.1,
+        0.25,
+        true);
+  }
+
+  @Test
+  void testBadInitialPose() {
+    var kinematics = new DifferentialDriveKinematics(1);
+
+    var estimator =
+        new DifferentialDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            0,
+            0,
+            Pose3d.kZero,
+            VecBuilder.fill(0.02, 0.02, 0.02, 0.01),
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1));
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(2, 2));
+
+    for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) {
+      for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) {
+        var pose_offset = Rotation2d.fromDegrees(offset_direction_degs);
+        var heading_offset = Rotation2d.fromDegrees(offset_heading_degs);
+
+        var initial_pose =
+            trajectory
+                .getInitialPose()
+                .plus(
+                    new Transform2d(
+                        new Translation2d(pose_offset.getCos(), pose_offset.getSin()),
+                        heading_offset));
+
+        testFollowTrajectory(
+            kinematics,
+            estimator,
+            trajectory,
+            state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
+            state -> state.pose,
+            initial_pose,
+            new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+            0.02,
+            0.1,
+            0.25,
+            false);
+      }
+    }
+  }
+
+  void testFollowTrajectory(
+      final DifferentialDriveKinematics kinematics,
+      final DifferentialDrivePoseEstimator3d estimator,
+      final Trajectory trajectory,
+      final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
+      final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
+      final Pose2d startingPose,
+      final Pose2d endingPose,
+      final double dt,
+      final double visionUpdateRate,
+      final double visionUpdateDelay,
+      final boolean checkError) {
+    double leftDistance = 0;
+    double rightDistance = 0;
+
+    estimator.resetPosition(
+        Rotation3d.kZero, leftDistance, rightDistance, new Pose3d(startingPose));
+
+    var rand = new Random(3538);
+
+    double t = 0.0;
+
+    final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    while (t <= trajectory.getTotalTime()) {
+      var groundTruthState = trajectory.sample(t);
+
+      // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
+      // last vision measurement
+      if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) {
+        Pose2d newVisionPose =
+            visionMeasurementGenerator
+                .apply(groundTruthState)
+                .plus(
+                    new Transform2d(
+                        new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1),
+                        new Rotation2d(rand.nextGaussian() * 0.05)));
+
+        visionUpdateQueue.put(t, newVisionPose);
+      }
+
+      // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds
+      // since it was measured
+      if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) {
+        var visionEntry = visionUpdateQueue.pollFirstEntry();
+        estimator.addVisionMeasurement(new Pose3d(visionEntry.getValue()), visionEntry.getKey());
+      }
+
+      var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
+
+      var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+
+      leftDistance += wheelSpeeds.left * dt;
+      rightDistance += wheelSpeeds.right * dt;
+
+      var xHat =
+          estimator.updateWithTime(
+              t,
+              new Rotation3d(
+                  groundTruthState
+                      .pose
+                      .getRotation()
+                      .plus(new Rotation2d(rand.nextGaussian() * 0.05))
+                      .minus(trajectory.getInitialPose().getRotation())),
+              leftDistance,
+              rightDistance);
+
+      double error =
+          groundTruthState
+              .pose
+              .getTranslation()
+              .getDistance(xHat.getTranslation().toTranslation2d());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+        endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X");
+    assertEquals(
+        endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y");
+    assertEquals(
+        endingPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().toRotation2d().getRadians(),
+        0.15,
+        "Incorrect Final Theta");
+
+    if (checkError) {
+      assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error");
+      assertEquals(0.0, maxError, 0.2, "Incorrect max error");
+    }
+  }
+
+  @Test
+  void testSimultaneousVisionMeasurements() {
+    // This tests for multiple vision measurements applied at the same time. The expected behavior
+    // is that all measurements affect the estimated pose. The alternative result is that only one
+    // vision measurement affects the outcome. If that were the case, after 1000 measurements, the
+    // estimated pose would converge to that measurement.
+    var kinematics = new DifferentialDriveKinematics(1);
+
+    var estimator =
+        new DifferentialDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            0,
+            0,
+            new Pose3d(1, 2, 0, new Rotation3d(0, 0, -Math.PI / 2)),
+            VecBuilder.fill(0.02, 0.02, 0.02, 0.01),
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1));
+
+    estimator.updateWithTime(0, Rotation3d.kZero, 0, 0);
+
+    var visionMeasurements =
+        new Pose2d[] {
+          new Pose2d(0, 0, Rotation2d.kZero),
+          new Pose2d(3, 1, Rotation2d.kCCW_Pi_2),
+          new Pose2d(2, 4, Rotation2d.kPi),
+        };
+
+    for (int i = 0; i < 1000; i++) {
+      for (var measurement : visionMeasurements) {
+        estimator.addVisionMeasurement(new Pose3d(measurement), 0);
+      }
+    }
+
+    for (var measurement : visionMeasurements) {
+      var errorLog =
+          "Estimator converged to one vision measurement: "
+              + estimator.getEstimatedPosition().toString()
+              + " -> "
+              + measurement;
+
+      var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
+      var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
+      var dtheta =
+          Math.abs(
+              measurement.getRotation().getDegrees()
+                  - estimator.getEstimatedPosition().getRotation().toRotation2d().getDegrees());
+
+      assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog);
+    }
+  }
+
+  @Test
+  void testDiscardsStaleVisionMeasurements() {
+    var kinematics = new DifferentialDriveKinematics(1);
+    var estimator =
+        new DifferentialDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            0,
+            0,
+            Pose3d.kZero,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9, 0.9));
+
+    double time = 0;
+
+    // Add enough measurements to fill up the buffer
+    for (; time < 4; time += 0.02) {
+      estimator.updateWithTime(time, Rotation3d.kZero, 0, 0);
+    }
+
+    var odometryPose = estimator.getEstimatedPosition();
+
+    // Apply a vision measurement made 3 seconds ago
+    // This test passes if this does not cause a ConcurrentModificationException.
+    estimator.addVisionMeasurement(
+        new Pose3d(10, 10, 0, new Rotation3d(0, 0, 0.1)), 1, VecBuilder.fill(0.1, 0.1, 0.1, 0.1));
+
+    assertAll(
+        () ->
+            assertEquals(
+                odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X"),
+        () ->
+            assertEquals(
+                odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y"),
+        () ->
+            assertEquals(
+                odometryPose.getZ(), estimator.getEstimatedPosition().getZ(), "Incorrect Final Y"),
+        () ->
+            assertEquals(
+                odometryPose.getRotation().getX(),
+                estimator.getEstimatedPosition().getRotation().getX(),
+                "Incorrect Final Roll"),
+        () ->
+            assertEquals(
+                odometryPose.getRotation().getY(),
+                estimator.getEstimatedPosition().getRotation().getY(),
+                "Incorrect Final Pitch"),
+        () ->
+            assertEquals(
+                odometryPose.getRotation().getZ(),
+                estimator.getEstimatedPosition().getRotation().getZ(),
+                "Incorrect Final Yaw"));
+  }
+
+  @Test
+  void testSampleAt() {
+    var kinematics = new DifferentialDriveKinematics(1);
+    var estimator =
+        new DifferentialDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            0,
+            0,
+            Pose3d.kZero,
+            VecBuilder.fill(1, 1, 1, 1),
+            VecBuilder.fill(1, 1, 1, 1));
+
+    // Returns empty when null
+    assertEquals(Optional.empty(), estimator.sampleAt(1));
+
+    // Add odometry measurements, but don't fill up the buffer
+    // Add a tiny tolerance for the upper bound because of floating point rounding error
+    for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
+      estimator.updateWithTime(time, Rotation3d.kZero, time, time);
+    }
+
+    // Sample at an added time
+    assertEquals(Optional.of(new Pose3d(1.02, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.02));
+    // Sample between updates (test interpolation)
+    assertEquals(Optional.of(new Pose3d(1.01, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.01));
+    // Sampling before the oldest value returns the oldest value
+    assertEquals(Optional.of(new Pose3d(1, 0, 0, Rotation3d.kZero)), estimator.sampleAt(0.5));
+    // Sampling after the newest value returns the newest value
+    assertEquals(Optional.of(new Pose3d(2, 0, 0, Rotation3d.kZero)), estimator.sampleAt(2.5));
+
+    // Add a vision measurement after the odometry measurements (while keeping all of the old
+    // odometry measurements)
+    estimator.addVisionMeasurement(new Pose3d(2, 0, 0, new Rotation3d(0, 0, 1)), 2.2);
+
+    // Make sure nothing changed (except the newest value)
+    assertEquals(Optional.of(new Pose3d(1.02, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose3d(1.01, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose3d(1, 0, 0, Rotation3d.kZero)), estimator.sampleAt(0.5));
+
+    // Add a vision measurement before the odometry measurements that's still in the buffer
+    estimator.addVisionMeasurement(new Pose3d(1, 0.2, 0, Rotation3d.kZero), 0.9);
+
+    // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
+    assertEquals(Optional.of(new Pose3d(1.02, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose3d(1.01, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose3d(1, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(0.5));
+    assertEquals(Optional.of(new Pose3d(2, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(2.5));
+  }
+
+  @Test
+  void testReset() {
+    var kinematics = new DifferentialDriveKinematics(1);
+    var estimator =
+        new DifferentialDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            0,
+            0,
+            Pose3d.kZero,
+            VecBuilder.fill(1, 1, 1, 1),
+            VecBuilder.fill(1, 1, 1, 1));
+
+    // Test reset position
+    estimator.resetPosition(Rotation3d.kZero, 1, 1, new Pose3d(1, 0, 0, Rotation3d.kZero));
+
+    assertAll(
+        () -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+
+    // Test orientation and wheel positions
+    estimator.update(Rotation3d.kZero, 2, 2);
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+
+    // Test reset rotation
+    estimator.resetRotation(new Rotation3d(Rotation2d.kCCW_Pi_2));
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+
+    // Test orientation
+    estimator.update(Rotation3d.kZero, 3, 3);
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+
+    // Test reset translation
+    estimator.resetTranslation(new Translation3d(-1, -1, -1));
+
+    assertAll(
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+
+    // Test reset pose
+    estimator.resetPose(Pose3d.kZero);
+
+    assertAll(
+        () -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+  }
+}

wpimath/src/test/java/org/wpilib/math/estimator/DifferentialDrivePoseEstimatorTest.java

@@ -0,0 +1,427 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.List;
+import java.util.Optional;
+import java.util.Random;
+import java.util.TreeMap;
+import java.util.function.Function;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDrivePoseEstimatorTest {
+  private static final double kEpsilon = 1e-9;
+
+  @Test
+  void testAccuracy() {
+    var kinematics = new DifferentialDriveKinematics(1);
+
+    var estimator =
+        new DifferentialDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            0,
+            0,
+            Pose2d.kZero,
+            VecBuilder.fill(0.02, 0.02, 0.01),
+            VecBuilder.fill(0.1, 0.1, 0.1));
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(2, 2));
+
+    testFollowTrajectory(
+        kinematics,
+        estimator,
+        trajectory,
+        state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
+        state -> state.pose,
+        trajectory.getInitialPose(),
+        new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+        0.02,
+        0.1,
+        0.25,
+        true);
+  }
+
+  @Test
+  void testBadInitialPose() {
+    var kinematics = new DifferentialDriveKinematics(1);
+
+    var estimator =
+        new DifferentialDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            0,
+            0,
+            Pose2d.kZero,
+            VecBuilder.fill(0.02, 0.02, 0.01),
+            VecBuilder.fill(0.1, 0.1, 0.1));
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(2, 2));
+
+    for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) {
+      for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) {
+        var pose_offset = Rotation2d.fromDegrees(offset_direction_degs);
+        var heading_offset = Rotation2d.fromDegrees(offset_heading_degs);
+
+        var initial_pose =
+            trajectory
+                .getInitialPose()
+                .plus(
+                    new Transform2d(
+                        new Translation2d(pose_offset.getCos(), pose_offset.getSin()),
+                        heading_offset));
+
+        testFollowTrajectory(
+            kinematics,
+            estimator,
+            trajectory,
+            state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
+            state -> state.pose,
+            initial_pose,
+            new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+            0.02,
+            0.1,
+            0.25,
+            false);
+      }
+    }
+  }
+
+  void testFollowTrajectory(
+      final DifferentialDriveKinematics kinematics,
+      final DifferentialDrivePoseEstimator estimator,
+      final Trajectory trajectory,
+      final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
+      final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
+      final Pose2d startingPose,
+      final Pose2d endingPose,
+      final double dt,
+      final double visionUpdateRate,
+      final double visionUpdateDelay,
+      final boolean checkError) {
+    double leftDistance = 0;
+    double rightDistance = 0;
+
+    estimator.resetPosition(Rotation2d.kZero, leftDistance, rightDistance, startingPose);
+
+    var rand = new Random(3538);
+
+    double t = 0.0;
+
+    final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    while (t <= trajectory.getTotalTime()) {
+      var groundTruthState = trajectory.sample(t);
+
+      // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
+      // last vision measurement
+      if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) {
+        Pose2d newVisionPose =
+            visionMeasurementGenerator
+                .apply(groundTruthState)
+                .plus(
+                    new Transform2d(
+                        new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1),
+                        new Rotation2d(rand.nextGaussian() * 0.05)));
+
+        visionUpdateQueue.put(t, newVisionPose);
+      }
+
+      // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds
+      // since it was measured
+      if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) {
+        var visionEntry = visionUpdateQueue.pollFirstEntry();
+        estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey());
+      }
+
+      var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
+
+      var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+
+      leftDistance += wheelSpeeds.left * dt;
+      rightDistance += wheelSpeeds.right * dt;
+
+      var xHat =
+          estimator.updateWithTime(
+              t,
+              groundTruthState
+                  .pose
+                  .getRotation()
+                  .plus(new Rotation2d(rand.nextGaussian() * 0.05))
+                  .minus(trajectory.getInitialPose().getRotation()),
+              leftDistance,
+              rightDistance);
+
+      double error = groundTruthState.pose.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+        endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X");
+    assertEquals(
+        endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y");
+    assertEquals(
+        endingPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().getRadians(),
+        0.15,
+        "Incorrect Final Theta");
+
+    if (checkError) {
+      assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error");
+      assertEquals(0.0, maxError, 0.2, "Incorrect max error");
+    }
+  }
+
+  @Test
+  void testSimultaneousVisionMeasurements() {
+    // This tests for multiple vision measurements applied at the same time. The expected behavior
+    // is that all measurements affect the estimated pose. The alternative result is that only one
+    // vision measurement affects the outcome. If that were the case, after 1000 measurements, the
+    // estimated pose would converge to that measurement.
+    var kinematics = new DifferentialDriveKinematics(1);
+
+    var estimator =
+        new DifferentialDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            0,
+            0,
+            new Pose2d(1, 2, Rotation2d.kCW_Pi_2),
+            VecBuilder.fill(0.02, 0.02, 0.01),
+            VecBuilder.fill(0.1, 0.1, 0.1));
+
+    estimator.updateWithTime(0, Rotation2d.kZero, 0, 0);
+
+    var visionMeasurements =
+        new Pose2d[] {
+          new Pose2d(0, 0, Rotation2d.kZero),
+          new Pose2d(3, 1, Rotation2d.kCCW_Pi_2),
+          new Pose2d(2, 4, Rotation2d.kPi),
+        };
+
+    for (int i = 0; i < 1000; i++) {
+      for (var measurement : visionMeasurements) {
+        estimator.addVisionMeasurement(measurement, 0);
+      }
+    }
+
+    for (var measurement : visionMeasurements) {
+      var errorLog =
+          "Estimator converged to one vision measurement: "
+              + estimator.getEstimatedPosition().toString()
+              + " -> "
+              + measurement;
+
+      var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
+      var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
+      var dtheta =
+          Math.abs(
+              measurement.getRotation().getDegrees()
+                  - estimator.getEstimatedPosition().getRotation().getDegrees());
+
+      assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog);
+    }
+  }
+
+  @Test
+  void testDiscardsStaleVisionMeasurements() {
+    var kinematics = new DifferentialDriveKinematics(1);
+    var estimator =
+        new DifferentialDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            0,
+            0,
+            Pose2d.kZero,
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9));
+
+    double time = 0;
+
+    // Add enough measurements to fill up the buffer
+    for (; time < 4; time += 0.02) {
+      estimator.updateWithTime(time, Rotation2d.kZero, 0, 0);
+    }
+
+    var odometryPose = estimator.getEstimatedPosition();
+
+    // Apply a vision measurement made 3 seconds ago
+    // This test passes if this does not cause a ConcurrentModificationException.
+    estimator.addVisionMeasurement(
+        new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)),
+        1,
+        VecBuilder.fill(0.1, 0.1, 0.1));
+
+    assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X");
+    assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y");
+    assertEquals(
+        odometryPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().getRadians(),
+        "Incorrect Final Theta");
+  }
+
+  @Test
+  void testSampleAt() {
+    var kinematics = new DifferentialDriveKinematics(1);
+    var estimator =
+        new DifferentialDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            0,
+            0,
+            Pose2d.kZero,
+            VecBuilder.fill(1, 1, 1),
+            VecBuilder.fill(1, 1, 1));
+
+    // Returns empty when null
+    assertEquals(Optional.empty(), estimator.sampleAt(1));
+
+    // Add odometry measurements, but don't fill up the buffer
+    // Add a tiny tolerance for the upper bound because of floating point rounding error
+    for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
+      estimator.updateWithTime(time, Rotation2d.kZero, time, time);
+    }
+
+    // Sample at an added time
+    assertEquals(Optional.of(new Pose2d(1.02, 0, Rotation2d.kZero)), estimator.sampleAt(1.02));
+    // Sample between updates (test interpolation)
+    assertEquals(Optional.of(new Pose2d(1.01, 0, Rotation2d.kZero)), estimator.sampleAt(1.01));
+    // Sampling before the oldest value returns the oldest value
+    assertEquals(Optional.of(new Pose2d(1, 0, Rotation2d.kZero)), estimator.sampleAt(0.5));
+    // Sampling after the newest value returns the newest value
+    assertEquals(Optional.of(new Pose2d(2, 0, Rotation2d.kZero)), estimator.sampleAt(2.5));
+
+    // Add a vision measurement after the odometry measurements (while keeping all of the old
+    // odometry measurements)
+    estimator.addVisionMeasurement(new Pose2d(2, 0, new Rotation2d(1)), 2.2);
+
+    // Make sure nothing changed (except the newest value)
+    assertEquals(Optional.of(new Pose2d(1.02, 0, Rotation2d.kZero)), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose2d(1.01, 0, Rotation2d.kZero)), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose2d(1, 0, Rotation2d.kZero)), estimator.sampleAt(0.5));
+
+    // Add a vision measurement before the odometry measurements that's still in the buffer
+    estimator.addVisionMeasurement(new Pose2d(1, 0.2, Rotation2d.kZero), 0.9);
+
+    // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
+    assertEquals(Optional.of(new Pose2d(1.02, 0.1, Rotation2d.kZero)), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose2d(1.01, 0.1, Rotation2d.kZero)), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose2d(1, 0.1, Rotation2d.kZero)), estimator.sampleAt(0.5));
+    assertEquals(Optional.of(new Pose2d(2, 0.1, Rotation2d.kZero)), estimator.sampleAt(2.5));
+  }
+
+  @Test
+  void testReset() {
+    var kinematics = new DifferentialDriveKinematics(1);
+    var estimator =
+        new DifferentialDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            0,
+            0,
+            Pose2d.kZero,
+            VecBuilder.fill(1, 1, 1),
+            VecBuilder.fill(1, 1, 1));
+
+    // Test reset position
+    estimator.resetPosition(Rotation2d.kZero, 1, 1, new Pose2d(1, 0, Rotation2d.kZero));
+
+    assertAll(
+        () -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
+
+    // Test orientation and wheel positions
+    estimator.update(Rotation2d.kZero, 2, 2);
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
+
+    // Test reset rotation
+    estimator.resetRotation(Rotation2d.kCCW_Pi_2);
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test orientation
+    estimator.update(Rotation2d.kZero, 3, 3);
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test reset translation
+    estimator.resetTranslation(new Translation2d(-1, -1));
+
+    assertAll(
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test reset pose
+    estimator.resetPose(Pose2d.kZero);
+
+    assertAll(
+        () -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
+  }
+}

wpimath/src/test/java/org/wpilib/math/estimator/ExtendedKalmanFilterTest.java

@@ -0,0 +1,185 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.NumericalJacobian;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class ExtendedKalmanFilterTest {
+  private static Matrix<N5, N1> getDynamics(final Matrix<N5, N1> x, final Matrix<N2, N1> u) {
+    final var motors = DCMotor.getCIM(2);
+
+    final var gr = 7.08; // Gear ratio
+    final var rb = 0.8382 / 2.0; // Wheelbase radius (trackwidth)
+    final var r = 0.0746125; // Wheel radius
+    final var m = 63.503; // Robot mass
+    final var J = 5.6; // Robot moment of inertia
+
+    final var C1 = -Math.pow(gr, 2) * motors.Kt / (motors.Kv * motors.R * r * r);
+    final var C2 = gr * motors.Kt / (motors.R * r);
+    final var k1 = 1.0 / m + rb * rb / J;
+    final var k2 = 1.0 / m - rb * rb / J;
+
+    final var vl = x.get(3, 0);
+    final var vr = x.get(4, 0);
+    final var Vl = u.get(0, 0);
+    final var Vr = u.get(1, 0);
+
+    final Matrix<N5, N1> result = new Matrix<>(Nat.N5(), Nat.N1());
+    final var v = 0.5 * (vl + vr);
+    result.set(0, 0, v * Math.cos(x.get(2, 0)));
+    result.set(1, 0, v * Math.sin(x.get(2, 0)));
+    result.set(2, 0, (vr - vl) / (2.0 * rb));
+    result.set(3, 0, k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr)));
+    result.set(4, 0, k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr)));
+    return result;
+  }
+
+  @SuppressWarnings("PMD.UnusedFormalParameter")
+  private static Matrix<N3, N1> getLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+    return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0));
+  }
+
+  @SuppressWarnings("PMD.UnusedFormalParameter")
+  private static Matrix<N5, N1> getGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+    return VecBuilder.fill(x.get(0, 0), x.get(1, 0), x.get(2, 0), x.get(3, 0), x.get(4, 0));
+  }
+
+  @Test
+  void testInit() {
+    double dt = 0.005;
+
+    assertDoesNotThrow(
+        () -> {
+          ExtendedKalmanFilter<N5, N2, N3> observer =
+              new ExtendedKalmanFilter<>(
+                  Nat.N5(),
+                  Nat.N2(),
+                  Nat.N3(),
+                  ExtendedKalmanFilterTest::getDynamics,
+                  ExtendedKalmanFilterTest::getLocalMeasurementModel,
+                  VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
+                  VecBuilder.fill(0.0001, 0.01, 0.01),
+                  dt);
+
+          Matrix<N2, N1> u = VecBuilder.fill(12.0, 12.0);
+          observer.predict(u, dt);
+
+          var localY = getLocalMeasurementModel(observer.getXhat(), u);
+          observer.correct(u, localY);
+
+          var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
+          var R = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
+          observer.correct(
+              Nat.N5(), u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R);
+        });
+  }
+
+  @Test
+  void testConvergence() {
+    double dt = 0.005;
+    double rb = 0.8382 / 2.0; // Robot radius
+
+    ExtendedKalmanFilter<N5, N2, N3> observer =
+        new ExtendedKalmanFilter<>(
+            Nat.N5(),
+            Nat.N2(),
+            Nat.N3(),
+            ExtendedKalmanFilterTest::getDynamics,
+            ExtendedKalmanFilterTest::getLocalMeasurementModel,
+            VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
+            VecBuilder.fill(0.001, 0.01, 0.01),
+            dt);
+
+    List<Pose2d> waypoints =
+        List.of(
+            new Pose2d(2.75, 22.521, Rotation2d.kZero),
+            new Pose2d(24.73, 19.68, Rotation2d.fromRadians(5.846)));
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));
+
+    Matrix<N5, N1> r = new Matrix<>(Nat.N5(), Nat.N1());
+
+    Matrix<N5, N1> nextR = new Matrix<>(Nat.N5(), Nat.N1());
+    Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
+
+    var B =
+        NumericalJacobian.numericalJacobianU(
+            Nat.N5(),
+            Nat.N2(),
+            ExtendedKalmanFilterTest::getDynamics,
+            new Matrix<>(Nat.N5(), Nat.N1()),
+            u);
+
+    observer.setXhat(
+        VecBuilder.fill(
+            trajectory.getInitialPose().getTranslation().getX(),
+            trajectory.getInitialPose().getTranslation().getY(),
+            trajectory.getInitialPose().getRotation().getRadians(),
+            0.0,
+            0.0));
+
+    var groundTruthX = observer.getXhat();
+
+    double totalTime = trajectory.getTotalTime();
+    for (int i = 0; i < (totalTime / dt); i++) {
+      var ref = trajectory.sample(dt * i);
+      double vl = ref.velocity * (1 - (ref.curvature * rb));
+      double vr = ref.velocity * (1 + (ref.curvature * rb));
+
+      nextR.set(0, 0, ref.pose.getTranslation().getX());
+      nextR.set(1, 0, ref.pose.getTranslation().getY());
+      nextR.set(2, 0, ref.pose.getRotation().getRadians());
+      nextR.set(3, 0, vl);
+      nextR.set(4, 0, vr);
+
+      var localY = getLocalMeasurementModel(groundTruthX, u);
+      var whiteNoiseStdDevs = VecBuilder.fill(0.0001, 0.5, 0.5);
+      observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(whiteNoiseStdDevs)));
+
+      Matrix<N5, N1> rdot = nextR.minus(r).div(dt);
+      u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
+
+      observer.predict(u, dt);
+
+      groundTruthX =
+          NumericalIntegration.rk4(ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dt);
+
+      r = nextR;
+    }
+
+    var localY = getLocalMeasurementModel(observer.getXhat(), u);
+    observer.correct(u, localY);
+
+    var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
+    var R = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
+    observer.correct(Nat.N5(), u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R);
+
+    var finalPosition = trajectory.sample(trajectory.getTotalTime());
+    assertEquals(finalPosition.pose.getTranslation().getX(), observer.getXhat(0), 1.0);
+    assertEquals(finalPosition.pose.getTranslation().getY(), observer.getXhat(1), 1.0);
+    assertEquals(finalPosition.pose.getRotation().getRadians(), observer.getXhat(2), 1.0);
+    assertEquals(0.0, observer.getXhat(3), 1.0);
+    assertEquals(0.0, observer.getXhat(4), 1.0);
+  }
+}

wpimath/src/test/java/org/wpilib/math/estimator/KalmanFilterTest.java

@@ -0,0 +1,193 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N6;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.List;
+import java.util.Random;
+import org.junit.jupiter.api.Test;
+
+class KalmanFilterTest {
+  private static LinearSystem<N2, N1, N2> elevatorPlant;
+
+  private static final double kDt = 0.005;
+
+  static {
+    createElevator();
+  }
+
+  private static void createElevator() {
+    var motors = DCMotor.getVex775Pro(2);
+
+    var m = 5.0;
+    var r = 0.0181864;
+    var G = 1.0;
+    elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G);
+  }
+
+  // A swerve drive system where the states are [x, y, theta, vx, vy, vTheta]ᵀ,
+  // Y is [x, y, theta]ᵀ and u is [ax, ay, alpha}ᵀ
+  LinearSystem<N6, N3, N3> m_swerveObserverSystem =
+      new LinearSystem<>(
+          MatBuilder.fill(
+              Nat.N6(), Nat.N6(), // A
+              0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+              0, 0, 0, 0, 0, 0, 0),
+          MatBuilder.fill(
+              Nat.N6(), Nat.N3(), // B
+              0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1),
+          MatBuilder.fill(
+              Nat.N3(), Nat.N6(), // C
+              1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0),
+          new Matrix<>(Nat.N3(), Nat.N3())); // D
+
+  @Test
+  @SuppressWarnings("unchecked")
+  void testElevatorKalmanFilter() {
+    var Q = VecBuilder.fill(0.05, 1.0);
+    var R = VecBuilder.fill(0.0001);
+
+    assertDoesNotThrow(
+        () ->
+            new KalmanFilter<>(
+                Nat.N2(), Nat.N1(), (LinearSystem<N2, N1, N1>) elevatorPlant.slice(0), Q, R, kDt));
+  }
+
+  @Test
+  void testSwerveKFStationary() {
+    var random = new Random();
+
+    var filter =
+        new KalmanFilter<>(
+            Nat.N6(),
+            Nat.N3(),
+            m_swerveObserverSystem,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
+            // weights
+            VecBuilder.fill(2, 2, 2), // measurement weights
+            0.020);
+
+    Matrix<N3, N1> measurement;
+    for (int i = 0; i < 100; i++) {
+      // the robot is at [0, 0, 0] so we just park here
+      measurement =
+          VecBuilder.fill(random.nextGaussian(), random.nextGaussian(), random.nextGaussian());
+      filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement);
+
+      // we continue to not accelerate
+      filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020);
+    }
+
+    assertEquals(0.0, filter.getXhat(0), 0.3);
+    assertEquals(0.0, filter.getXhat(0), 0.3);
+  }
+
+  @Test
+  void testSwerveKFMovingWithoutAccelerating() {
+    var random = new Random();
+
+    var filter =
+        new KalmanFilter<>(
+            Nat.N6(),
+            Nat.N3(),
+            m_swerveObserverSystem,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
+            // weights
+            VecBuilder.fill(4, 4, 4), // measurement weights
+            0.020);
+
+    // we set the velocity of the robot so that it's moving forward slowly
+    filter.setXhat(0, 0.5);
+    filter.setXhat(1, 0.5);
+
+    for (int i = 0; i < 300; i++) {
+      // the robot is at [0, 0, 0] so we just park here
+      var measurement =
+          VecBuilder.fill(
+              random.nextGaussian() / 10d,
+              random.nextGaussian() / 10d,
+              random.nextGaussian() / 4d // std dev of [1, 1, 1]
+              );
+
+      filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement);
+
+      // we continue to not accelerate
+      filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020);
+    }
+
+    assertEquals(0.0, filter.getXhat(0), 0.2);
+    assertEquals(0.0, filter.getXhat(1), 0.2);
+  }
+
+  @Test
+  void testSwerveKFMovingOverTrajectory() {
+    var random = new Random();
+
+    var filter =
+        new KalmanFilter<>(
+            Nat.N6(),
+            Nat.N3(),
+            m_swerveObserverSystem,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
+            // weights
+            VecBuilder.fill(4, 4, 4), // measurement weights
+            0.020);
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(new Pose2d(0, 0, Rotation2d.kZero), new Pose2d(5, 5, Rotation2d.kZero)),
+            new TrajectoryConfig(2, 2));
+    var time = 0.0;
+    var lastVelocity = VecBuilder.fill(0.0, 0.0, 0.0);
+
+    while (time <= trajectory.getTotalTime()) {
+      var sample = trajectory.sample(time);
+      var measurement =
+          VecBuilder.fill(
+              sample.pose.getTranslation().getX() + random.nextGaussian() / 5d,
+              sample.pose.getTranslation().getY() + random.nextGaussian() / 5d,
+              sample.pose.getRotation().getRadians() + random.nextGaussian() / 3d);
+
+      var velocity =
+          VecBuilder.fill(
+              sample.velocity * sample.pose.getRotation().getCos(),
+              sample.velocity * sample.pose.getRotation().getSin(),
+              sample.curvature * sample.velocity);
+      var u = (velocity.minus(lastVelocity)).div(0.020);
+      lastVelocity = velocity;
+
+      filter.correct(u, measurement);
+      filter.predict(u, 0.020);
+
+      time += 0.020;
+    }
+
+    assertEquals(
+        trajectory.sample(trajectory.getTotalTime()).pose.getTranslation().getX(),
+        filter.getXhat(0),
+        0.2);
+    assertEquals(
+        trajectory.sample(trajectory.getTotalTime()).pose.getTranslation().getY(),
+        filter.getXhat(1),
+        0.2);
+  }
+}

wpimath/src/test/java/org/wpilib/math/estimator/MecanumDrivePoseEstimator3dTest.java

@@ -0,0 +1,491 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+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.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.List;
+import java.util.Optional;
+import java.util.Random;
+import java.util.TreeMap;
+import java.util.function.Function;
+import org.junit.jupiter.api.Test;
+
+class MecanumDrivePoseEstimator3dTest {
+  private static final double kEpsilon = 1e-9;
+
+  @Test
+  void testAccuracyFacingTrajectory() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1), new Translation2d(1, -1),
+            new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+    var wheelPositions = new MecanumDriveWheelPositions();
+
+    var estimator =
+        new MecanumDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            wheelPositions,
+            Pose3d.kZero,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1),
+            VecBuilder.fill(0.45, 0.45, 0.45, 0.1));
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(2, 2));
+
+    testFollowTrajectory(
+        kinematics,
+        estimator,
+        trajectory,
+        state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
+        state -> state.pose,
+        trajectory.getInitialPose(),
+        new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+        0.02,
+        0.1,
+        0.25,
+        true);
+  }
+
+  @Test
+  void testBadInitialPose() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1), new Translation2d(1, -1),
+            new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+    var wheelPositions = new MecanumDriveWheelPositions();
+
+    var estimator =
+        new MecanumDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            wheelPositions,
+            Pose3d.kZero,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1),
+            VecBuilder.fill(0.45, 0.45, 0.45, 0.1));
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(2, 2));
+
+    for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) {
+      for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) {
+        var pose_offset = Rotation2d.fromDegrees(offset_direction_degs);
+        var heading_offset = Rotation2d.fromDegrees(offset_heading_degs);
+
+        var initial_pose =
+            trajectory
+                .getInitialPose()
+                .plus(
+                    new Transform2d(
+                        new Translation2d(pose_offset.getCos(), pose_offset.getSin()),
+                        heading_offset));
+
+        testFollowTrajectory(
+            kinematics,
+            estimator,
+            trajectory,
+            state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
+            state -> state.pose,
+            initial_pose,
+            new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+            0.02,
+            0.1,
+            0.25,
+            false);
+      }
+    }
+  }
+
+  void testFollowTrajectory(
+      final MecanumDriveKinematics kinematics,
+      final MecanumDrivePoseEstimator3d estimator,
+      final Trajectory trajectory,
+      final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
+      final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
+      final Pose2d startingPose,
+      final Pose2d endingPose,
+      final double dt,
+      final double visionUpdateRate,
+      final double visionUpdateDelay,
+      final boolean checkError) {
+    var wheelPositions = new MecanumDriveWheelPositions();
+
+    estimator.resetPosition(Rotation3d.kZero, wheelPositions, new Pose3d(startingPose));
+
+    var rand = new Random(3538);
+
+    double t = 0.0;
+
+    final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    while (t <= trajectory.getTotalTime()) {
+      var groundTruthState = trajectory.sample(t);
+
+      // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
+      // last vision measurement
+      if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) {
+        Pose2d newVisionPose =
+            visionMeasurementGenerator
+                .apply(groundTruthState)
+                .plus(
+                    new Transform2d(
+                        new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1),
+                        new Rotation2d(rand.nextGaussian() * 0.05)));
+
+        visionUpdateQueue.put(t, newVisionPose);
+      }
+
+      // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds
+      // since it was measured
+      if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) {
+        var visionEntry = visionUpdateQueue.pollFirstEntry();
+        estimator.addVisionMeasurement(new Pose3d(visionEntry.getValue()), visionEntry.getKey());
+      }
+
+      var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
+
+      var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+
+      wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
+      wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
+      wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
+      wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
+
+      var xHat =
+          estimator.updateWithTime(
+              t,
+              new Rotation3d(
+                  groundTruthState
+                      .pose
+                      .getRotation()
+                      .plus(new Rotation2d(rand.nextGaussian() * 0.05))
+                      .minus(trajectory.getInitialPose().getRotation())),
+              wheelPositions);
+
+      double error =
+          groundTruthState
+              .pose
+              .getTranslation()
+              .getDistance(xHat.getTranslation().toTranslation2d());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+        endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X");
+    assertEquals(
+        endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y");
+    assertEquals(
+        endingPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().toRotation2d().getRadians(),
+        0.15,
+        "Incorrect Final Theta");
+
+    if (checkError) {
+      assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error");
+      assertEquals(0.0, maxError, 0.2, "Incorrect max error");
+    }
+  }
+
+  @Test
+  void testSimultaneousVisionMeasurements() {
+    // This tests for multiple vision measurements applied at the same time. The expected behavior
+    // is that all measurements affect the estimated pose. The alternative result is that only one
+    // vision measurement affects the outcome. If that were the case, after 1000 measurements, the
+    // estimated pose would converge to that measurement.
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1), new Translation2d(1, -1),
+            new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+    var wheelPositions = new MecanumDriveWheelPositions();
+
+    var estimator =
+        new MecanumDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            wheelPositions,
+            new Pose3d(1, 2, 0, new Rotation3d(0, 0, -Math.PI / 2)),
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1),
+            VecBuilder.fill(0.45, 0.45, 0.45, 0.1));
+
+    estimator.updateWithTime(0, Rotation3d.kZero, wheelPositions);
+
+    var visionMeasurements =
+        new Pose2d[] {
+          new Pose2d(0, 0, Rotation2d.kZero),
+          new Pose2d(3, 1, Rotation2d.kCCW_Pi_2),
+          new Pose2d(2, 4, Rotation2d.kPi),
+        };
+
+    for (int i = 0; i < 1000; i++) {
+      for (var measurement : visionMeasurements) {
+        estimator.addVisionMeasurement(new Pose3d(measurement), 0);
+      }
+    }
+
+    for (var measurement : visionMeasurements) {
+      var errorLog =
+          "Estimator converged to one vision measurement: "
+              + estimator.getEstimatedPosition().toString()
+              + " -> "
+              + measurement;
+
+      var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
+      var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
+      var dtheta =
+          Math.abs(
+              measurement.getRotation().getDegrees()
+                  - estimator.getEstimatedPosition().getRotation().toRotation2d().getDegrees());
+
+      assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog);
+    }
+  }
+
+  @Test
+  void testDiscardsOldVisionMeasurements() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new MecanumDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            new MecanumDriveWheelPositions(),
+            Pose3d.kZero,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9, 0.9));
+
+    double time = 0;
+
+    // Add enough measurements to fill up the buffer
+    for (; time < 4; time += 0.02) {
+      estimator.updateWithTime(time, Rotation3d.kZero, new MecanumDriveWheelPositions());
+    }
+
+    var odometryPose = estimator.getEstimatedPosition();
+
+    // Apply a vision measurement made 3 seconds ago
+    // This test passes if this does not cause a ConcurrentModificationException.
+    estimator.addVisionMeasurement(
+        new Pose3d(10, 10, 0, new Rotation3d(0, 0, 0.1)), 1, VecBuilder.fill(0.1, 0.1, 0.1, 0.1));
+
+    assertAll(
+        () ->
+            assertEquals(
+                odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X"),
+        () ->
+            assertEquals(
+                odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y"),
+        () ->
+            assertEquals(
+                odometryPose.getZ(), estimator.getEstimatedPosition().getZ(), "Incorrect Final Y"),
+        () ->
+            assertEquals(
+                odometryPose.getRotation().getX(),
+                estimator.getEstimatedPosition().getRotation().getX(),
+                "Incorrect Final Roll"),
+        () ->
+            assertEquals(
+                odometryPose.getRotation().getY(),
+                estimator.getEstimatedPosition().getRotation().getY(),
+                "Incorrect Final Pitch"),
+        () ->
+            assertEquals(
+                odometryPose.getRotation().getZ(),
+                estimator.getEstimatedPosition().getRotation().getZ(),
+                "Incorrect Final Yaw"));
+  }
+
+  @Test
+  void testSampleAt() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new MecanumDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            new MecanumDriveWheelPositions(),
+            Pose3d.kZero,
+            VecBuilder.fill(1, 1, 1, 1),
+            VecBuilder.fill(1, 1, 1, 1));
+
+    // Returns empty when null
+    assertEquals(Optional.empty(), estimator.sampleAt(1));
+
+    // Add odometry measurements, but don't fill up the buffer
+    // Add a tiny tolerance for the upper bound because of floating point rounding error
+    for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
+      var wheelPositions = new MecanumDriveWheelPositions(time, time, time, time);
+      estimator.updateWithTime(time, Rotation3d.kZero, wheelPositions);
+    }
+
+    // Sample at an added time
+    assertEquals(Optional.of(new Pose3d(1.02, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.02));
+    // Sample between updates (test interpolation)
+    assertEquals(Optional.of(new Pose3d(1.01, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.01));
+    // Sampling before the oldest value returns the oldest value
+    assertEquals(Optional.of(new Pose3d(1, 0, 0, Rotation3d.kZero)), estimator.sampleAt(0.5));
+    // Sampling after the newest value returns the newest value
+    assertEquals(Optional.of(new Pose3d(2, 0, 0, Rotation3d.kZero)), estimator.sampleAt(2.5));
+
+    // Add a vision measurement after the odometry measurements (while keeping all of the old
+    // odometry measurements)
+    estimator.addVisionMeasurement(new Pose3d(2, 0, 0, new Rotation3d(0, 0, 1)), 2.2);
+
+    // Make sure nothing changed (except the newest value)
+    assertEquals(Optional.of(new Pose3d(1.02, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose3d(1.01, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose3d(1, 0, 0, Rotation3d.kZero)), estimator.sampleAt(0.5));
+
+    // Add a vision measurement before the odometry measurements that's still in the buffer
+    estimator.addVisionMeasurement(new Pose3d(1, 0.2, 0, Rotation3d.kZero), 0.9);
+
+    // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
+    assertEquals(Optional.of(new Pose3d(1.02, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose3d(1.01, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose3d(1, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(0.5));
+    assertEquals(Optional.of(new Pose3d(2, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(2.5));
+  }
+
+  @Test
+  void testReset() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new MecanumDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            new MecanumDriveWheelPositions(),
+            Pose3d.kZero,
+            VecBuilder.fill(1, 1, 1, 1),
+            VecBuilder.fill(1, 1, 1, 1));
+
+    // Test reset position
+    estimator.resetPosition(
+        Rotation3d.kZero,
+        new MecanumDriveWheelPositions(1, 1, 1, 1),
+        new Pose3d(1, 0, 0, Rotation3d.kZero));
+
+    assertAll(
+        () -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+
+    // Test orientation and wheel positions
+    estimator.update(Rotation3d.kZero, new MecanumDriveWheelPositions(2, 2, 2, 2));
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+
+    // Test reset rotation
+    estimator.resetRotation(new Rotation3d(Rotation2d.kCCW_Pi_2));
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+
+    // Test orientation
+    estimator.update(Rotation3d.kZero, new MecanumDriveWheelPositions(3, 3, 3, 3));
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+
+    // Test reset translation
+    estimator.resetTranslation(new Translation3d(-1, -1, -1));
+
+    assertAll(
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+
+    // Test reset pose
+    estimator.resetPose(Pose3d.kZero);
+
+    assertAll(
+        () -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+  }
+}

wpimath/src/test/java/org/wpilib/math/estimator/MecanumDrivePoseEstimatorTest.java

@@ -0,0 +1,457 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.List;
+import java.util.Optional;
+import java.util.Random;
+import java.util.TreeMap;
+import java.util.function.Function;
+import org.junit.jupiter.api.Test;
+
+class MecanumDrivePoseEstimatorTest {
+  private static final double kEpsilon = 1e-9;
+
+  @Test
+  void testAccuracyFacingTrajectory() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1), new Translation2d(1, -1),
+            new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+    var wheelPositions = new MecanumDriveWheelPositions();
+
+    var estimator =
+        new MecanumDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            wheelPositions,
+            Pose2d.kZero,
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.45, 0.45, 0.1));
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(2, 2));
+
+    testFollowTrajectory(
+        kinematics,
+        estimator,
+        trajectory,
+        state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
+        state -> state.pose,
+        trajectory.getInitialPose(),
+        new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+        0.02,
+        0.1,
+        0.25,
+        true);
+  }
+
+  @Test
+  void testBadInitialPose() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1), new Translation2d(1, -1),
+            new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+    var wheelPositions = new MecanumDriveWheelPositions();
+
+    var estimator =
+        new MecanumDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            wheelPositions,
+            Pose2d.kZero,
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.45, 0.45, 0.1));
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(2, 2));
+
+    for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) {
+      for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) {
+        var pose_offset = Rotation2d.fromDegrees(offset_direction_degs);
+        var heading_offset = Rotation2d.fromDegrees(offset_heading_degs);
+
+        var initial_pose =
+            trajectory
+                .getInitialPose()
+                .plus(
+                    new Transform2d(
+                        new Translation2d(pose_offset.getCos(), pose_offset.getSin()),
+                        heading_offset));
+
+        testFollowTrajectory(
+            kinematics,
+            estimator,
+            trajectory,
+            state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
+            state -> state.pose,
+            initial_pose,
+            new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+            0.02,
+            0.1,
+            0.25,
+            false);
+      }
+    }
+  }
+
+  void testFollowTrajectory(
+      final MecanumDriveKinematics kinematics,
+      final MecanumDrivePoseEstimator estimator,
+      final Trajectory trajectory,
+      final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
+      final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
+      final Pose2d startingPose,
+      final Pose2d endingPose,
+      final double dt,
+      final double visionUpdateRate,
+      final double visionUpdateDelay,
+      final boolean checkError) {
+    var wheelPositions = new MecanumDriveWheelPositions();
+
+    estimator.resetPosition(Rotation2d.kZero, wheelPositions, startingPose);
+
+    var rand = new Random(3538);
+
+    double t = 0.0;
+
+    final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    while (t <= trajectory.getTotalTime()) {
+      var groundTruthState = trajectory.sample(t);
+
+      // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
+      // last vision measurement
+      if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) {
+        Pose2d newVisionPose =
+            visionMeasurementGenerator
+                .apply(groundTruthState)
+                .plus(
+                    new Transform2d(
+                        new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1),
+                        new Rotation2d(rand.nextGaussian() * 0.05)));
+
+        visionUpdateQueue.put(t, newVisionPose);
+      }
+
+      // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds
+      // since it was measured
+      if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) {
+        var visionEntry = visionUpdateQueue.pollFirstEntry();
+        estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey());
+      }
+
+      var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
+
+      var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+
+      wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
+      wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
+      wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
+      wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
+
+      var xHat =
+          estimator.updateWithTime(
+              t,
+              groundTruthState
+                  .pose
+                  .getRotation()
+                  .plus(new Rotation2d(rand.nextGaussian() * 0.05))
+                  .minus(trajectory.getInitialPose().getRotation()),
+              wheelPositions);
+
+      double error = groundTruthState.pose.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+        endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X");
+    assertEquals(
+        endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y");
+    assertEquals(
+        endingPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().getRadians(),
+        0.15,
+        "Incorrect Final Theta");
+
+    if (checkError) {
+      assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error");
+      assertEquals(0.0, maxError, 0.2, "Incorrect max error");
+    }
+  }
+
+  @Test
+  void testSimultaneousVisionMeasurements() {
+    // This tests for multiple vision measurements applied at the same time. The expected behavior
+    // is that all measurements affect the estimated pose. The alternative result is that only one
+    // vision measurement affects the outcome. If that were the case, after 1000 measurements, the
+    // estimated pose would converge to that measurement.
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1), new Translation2d(1, -1),
+            new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+    var wheelPositions = new MecanumDriveWheelPositions();
+
+    var estimator =
+        new MecanumDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            wheelPositions,
+            new Pose2d(1, 2, Rotation2d.kCW_Pi_2),
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.45, 0.45, 0.1));
+
+    estimator.updateWithTime(0, Rotation2d.kZero, wheelPositions);
+
+    var visionMeasurements =
+        new Pose2d[] {
+          new Pose2d(0, 0, Rotation2d.kZero),
+          new Pose2d(3, 1, Rotation2d.kCCW_Pi_2),
+          new Pose2d(2, 4, Rotation2d.kPi),
+        };
+
+    for (int i = 0; i < 1000; i++) {
+      for (var measurement : visionMeasurements) {
+        estimator.addVisionMeasurement(measurement, 0);
+      }
+    }
+
+    for (var measurement : visionMeasurements) {
+      var errorLog =
+          "Estimator converged to one vision measurement: "
+              + estimator.getEstimatedPosition().toString()
+              + " -> "
+              + measurement;
+
+      var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
+      var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
+      var dtheta =
+          Math.abs(
+              measurement.getRotation().getDegrees()
+                  - estimator.getEstimatedPosition().getRotation().getDegrees());
+
+      assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog);
+    }
+  }
+
+  @Test
+  void testDiscardsOldVisionMeasurements() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new MecanumDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            new MecanumDriveWheelPositions(),
+            Pose2d.kZero,
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9));
+
+    double time = 0;
+
+    // Add enough measurements to fill up the buffer
+    for (; time < 4; time += 0.02) {
+      estimator.updateWithTime(time, Rotation2d.kZero, new MecanumDriveWheelPositions());
+    }
+
+    var odometryPose = estimator.getEstimatedPosition();
+
+    // Apply a vision measurement made 3 seconds ago
+    // This test passes if this does not cause a ConcurrentModificationException.
+    estimator.addVisionMeasurement(
+        new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)),
+        1,
+        VecBuilder.fill(0.1, 0.1, 0.1));
+
+    assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X");
+    assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y");
+    assertEquals(
+        odometryPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().getRadians(),
+        "Incorrect Final Theta");
+  }
+
+  @Test
+  void testSampleAt() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new MecanumDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            new MecanumDriveWheelPositions(),
+            Pose2d.kZero,
+            VecBuilder.fill(1, 1, 1),
+            VecBuilder.fill(1, 1, 1));
+
+    // Returns empty when null
+    assertEquals(Optional.empty(), estimator.sampleAt(1));
+
+    // Add odometry measurements, but don't fill up the buffer
+    // Add a tiny tolerance for the upper bound because of floating point rounding error
+    for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
+      var wheelPositions = new MecanumDriveWheelPositions(time, time, time, time);
+      estimator.updateWithTime(time, Rotation2d.kZero, wheelPositions);
+    }
+
+    // Sample at an added time
+    assertEquals(Optional.of(new Pose2d(1.02, 0, Rotation2d.kZero)), estimator.sampleAt(1.02));
+    // Sample between updates (test interpolation)
+    assertEquals(Optional.of(new Pose2d(1.01, 0, Rotation2d.kZero)), estimator.sampleAt(1.01));
+    // Sampling before the oldest value returns the oldest value
+    assertEquals(Optional.of(new Pose2d(1, 0, Rotation2d.kZero)), estimator.sampleAt(0.5));
+    // Sampling after the newest value returns the newest value
+    assertEquals(Optional.of(new Pose2d(2, 0, Rotation2d.kZero)), estimator.sampleAt(2.5));
+
+    // Add a vision measurement after the odometry measurements (while keeping all of the old
+    // odometry measurements)
+    estimator.addVisionMeasurement(new Pose2d(2, 0, new Rotation2d(1)), 2.2);
+
+    // Make sure nothing changed (except the newest value)
+    assertEquals(Optional.of(new Pose2d(1.02, 0, Rotation2d.kZero)), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose2d(1.01, 0, Rotation2d.kZero)), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose2d(1, 0, Rotation2d.kZero)), estimator.sampleAt(0.5));
+
+    // Add a vision measurement before the odometry measurements that's still in the buffer
+    estimator.addVisionMeasurement(new Pose2d(1, 0.2, Rotation2d.kZero), 0.9);
+
+    // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
+    assertEquals(Optional.of(new Pose2d(1.02, 0.1, Rotation2d.kZero)), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose2d(1.01, 0.1, Rotation2d.kZero)), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose2d(1, 0.1, Rotation2d.kZero)), estimator.sampleAt(0.5));
+    assertEquals(Optional.of(new Pose2d(2, 0.1, Rotation2d.kZero)), estimator.sampleAt(2.5));
+  }
+
+  @Test
+  void testReset() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new MecanumDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            new MecanumDriveWheelPositions(),
+            Pose2d.kZero,
+            VecBuilder.fill(1, 1, 1),
+            VecBuilder.fill(1, 1, 1));
+
+    // Test reset position
+    estimator.resetPosition(
+        Rotation2d.kZero,
+        new MecanumDriveWheelPositions(1, 1, 1, 1),
+        new Pose2d(1, 0, Rotation2d.kZero));
+
+    assertAll(
+        () -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
+
+    // Test orientation and wheel positions
+    estimator.update(Rotation2d.kZero, new MecanumDriveWheelPositions(2, 2, 2, 2));
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
+
+    // Test reset rotation
+    estimator.resetRotation(Rotation2d.kCCW_Pi_2);
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test orientation
+    estimator.update(Rotation2d.kZero, new MecanumDriveWheelPositions(3, 3, 3, 3));
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test reset translation
+    estimator.resetTranslation(new Translation2d(-1, -1));
+
+    assertAll(
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test reset pose
+    estimator.resetPose(Pose2d.kZero);
+
+    assertAll(
+        () -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
+  }
+}

wpimath/src/test/java/org/wpilib/math/estimator/MerweScaledSigmaPointsTest.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 static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import org.junit.jupiter.api.Test;
+
+class MerweScaledSigmaPointsTest {
+  @Test
+  void testZeroMeanPoints() {
+    var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
+    var points =
+        merweScaledSigmaPoints.squareRootSigmaPoints(
+            VecBuilder.fill(0, 0), MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1));
+
+    assertTrue(
+        points.isEqual(
+            MatBuilder.fill(
+                Nat.N2(),
+                Nat.N5(),
+                0.0,
+                0.00173205,
+                0.0,
+                -0.00173205,
+                0.0,
+                0.0,
+                0.0,
+                0.00173205,
+                0.0,
+                -0.00173205),
+            1E-6));
+  }
+
+  @Test
+  void testNonzeroMeanPoints() {
+    var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
+    var points =
+        merweScaledSigmaPoints.squareRootSigmaPoints(
+            VecBuilder.fill(1, 2), MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, Math.sqrt(10)));
+
+    assertTrue(
+        points.isEqual(
+            MatBuilder.fill(
+                Nat.N2(),
+                Nat.N5(),
+                1.0,
+                1.00173205,
+                1.0,
+                0.99826795,
+                1.0,
+                2.0,
+                2.0,
+                2.00547723,
+                2.0,
+                1.99452277),
+            1E-6));
+  }
+}

wpimath/src/test/java/org/wpilib/math/estimator/MerweUKFTest.java

@@ -0,0 +1,392 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.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.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N4;
+import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.system.Discretization;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.NumericalJacobian;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class MerweUKFTest {
+  private static Matrix<N5, N1> driveDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+    var motors = DCMotor.getCIM(2);
+
+    // var gLow = 15.32;    // Low gear ratio
+    var gHigh = 7.08; // High gear ratio
+    var rb = 0.8382 / 2.0; // Robot radius
+    var r = 0.0746125; // Wheel radius
+    var m = 63.503; // Robot mass
+    var J = 5.6; // Robot moment of inertia
+
+    var C1 = -Math.pow(gHigh, 2) * motors.Kt / (motors.Kv * motors.R * r * r);
+    var C2 = gHigh * motors.Kt / (motors.R * r);
+    var k1 = 1.0 / m + Math.pow(rb, 2) / J;
+    var k2 = 1.0 / m - Math.pow(rb, 2) / J;
+
+    var vl = x.get(3, 0);
+    var vr = x.get(4, 0);
+    var Vl = u.get(0, 0);
+    var Vr = u.get(1, 0);
+
+    var v = 0.5 * (vl + vr);
+    return VecBuilder.fill(
+        v * Math.cos(x.get(2, 0)),
+        v * Math.sin(x.get(2, 0)),
+        (vr - vl) / (2.0 * rb),
+        k1 * (C1 * vl + C2 * Vl) + k2 * (C1 * vr + C2 * Vr),
+        k2 * (C1 * vl + C2 * Vl) + k1 * (C1 * vr + C2 * Vr));
+  }
+
+  @SuppressWarnings("PMD.UnusedFormalParameter")
+  private static Matrix<N3, N1> driveLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+    return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0));
+  }
+
+  @SuppressWarnings("PMD.UnusedFormalParameter")
+  private static Matrix<N5, N1> driveGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+    return x.copy();
+  }
+
+  @Test
+  void testDriveInit() {
+    var dt = 0.005;
+    assertDoesNotThrow(
+        () -> {
+          MerweUKF<N5, N2, N3> observer =
+              new MerweUKF<>(
+                  Nat.N5(),
+                  Nat.N3(),
+                  MerweUKFTest::driveDynamics,
+                  MerweUKFTest::driveLocalMeasurementModel,
+                  VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
+                  VecBuilder.fill(0.0001, 0.01, 0.01),
+                  AngleStatistics.angleMean(2),
+                  AngleStatistics.angleMean(0),
+                  AngleStatistics.angleResidual(2),
+                  AngleStatistics.angleResidual(0),
+                  AngleStatistics.angleAdd(2),
+                  dt);
+
+          var u = VecBuilder.fill(12.0, 12.0);
+          observer.predict(u, dt);
+
+          var localY = driveLocalMeasurementModel(observer.getXhat(), u);
+          observer.correct(u, localY);
+
+          var globalY = driveGlobalMeasurementModel(observer.getXhat(), u);
+          var R =
+              StateSpaceUtil.makeCovarianceMatrix(
+                  Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.01, 0.01));
+          observer.correct(
+              Nat.N5(),
+              u,
+              globalY,
+              MerweUKFTest::driveGlobalMeasurementModel,
+              R,
+              AngleStatistics.angleMean(2),
+              AngleStatistics.angleResidual(2),
+              AngleStatistics.angleResidual(2),
+              AngleStatistics.angleAdd(2));
+        });
+  }
+
+  @Test
+  void testDriveConvergence() {
+    final double dt = 0.005;
+    final double rb = 0.8382 / 2.0; // Robot radius
+
+    MerweUKF<N5, N2, N3> observer =
+        new MerweUKF<>(
+            Nat.N5(),
+            Nat.N3(),
+            MerweUKFTest::driveDynamics,
+            MerweUKFTest::driveLocalMeasurementModel,
+            VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
+            VecBuilder.fill(0.0001, 0.5, 0.5),
+            AngleStatistics.angleMean(2),
+            AngleStatistics.angleMean(0),
+            AngleStatistics.angleResidual(2),
+            AngleStatistics.angleResidual(0),
+            AngleStatistics.angleAdd(2),
+            dt);
+
+    List<Pose2d> waypoints =
+        List.of(
+            new Pose2d(2.75, 22.521, Rotation2d.kZero),
+            new Pose2d(24.73, 19.68, Rotation2d.fromRadians(5.846)));
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));
+
+    Matrix<N5, N1> r = new Matrix<>(Nat.N5(), Nat.N1());
+    Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
+
+    var B =
+        NumericalJacobian.numericalJacobianU(
+            Nat.N5(),
+            Nat.N2(),
+            MerweUKFTest::driveDynamics,
+            new Matrix<>(Nat.N5(), Nat.N1()),
+            new Matrix<>(Nat.N2(), Nat.N1()));
+
+    observer.setXhat(
+        VecBuilder.fill(
+            trajectory.getInitialPose().getTranslation().getX(),
+            trajectory.getInitialPose().getTranslation().getY(),
+            trajectory.getInitialPose().getRotation().getRadians(),
+            0.0,
+            0.0));
+
+    var trueXhat = observer.getXhat();
+
+    double totalTime = trajectory.getTotalTime();
+    for (int i = 0; i < (totalTime / dt); ++i) {
+      var ref = trajectory.sample(dt * i);
+      double vl = ref.velocity * (1 - (ref.curvature * rb));
+      double vr = ref.velocity * (1 + (ref.curvature * rb));
+
+      var nextR =
+          VecBuilder.fill(
+              ref.pose.getTranslation().getX(),
+              ref.pose.getTranslation().getY(),
+              ref.pose.getRotation().getRadians(),
+              vl,
+              vr);
+
+      Matrix<N3, N1> localY =
+          driveLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
+      var noiseStdDev = VecBuilder.fill(0.0001, 0.5, 0.5);
+
+      observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev)));
+
+      var rdot = nextR.minus(r).div(dt);
+      u = new Matrix<>(B.solve(rdot.minus(driveDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
+
+      observer.predict(u, dt);
+
+      r = nextR;
+      trueXhat = NumericalIntegration.rk4(MerweUKFTest::driveDynamics, trueXhat, u, dt);
+    }
+
+    var localY = driveLocalMeasurementModel(trueXhat, u);
+    observer.correct(u, localY);
+
+    var globalY = driveGlobalMeasurementModel(trueXhat, u);
+    var R =
+        StateSpaceUtil.makeCovarianceMatrix(
+            Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
+    observer.correct(
+        Nat.N5(),
+        u,
+        globalY,
+        MerweUKFTest::driveGlobalMeasurementModel,
+        R,
+        AngleStatistics.angleMean(2),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleAdd(2));
+
+    final var finalPosition = trajectory.sample(trajectory.getTotalTime());
+
+    assertEquals(finalPosition.pose.getTranslation().getX(), observer.getXhat(0), 0.055);
+    assertEquals(finalPosition.pose.getTranslation().getY(), observer.getXhat(1), 0.15);
+    assertEquals(finalPosition.pose.getRotation().getRadians(), observer.getXhat(2), 0.000005);
+    assertEquals(0.0, observer.getXhat(3), 0.1);
+    assertEquals(0.0, observer.getXhat(4), 0.1);
+  }
+
+  @Test
+  void testLinearUKF() {
+    var dt = 0.020;
+    var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
+    var observer =
+        new MerweUKF<>(
+            Nat.N1(),
+            Nat.N1(),
+            (x, u) -> plant.getA().times(x).plus(plant.getB().times(u)),
+            plant::calculateY,
+            VecBuilder.fill(0.05),
+            VecBuilder.fill(1.0),
+            dt);
+
+    var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dt);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    Matrix<N1, N1> ref = VecBuilder.fill(100);
+    Matrix<N1, N1> u = VecBuilder.fill(0);
+
+    for (int i = 0; i < (2.0 / dt); ++i) {
+      observer.predict(u, dt);
+
+      u = discB.solve(ref.minus(discA.times(ref)));
+    }
+
+    assertEquals(ref.get(0, 0), observer.getXhat(0), 5);
+  }
+
+  @Test
+  void testRoundTripP() {
+    var dt = 0.005;
+
+    var observer =
+        new MerweUKF<>(
+            Nat.N2(),
+            Nat.N2(),
+            (x, u) -> x,
+            (x, u) -> x,
+            VecBuilder.fill(0.0, 0.0),
+            VecBuilder.fill(0.0, 0.0),
+            dt);
+
+    var P = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 1.0, 2.0);
+    observer.setP(P);
+
+    assertTrue(observer.getP().isEqual(P, 1e-9));
+  }
+
+  // Second system, single motor feedforward estimator
+  private static Matrix<N4, N1> motorDynamics(Matrix<N4, N1> x, Matrix<N1, N1> u) {
+    double v = x.get(1, 0);
+    double kV = x.get(2, 0);
+    double kA = x.get(3, 0);
+
+    double V = u.get(0, 0);
+
+    double a = -kV / kA * v + 1.0 / kA * V;
+    return MatBuilder.fill(Nat.N4(), Nat.N1(), v, a, 0, 0);
+  }
+
+  private static Matrix<N3, N1> motorMeasurementModel(Matrix<N4, N1> x, Matrix<N1, N1> u) {
+    double p = x.get(0, 0);
+    double v = x.get(1, 0);
+    double kV = x.get(2, 0);
+    double kA = x.get(3, 0);
+    double V = u.get(0, 0);
+
+    double a = -kV / kA * v + 1.0 / kA * V;
+    return MatBuilder.fill(Nat.N3(), Nat.N1(), p, v, a);
+  }
+
+  private static Matrix<N1, N1> motorControlInput(double t) {
+    return MatBuilder.fill(
+        Nat.N1(),
+        Nat.N1(),
+        Math.clamp(
+            8 * Math.sin(Math.PI * Math.sqrt(2.0) * t)
+                + 6 * Math.sin(Math.PI * Math.sqrt(3.0) * t)
+                + 4 * Math.sin(Math.PI * Math.sqrt(5.0) * t),
+            -12.0,
+            12.0));
+  }
+
+  @Test
+  void testMotorConvergence() {
+    final double dt = 0.01;
+    final int steps = 500;
+    final double true_kV = 3;
+    final double true_kA = 0.2;
+
+    final double pos_stddev = 0.02;
+    final double vel_stddev = 0.1;
+    final double accel_stddev = 0.1;
+
+    var states =
+        new ArrayList<>(
+            Collections.nCopies(
+                steps + 1, MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, 0.0, 0.0)));
+    var inputs =
+        new ArrayList<>(Collections.nCopies(steps, MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0)));
+    var measurements =
+        new ArrayList<>(
+            Collections.nCopies(steps + 1, MatBuilder.fill(Nat.N3(), Nat.N1(), 0.0, 0.0, 0.0)));
+    states.set(0, MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, true_kV, true_kA));
+
+    var A =
+        MatBuilder.fill(
+            Nat.N4(),
+            Nat.N4(),
+            0.0,
+            1.0,
+            0.0,
+            0.0,
+            0.0,
+            -true_kV / true_kA,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0);
+    var B = MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 1.0 / true_kA, 0.0, 0.0);
+
+    var discABPair = Discretization.discretizeAB(A, B, dt);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    for (int i = 0; i < steps; ++i) {
+      inputs.set(i, motorControlInput(i * dt));
+      states.set(i + 1, discA.times(states.get(i)).plus(discB.times(inputs.get(i))));
+      measurements.set(
+          i,
+          motorMeasurementModel(states.get(i + 1), inputs.get(i))
+              .plus(
+                  StateSpaceUtil.makeWhiteNoiseVector(
+                      VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev))));
+    }
+
+    var P0 =
+        MatBuilder.fill(
+            Nat.N4(), Nat.N4(), 0.001, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 10, 0.0, 0.0,
+            0.0, 0.0, 10);
+
+    var observer =
+        new MerweUKF<N4, N1, N3>(
+            Nat.N4(),
+            Nat.N3(),
+            MerweUKFTest::motorDynamics,
+            MerweUKFTest::motorMeasurementModel,
+            VecBuilder.fill(0.1, 1.0, 1e-10, 1e-10),
+            VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev),
+            dt);
+
+    observer.setXhat(MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, 2.0, 2.0));
+    observer.setP(P0);
+
+    for (int i = 0; i < steps; ++i) {
+      observer.predict(inputs.get(i), dt);
+      observer.correct(inputs.get(i), measurements.get(i));
+    }
+
+    assertEquals(true_kV, observer.getXhat(2), true_kV * 0.5);
+    assertEquals(true_kA, observer.getXhat(3), true_kA * 0.5);
+  }
+}

wpimath/src/test/java/org/wpilib/math/estimator/S3SigmaPointsTest.java

@@ -0,0 +1,86 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.numbers.N2;
+import org.junit.jupiter.api.Test;
+
+class S3SigmaPointsTest {
+  @Test
+  void testSimplex() {
+    double alpha = 1e-3;
+    double beta = 2;
+    Nat<N2> N = Nat.N2();
+
+    var sigmaPoints = new S3SigmaPoints<>(N, alpha, beta);
+    var points = sigmaPoints.squareRootSigmaPoints(new Vector<>(N), Matrix.eye(N));
+
+    var v1 = new Vector<>(points.extractColumnVector(1));
+    var v2 = new Vector<>(points.extractColumnVector(2));
+    var v3 = new Vector<>(points.extractColumnVector(3));
+
+    assertAll(
+        () -> assertEquals(alpha * Math.sqrt(N.getNum()), v1.norm(), 1e-15),
+        () -> assertEquals(alpha * Math.sqrt(N.getNum()), v2.norm(), 1e-15),
+        () -> assertEquals(alpha * Math.sqrt(N.getNum()), v3.norm(), 1e-15),
+        () -> assertEquals(v1.minus(v2).norm(), v1.minus(v3).norm(), 1e-15),
+        () -> assertEquals(v1.minus(v2).norm(), v2.minus(v3).norm(), 1e-15));
+  }
+
+  @Test
+  void testZeroMeanPoints() {
+    var sigmaPoints = new S3SigmaPoints<>(Nat.N2());
+    var points =
+        sigmaPoints.squareRootSigmaPoints(
+            VecBuilder.fill(0, 0), MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1));
+
+    assertTrue(
+        points.isEqual(
+            MatBuilder.fill(
+                Nat.N2(),
+                Nat.N4(),
+                0.0,
+                -0.00122474,
+                0.00122474,
+                0.0,
+                0.0,
+                -0.00070711,
+                -0.00070711,
+                0.00141421),
+            1E-6));
+  }
+
+  @Test
+  void testNonzeroMeanPoints() {
+    var sigmaPoints = new S3SigmaPoints<>(Nat.N2());
+    var points =
+        sigmaPoints.squareRootSigmaPoints(
+            VecBuilder.fill(1, 2), MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, Math.sqrt(10)));
+
+    assertTrue(
+        points.isEqual(
+            MatBuilder.fill(
+                Nat.N2(),
+                Nat.N4(),
+                1.0,
+                0.99877526,
+                1.00122474,
+                1.0,
+                2.0,
+                1.99776393,
+                1.99776393,
+                2.00447214),
+            1E-6));
+  }
+}

wpimath/src/test/java/org/wpilib/math/estimator/S3UKFTest.java

@@ -0,0 +1,392 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.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.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N4;
+import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.system.Discretization;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.NumericalJacobian;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class S3UKFTest {
+  private static Matrix<N5, N1> driveDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+    var motors = DCMotor.getCIM(2);
+
+    // var gLow = 15.32;    // Low gear ratio
+    var gHigh = 7.08; // High gear ratio
+    var rb = 0.8382 / 2.0; // Robot radius
+    var r = 0.0746125; // Wheel radius
+    var m = 63.503; // Robot mass
+    var J = 5.6; // Robot moment of inertia
+
+    var C1 = -Math.pow(gHigh, 2) * motors.Kt / (motors.Kv * motors.R * r * r);
+    var C2 = gHigh * motors.Kt / (motors.R * r);
+    var k1 = 1.0 / m + Math.pow(rb, 2) / J;
+    var k2 = 1.0 / m - Math.pow(rb, 2) / J;
+
+    var vl = x.get(3, 0);
+    var vr = x.get(4, 0);
+    var Vl = u.get(0, 0);
+    var Vr = u.get(1, 0);
+
+    var v = 0.5 * (vl + vr);
+    return VecBuilder.fill(
+        v * Math.cos(x.get(2, 0)),
+        v * Math.sin(x.get(2, 0)),
+        (vr - vl) / (2.0 * rb),
+        k1 * (C1 * vl + C2 * Vl) + k2 * (C1 * vr + C2 * Vr),
+        k2 * (C1 * vl + C2 * Vl) + k1 * (C1 * vr + C2 * Vr));
+  }
+
+  @SuppressWarnings("PMD.UnusedFormalParameter")
+  private static Matrix<N3, N1> driveLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+    return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0));
+  }
+
+  @SuppressWarnings("PMD.UnusedFormalParameter")
+  private static Matrix<N5, N1> driveGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+    return x.copy();
+  }
+
+  @Test
+  void testDriveInit() {
+    var dt = 0.005;
+    assertDoesNotThrow(
+        () -> {
+          S3UKF<N5, N2, N3> observer =
+              new S3UKF<>(
+                  Nat.N5(),
+                  Nat.N3(),
+                  S3UKFTest::driveDynamics,
+                  S3UKFTest::driveLocalMeasurementModel,
+                  VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
+                  VecBuilder.fill(0.0001, 0.01, 0.01),
+                  AngleStatistics.angleMean(2),
+                  AngleStatistics.angleMean(0),
+                  AngleStatistics.angleResidual(2),
+                  AngleStatistics.angleResidual(0),
+                  AngleStatistics.angleAdd(2),
+                  dt);
+
+          var u = VecBuilder.fill(12.0, 12.0);
+          observer.predict(u, dt);
+
+          var localY = driveLocalMeasurementModel(observer.getXhat(), u);
+          observer.correct(u, localY);
+
+          var globalY = driveGlobalMeasurementModel(observer.getXhat(), u);
+          var R =
+              StateSpaceUtil.makeCovarianceMatrix(
+                  Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.01, 0.01));
+          observer.correct(
+              Nat.N5(),
+              u,
+              globalY,
+              S3UKFTest::driveGlobalMeasurementModel,
+              R,
+              AngleStatistics.angleMean(2),
+              AngleStatistics.angleResidual(2),
+              AngleStatistics.angleResidual(2),
+              AngleStatistics.angleAdd(2));
+        });
+  }
+
+  @Test
+  void testDriveConvergence() {
+    final double dt = 0.005;
+    final double rb = 0.8382 / 2.0; // Robot radius
+
+    S3UKF<N5, N2, N3> observer =
+        new S3UKF<>(
+            Nat.N5(),
+            Nat.N3(),
+            S3UKFTest::driveDynamics,
+            S3UKFTest::driveLocalMeasurementModel,
+            VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
+            VecBuilder.fill(0.0001, 0.5, 0.5),
+            AngleStatistics.angleMean(2),
+            AngleStatistics.angleMean(0),
+            AngleStatistics.angleResidual(2),
+            AngleStatistics.angleResidual(0),
+            AngleStatistics.angleAdd(2),
+            dt);
+
+    List<Pose2d> waypoints =
+        List.of(
+            new Pose2d(2.75, 22.521, Rotation2d.kZero),
+            new Pose2d(24.73, 19.68, Rotation2d.fromRadians(5.846)));
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));
+
+    Matrix<N5, N1> r = new Matrix<>(Nat.N5(), Nat.N1());
+    Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
+
+    var B =
+        NumericalJacobian.numericalJacobianU(
+            Nat.N5(),
+            Nat.N2(),
+            S3UKFTest::driveDynamics,
+            new Matrix<>(Nat.N5(), Nat.N1()),
+            new Matrix<>(Nat.N2(), Nat.N1()));
+
+    observer.setXhat(
+        VecBuilder.fill(
+            trajectory.getInitialPose().getTranslation().getX(),
+            trajectory.getInitialPose().getTranslation().getY(),
+            trajectory.getInitialPose().getRotation().getRadians(),
+            0.0,
+            0.0));
+
+    var trueXhat = observer.getXhat();
+
+    double totalTime = trajectory.getTotalTime();
+    for (int i = 0; i < (totalTime / dt); ++i) {
+      var ref = trajectory.sample(dt * i);
+      double vl = ref.velocity * (1 - (ref.curvature * rb));
+      double vr = ref.velocity * (1 + (ref.curvature * rb));
+
+      var nextR =
+          VecBuilder.fill(
+              ref.pose.getTranslation().getX(),
+              ref.pose.getTranslation().getY(),
+              ref.pose.getRotation().getRadians(),
+              vl,
+              vr);
+
+      Matrix<N3, N1> localY =
+          driveLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
+      var noiseStdDev = VecBuilder.fill(0.0001, 0.5, 0.5);
+
+      observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev)));
+
+      var rdot = nextR.minus(r).div(dt);
+      u = new Matrix<>(B.solve(rdot.minus(driveDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
+
+      observer.predict(u, dt);
+
+      r = nextR;
+      trueXhat = NumericalIntegration.rk4(S3UKFTest::driveDynamics, trueXhat, u, dt);
+    }
+
+    var localY = driveLocalMeasurementModel(trueXhat, u);
+    observer.correct(u, localY);
+
+    var globalY = driveGlobalMeasurementModel(trueXhat, u);
+    var R =
+        StateSpaceUtil.makeCovarianceMatrix(
+            Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
+    observer.correct(
+        Nat.N5(),
+        u,
+        globalY,
+        S3UKFTest::driveGlobalMeasurementModel,
+        R,
+        AngleStatistics.angleMean(2),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleAdd(2));
+
+    final var finalPosition = trajectory.sample(trajectory.getTotalTime());
+
+    assertEquals(finalPosition.pose.getTranslation().getX(), observer.getXhat(0), 0.055);
+    assertEquals(finalPosition.pose.getTranslation().getY(), observer.getXhat(1), 0.15);
+    assertEquals(finalPosition.pose.getRotation().getRadians(), observer.getXhat(2), 0.00015);
+    assertEquals(0.0, observer.getXhat(3), 0.1);
+    assertEquals(0.0, observer.getXhat(4), 0.1);
+  }
+
+  @Test
+  void testLinearUKF() {
+    var dt = 0.020;
+    var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
+    var observer =
+        new S3UKF<>(
+            Nat.N1(),
+            Nat.N1(),
+            (x, u) -> plant.getA().times(x).plus(plant.getB().times(u)),
+            plant::calculateY,
+            VecBuilder.fill(0.05),
+            VecBuilder.fill(1.0),
+            dt);
+
+    var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dt);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    Matrix<N1, N1> ref = VecBuilder.fill(100);
+    Matrix<N1, N1> u = VecBuilder.fill(0);
+
+    for (int i = 0; i < (2.0 / dt); ++i) {
+      observer.predict(u, dt);
+
+      u = discB.solve(ref.minus(discA.times(ref)));
+    }
+
+    assertEquals(ref.get(0, 0), observer.getXhat(0), 5);
+  }
+
+  @Test
+  void testRoundTripP() {
+    var dt = 0.005;
+
+    var observer =
+        new S3UKF<>(
+            Nat.N2(),
+            Nat.N2(),
+            (x, u) -> x,
+            (x, u) -> x,
+            VecBuilder.fill(0.0, 0.0),
+            VecBuilder.fill(0.0, 0.0),
+            dt);
+
+    var P = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 1.0, 2.0);
+    observer.setP(P);
+
+    assertTrue(observer.getP().isEqual(P, 1e-9));
+  }
+
+  // Second system, single motor feedforward estimator
+  private static Matrix<N4, N1> motorDynamics(Matrix<N4, N1> x, Matrix<N1, N1> u) {
+    double v = x.get(1, 0);
+    double kV = x.get(2, 0);
+    double kA = x.get(3, 0);
+
+    double V = u.get(0, 0);
+
+    double a = -kV / kA * v + 1.0 / kA * V;
+    return MatBuilder.fill(Nat.N4(), Nat.N1(), v, a, 0, 0);
+  }
+
+  private static Matrix<N3, N1> motorMeasurementModel(Matrix<N4, N1> x, Matrix<N1, N1> u) {
+    double p = x.get(0, 0);
+    double v = x.get(1, 0);
+    double kV = x.get(2, 0);
+    double kA = x.get(3, 0);
+    double V = u.get(0, 0);
+
+    double a = -kV / kA * v + 1.0 / kA * V;
+    return MatBuilder.fill(Nat.N3(), Nat.N1(), p, v, a);
+  }
+
+  private static Matrix<N1, N1> motorControlInput(double t) {
+    return MatBuilder.fill(
+        Nat.N1(),
+        Nat.N1(),
+        Math.clamp(
+            8 * Math.sin(Math.PI * Math.sqrt(2.0) * t)
+                + 6 * Math.sin(Math.PI * Math.sqrt(3.0) * t)
+                + 4 * Math.sin(Math.PI * Math.sqrt(5.0) * t),
+            -12.0,
+            12.0));
+  }
+
+  @Test
+  void testMotorConvergence() {
+    final double dt = 0.01;
+    final int steps = 500;
+    final double true_kV = 3;
+    final double true_kA = 0.2;
+
+    final double pos_stddev = 0.02;
+    final double vel_stddev = 0.1;
+    final double accel_stddev = 0.1;
+
+    var states =
+        new ArrayList<>(
+            Collections.nCopies(
+                steps + 1, MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, 0.0, 0.0)));
+    var inputs =
+        new ArrayList<>(Collections.nCopies(steps, MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0)));
+    var measurements =
+        new ArrayList<>(
+            Collections.nCopies(steps + 1, MatBuilder.fill(Nat.N3(), Nat.N1(), 0.0, 0.0, 0.0)));
+    states.set(0, MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, true_kV, true_kA));
+
+    var A =
+        MatBuilder.fill(
+            Nat.N4(),
+            Nat.N4(),
+            0.0,
+            1.0,
+            0.0,
+            0.0,
+            0.0,
+            -true_kV / true_kA,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0);
+    var B = MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 1.0 / true_kA, 0.0, 0.0);
+
+    var discABPair = Discretization.discretizeAB(A, B, dt);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    for (int i = 0; i < steps; ++i) {
+      inputs.set(i, motorControlInput(i * dt));
+      states.set(i + 1, discA.times(states.get(i)).plus(discB.times(inputs.get(i))));
+      measurements.set(
+          i,
+          motorMeasurementModel(states.get(i + 1), inputs.get(i))
+              .plus(
+                  StateSpaceUtil.makeWhiteNoiseVector(
+                      VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev))));
+    }
+
+    var P0 =
+        MatBuilder.fill(
+            Nat.N4(), Nat.N4(), 0.001, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 10, 0.0, 0.0,
+            0.0, 0.0, 10);
+
+    var observer =
+        new S3UKF<N4, N1, N3>(
+            Nat.N4(),
+            Nat.N3(),
+            S3UKFTest::motorDynamics,
+            S3UKFTest::motorMeasurementModel,
+            VecBuilder.fill(0.1, 1.0, 1e-10, 1e-10),
+            VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev),
+            dt);
+
+    observer.setXhat(MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, 2.0, 2.0));
+    observer.setP(P0);
+
+    for (int i = 0; i < steps; ++i) {
+      observer.predict(inputs.get(i), dt);
+      observer.correct(inputs.get(i), measurements.get(i));
+    }
+
+    assertEquals(true_kV, observer.getXhat(2), true_kV * 0.5);
+    assertEquals(true_kA, observer.getXhat(3), true_kA * 0.5);
+  }
+}

wpimath/src/test/java/org/wpilib/math/estimator/SwerveDrivePoseEstimator3dTest.java

@@ -0,0 +1,559 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+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.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.List;
+import java.util.Optional;
+import java.util.Random;
+import java.util.TreeMap;
+import java.util.function.Function;
+import org.junit.jupiter.api.Test;
+
+class SwerveDrivePoseEstimator3dTest {
+  private static final double kEpsilon = 1e-9;
+
+  @Test
+  void testAccuracyFacingTrajectory() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1),
+            new Translation2d(-1, 1));
+
+    var fl = new SwerveModulePosition();
+    var fr = new SwerveModulePosition();
+    var bl = new SwerveModulePosition();
+    var br = new SwerveModulePosition();
+
+    var estimator =
+        new SwerveDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            new SwerveModulePosition[] {fl, fr, bl, br},
+            Pose3d.kZero,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1),
+            VecBuilder.fill(0.5, 0.5, 0.5, 0.5));
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(2, 2));
+
+    testFollowTrajectory(
+        kinematics,
+        estimator,
+        trajectory,
+        state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
+        state -> state.pose,
+        trajectory.getInitialPose(),
+        new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+        0.02,
+        0.1,
+        0.25,
+        true);
+  }
+
+  @Test
+  void testBadInitialPose() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1),
+            new Translation2d(-1, 1));
+
+    var fl = new SwerveModulePosition();
+    var fr = new SwerveModulePosition();
+    var bl = new SwerveModulePosition();
+    var br = new SwerveModulePosition();
+
+    var estimator =
+        new SwerveDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            new SwerveModulePosition[] {fl, fr, bl, br},
+            Pose3d.kZero,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9, 0.9));
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(2, 2));
+
+    for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) {
+      for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) {
+        var pose_offset = Rotation2d.fromDegrees(offset_direction_degs);
+        var heading_offset = Rotation2d.fromDegrees(offset_heading_degs);
+
+        var initial_pose =
+            trajectory
+                .getInitialPose()
+                .plus(
+                    new Transform2d(
+                        new Translation2d(pose_offset.getCos(), pose_offset.getSin()),
+                        heading_offset));
+
+        testFollowTrajectory(
+            kinematics,
+            estimator,
+            trajectory,
+            state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
+            state -> state.pose,
+            initial_pose,
+            new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+            0.02,
+            0.1,
+            1.0,
+            false);
+      }
+    }
+  }
+
+  void testFollowTrajectory(
+      final SwerveDriveKinematics kinematics,
+      final SwerveDrivePoseEstimator3d estimator,
+      final Trajectory trajectory,
+      final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
+      final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
+      final Pose2d startingPose,
+      final Pose2d endingPose,
+      final double dt,
+      final double visionUpdateRate,
+      final double visionUpdateDelay,
+      final boolean checkError) {
+    SwerveModulePosition[] positions = {
+      new SwerveModulePosition(),
+      new SwerveModulePosition(),
+      new SwerveModulePosition(),
+      new SwerveModulePosition()
+    };
+
+    estimator.resetPosition(Rotation3d.kZero, positions, new Pose3d(startingPose));
+
+    var rand = new Random(3538);
+
+    double t = 0.0;
+
+    final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    while (t <= trajectory.getTotalTime()) {
+      var groundTruthState = trajectory.sample(t);
+
+      // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
+      // last vision measurement
+      if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) {
+        Pose2d newVisionPose =
+            visionMeasurementGenerator
+                .apply(groundTruthState)
+                .plus(
+                    new Transform2d(
+                        new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1),
+                        new Rotation2d(rand.nextGaussian() * 0.05)));
+
+        visionUpdateQueue.put(t, newVisionPose);
+      }
+
+      // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds
+      // since it was measured
+      if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) {
+        var visionEntry = visionUpdateQueue.pollFirstEntry();
+        estimator.addVisionMeasurement(new Pose3d(visionEntry.getValue()), visionEntry.getKey());
+      }
+
+      var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
+
+      var moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds);
+
+      for (int i = 0; i < moduleStates.length; i++) {
+        positions[i].distance += moduleStates[i].speed * (1 - rand.nextGaussian() * 0.05) * dt;
+        positions[i].angle =
+            moduleStates[i].angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
+      }
+
+      var xHat =
+          estimator.updateWithTime(
+              t,
+              new Rotation3d(
+                  groundTruthState
+                      .pose
+                      .getRotation()
+                      .plus(new Rotation2d(rand.nextGaussian() * 0.05))
+                      .minus(trajectory.getInitialPose().getRotation())),
+              positions);
+
+      double error =
+          groundTruthState
+              .pose
+              .getTranslation()
+              .getDistance(xHat.getTranslation().toTranslation2d());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+        endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X");
+    assertEquals(
+        endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y");
+    assertEquals(
+        endingPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().toRotation2d().getRadians(),
+        0.15,
+        "Incorrect Final Theta");
+
+    if (checkError) {
+      assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error");
+      assertEquals(0.0, maxError, 0.2, "Incorrect max error");
+    }
+  }
+
+  @Test
+  void testSimultaneousVisionMeasurements() {
+    // This tests for multiple vision measurements applied at the same time. The expected behavior
+    // is that all measurements affect the estimated pose. The alternative result is that only one
+    // vision measurement affects the outcome. If that were the case, after 1000 measurements, the
+    // estimated pose would converge to that measurement.
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1),
+            new Translation2d(-1, 1));
+
+    var fl = new SwerveModulePosition();
+    var fr = new SwerveModulePosition();
+    var bl = new SwerveModulePosition();
+    var br = new SwerveModulePosition();
+
+    var estimator =
+        new SwerveDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            new SwerveModulePosition[] {fl, fr, bl, br},
+            new Pose3d(1, 2, 0, new Rotation3d(0, 0, -Math.PI / 2)),
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9, 0.9));
+
+    estimator.updateWithTime(0, Rotation3d.kZero, new SwerveModulePosition[] {fl, fr, bl, br});
+
+    var visionMeasurements =
+        new Pose2d[] {
+          new Pose2d(0, 0, Rotation2d.kZero),
+          new Pose2d(3, 1, Rotation2d.kCCW_Pi_2),
+          new Pose2d(2, 4, Rotation2d.kPi),
+        };
+
+    for (int i = 0; i < 1000; i++) {
+      for (var measurement : visionMeasurements) {
+        estimator.addVisionMeasurement(new Pose3d(measurement), 0);
+      }
+    }
+
+    for (var measurement : visionMeasurements) {
+      var errorLog =
+          "Estimator converged to one vision measurement: "
+              + estimator.getEstimatedPosition().toString()
+              + " -> "
+              + measurement;
+
+      var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
+      var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
+      var dtheta =
+          Math.abs(
+              measurement.getRotation().getDegrees()
+                  - estimator.getEstimatedPosition().getRotation().toRotation2d().getDegrees());
+
+      assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog);
+    }
+  }
+
+  @Test
+  void testDiscardsOldVisionMeasurements() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new SwerveDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            new SwerveModulePosition[] {
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition()
+            },
+            Pose3d.kZero,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9, 0.9));
+
+    double time = 0;
+
+    // Add enough measurements to fill up the buffer
+    for (; time < 4; time += 0.02) {
+      estimator.updateWithTime(
+          time,
+          Rotation3d.kZero,
+          new SwerveModulePosition[] {
+            new SwerveModulePosition(),
+            new SwerveModulePosition(),
+            new SwerveModulePosition(),
+            new SwerveModulePosition()
+          });
+    }
+
+    var odometryPose = estimator.getEstimatedPosition();
+
+    // Apply a vision measurement made 3 seconds ago
+    // This test passes if this does not cause a ConcurrentModificationException.
+    estimator.addVisionMeasurement(
+        new Pose3d(10, 10, 0, new Rotation3d(0, 0, 0.1)), 1, VecBuilder.fill(0.1, 0.1, 0.1, 0.1));
+
+    assertAll(
+        () ->
+            assertEquals(
+                odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X"),
+        () ->
+            assertEquals(
+                odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y"),
+        () ->
+            assertEquals(
+                odometryPose.getZ(), estimator.getEstimatedPosition().getZ(), "Incorrect Final Y"),
+        () ->
+            assertEquals(
+                odometryPose.getRotation().getX(),
+                estimator.getEstimatedPosition().getRotation().getX(),
+                "Incorrect Final Roll"),
+        () ->
+            assertEquals(
+                odometryPose.getRotation().getY(),
+                estimator.getEstimatedPosition().getRotation().getY(),
+                "Incorrect Final Pitch"),
+        () ->
+            assertEquals(
+                odometryPose.getRotation().getZ(),
+                estimator.getEstimatedPosition().getRotation().getZ(),
+                "Incorrect Final Yaw"));
+  }
+
+  @Test
+  void testSampleAt() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new SwerveDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            new SwerveModulePosition[] {
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition()
+            },
+            Pose3d.kZero,
+            VecBuilder.fill(1, 1, 1, 1),
+            VecBuilder.fill(1, 1, 1, 1));
+
+    // Returns empty when null
+    assertEquals(Optional.empty(), estimator.sampleAt(1));
+
+    // Add odometry measurements, but don't fill up the buffer
+    // Add a tiny tolerance for the upper bound because of floating point rounding error
+    for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
+      var wheelPositions =
+          new SwerveModulePosition[] {
+            new SwerveModulePosition(time, Rotation2d.kZero),
+            new SwerveModulePosition(time, Rotation2d.kZero),
+            new SwerveModulePosition(time, Rotation2d.kZero),
+            new SwerveModulePosition(time, Rotation2d.kZero)
+          };
+      estimator.updateWithTime(time, Rotation3d.kZero, wheelPositions);
+    }
+
+    // Sample at an added time
+    assertEquals(Optional.of(new Pose3d(1.02, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.02));
+    // Sample between updates (test interpolation)
+    assertEquals(Optional.of(new Pose3d(1.01, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.01));
+    // Sampling before the oldest value returns the oldest value
+    assertEquals(Optional.of(new Pose3d(1, 0, 0, Rotation3d.kZero)), estimator.sampleAt(0.5));
+    // Sampling after the newest value returns the newest value
+    assertEquals(Optional.of(new Pose3d(2, 0, 0, Rotation3d.kZero)), estimator.sampleAt(2.5));
+
+    // Add a vision measurement after the odometry measurements (while keeping all of the old
+    // odometry measurements)
+    estimator.addVisionMeasurement(new Pose3d(2, 0, 0, new Rotation3d(0, 0, 1)), 2.2);
+
+    // Make sure nothing changed (except the newest value)
+    assertEquals(Optional.of(new Pose3d(1.02, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose3d(1.01, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose3d(1, 0, 0, Rotation3d.kZero)), estimator.sampleAt(0.5));
+
+    // Add a vision measurement before the odometry measurements that's still in the buffer
+    estimator.addVisionMeasurement(new Pose3d(1, 0.2, 0, Rotation3d.kZero), 0.9);
+
+    // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
+    assertEquals(Optional.of(new Pose3d(1.02, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose3d(1.01, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose3d(1, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(0.5));
+    assertEquals(Optional.of(new Pose3d(2, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(2.5));
+  }
+
+  @Test
+  void testReset() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new SwerveDrivePoseEstimator3d(
+            kinematics,
+            Rotation3d.kZero,
+            new SwerveModulePosition[] {
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition()
+            },
+            Pose3d.kZero,
+            VecBuilder.fill(1, 1, 1, 1),
+            VecBuilder.fill(1, 1, 1, 1));
+
+    // Test reset position
+    {
+      var modulePosition = new SwerveModulePosition(1, Rotation2d.kZero);
+      estimator.resetPosition(
+          Rotation3d.kZero,
+          new SwerveModulePosition[] {
+            modulePosition, modulePosition, modulePosition, modulePosition
+          },
+          new Pose3d(1, 0, 0, Rotation3d.kZero));
+    }
+
+    assertAll(
+        () -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+
+    // Test orientation and wheel positions
+    {
+      var modulePosition = new SwerveModulePosition(2, Rotation2d.kZero);
+      estimator.update(
+          Rotation3d.kZero,
+          new SwerveModulePosition[] {
+            modulePosition, modulePosition, modulePosition, modulePosition
+          });
+    }
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+
+    // Test reset rotation
+    estimator.resetRotation(new Rotation3d(Rotation2d.kCCW_Pi_2));
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+
+    // Test orientation
+    {
+      var modulePosition = new SwerveModulePosition(3, Rotation2d.kZero);
+      estimator.update(
+          Rotation3d.kZero,
+          new SwerveModulePosition[] {
+            modulePosition, modulePosition, modulePosition, modulePosition
+          });
+    }
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+
+    // Test reset translation
+    estimator.resetTranslation(new Translation3d(-1, -1, -1));
+
+    assertAll(
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+
+    // Test reset pose
+    estimator.resetPose(Pose3d.kZero);
+
+    assertAll(
+        () -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+  }
+}

wpimath/src/test/java/org/wpilib/math/estimator/SwerveDrivePoseEstimatorTest.java

@@ -0,0 +1,525 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.List;
+import java.util.Optional;
+import java.util.Random;
+import java.util.TreeMap;
+import java.util.function.Function;
+import org.junit.jupiter.api.Test;
+
+class SwerveDrivePoseEstimatorTest {
+  private static final double kEpsilon = 1e-9;
+
+  @Test
+  void testAccuracyFacingTrajectory() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1),
+            new Translation2d(-1, 1));
+
+    var fl = new SwerveModulePosition();
+    var fr = new SwerveModulePosition();
+    var bl = new SwerveModulePosition();
+    var br = new SwerveModulePosition();
+
+    var estimator =
+        new SwerveDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            new SwerveModulePosition[] {fl, fr, bl, br},
+            Pose2d.kZero,
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.5, 0.5, 0.5));
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(2, 2));
+
+    testFollowTrajectory(
+        kinematics,
+        estimator,
+        trajectory,
+        state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
+        state -> state.pose,
+        trajectory.getInitialPose(),
+        new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+        0.02,
+        0.1,
+        0.25,
+        true);
+  }
+
+  @Test
+  void testBadInitialPose() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1),
+            new Translation2d(-1, 1));
+
+    var fl = new SwerveModulePosition();
+    var fr = new SwerveModulePosition();
+    var bl = new SwerveModulePosition();
+    var br = new SwerveModulePosition();
+
+    var estimator =
+        new SwerveDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            new SwerveModulePosition[] {fl, fr, bl, br},
+            new Pose2d(-1, -1, Rotation2d.fromRadians(-1)),
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9));
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(2, 2));
+
+    for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) {
+      for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) {
+        var pose_offset = Rotation2d.fromDegrees(offset_direction_degs);
+        var heading_offset = Rotation2d.fromDegrees(offset_heading_degs);
+
+        var initial_pose =
+            trajectory
+                .getInitialPose()
+                .plus(
+                    new Transform2d(
+                        new Translation2d(pose_offset.getCos(), pose_offset.getSin()),
+                        heading_offset));
+
+        testFollowTrajectory(
+            kinematics,
+            estimator,
+            trajectory,
+            state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
+            state -> state.pose,
+            initial_pose,
+            new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+            0.02,
+            0.1,
+            1.0,
+            false);
+      }
+    }
+  }
+
+  void testFollowTrajectory(
+      final SwerveDriveKinematics kinematics,
+      final SwerveDrivePoseEstimator estimator,
+      final Trajectory trajectory,
+      final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
+      final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
+      final Pose2d startingPose,
+      final Pose2d endingPose,
+      final double dt,
+      final double visionUpdateRate,
+      final double visionUpdateDelay,
+      final boolean checkError) {
+    SwerveModulePosition[] positions = {
+      new SwerveModulePosition(),
+      new SwerveModulePosition(),
+      new SwerveModulePosition(),
+      new SwerveModulePosition()
+    };
+
+    estimator.resetPosition(Rotation2d.kZero, positions, startingPose);
+
+    var rand = new Random(3538);
+
+    double t = 0.0;
+
+    final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    while (t <= trajectory.getTotalTime()) {
+      var groundTruthState = trajectory.sample(t);
+
+      // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
+      // last vision measurement
+      if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) {
+        Pose2d newVisionPose =
+            visionMeasurementGenerator
+                .apply(groundTruthState)
+                .plus(
+                    new Transform2d(
+                        new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1),
+                        new Rotation2d(rand.nextGaussian() * 0.05)));
+
+        visionUpdateQueue.put(t, newVisionPose);
+      }
+
+      // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds
+      // since it was measured
+      if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) {
+        var visionEntry = visionUpdateQueue.pollFirstEntry();
+        estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey());
+      }
+
+      var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
+
+      var moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds);
+
+      for (int i = 0; i < moduleStates.length; i++) {
+        positions[i].distance += moduleStates[i].speed * (1 - rand.nextGaussian() * 0.05) * dt;
+        positions[i].angle =
+            moduleStates[i].angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
+      }
+
+      var xHat =
+          estimator.updateWithTime(
+              t,
+              groundTruthState
+                  .pose
+                  .getRotation()
+                  .plus(new Rotation2d(rand.nextGaussian() * 0.05))
+                  .minus(trajectory.getInitialPose().getRotation()),
+              positions);
+
+      double error = groundTruthState.pose.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+        endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X");
+    assertEquals(
+        endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y");
+    assertEquals(
+        endingPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().getRadians(),
+        0.15,
+        "Incorrect Final Theta");
+
+    if (checkError) {
+      assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error");
+      assertEquals(0.0, maxError, 0.2, "Incorrect max error");
+    }
+  }
+
+  @Test
+  void testSimultaneousVisionMeasurements() {
+    // This tests for multiple vision measurements applied at the same time. The expected behavior
+    // is that all measurements affect the estimated pose. The alternative result is that only one
+    // vision measurement affects the outcome. If that were the case, after 1000 measurements, the
+    // estimated pose would converge to that measurement.
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1),
+            new Translation2d(-1, 1));
+
+    var fl = new SwerveModulePosition();
+    var fr = new SwerveModulePosition();
+    var bl = new SwerveModulePosition();
+    var br = new SwerveModulePosition();
+
+    var estimator =
+        new SwerveDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            new SwerveModulePosition[] {fl, fr, bl, br},
+            new Pose2d(1, 2, Rotation2d.kCW_Pi_2),
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9));
+
+    estimator.updateWithTime(0, Rotation2d.kZero, new SwerveModulePosition[] {fl, fr, bl, br});
+
+    var visionMeasurements =
+        new Pose2d[] {
+          new Pose2d(0, 0, Rotation2d.kZero),
+          new Pose2d(3, 1, Rotation2d.kCCW_Pi_2),
+          new Pose2d(2, 4, Rotation2d.kPi),
+        };
+
+    for (int i = 0; i < 1000; i++) {
+      for (var measurement : visionMeasurements) {
+        estimator.addVisionMeasurement(measurement, 0);
+      }
+    }
+
+    for (var measurement : visionMeasurements) {
+      var errorLog =
+          "Estimator converged to one vision measurement: "
+              + estimator.getEstimatedPosition().toString()
+              + " -> "
+              + measurement;
+
+      var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
+      var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
+      var dtheta =
+          Math.abs(
+              measurement.getRotation().getDegrees()
+                  - estimator.getEstimatedPosition().getRotation().getDegrees());
+
+      assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog);
+    }
+  }
+
+  @Test
+  void testDiscardsOldVisionMeasurements() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new SwerveDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            new SwerveModulePosition[] {
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition()
+            },
+            Pose2d.kZero,
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9));
+
+    double time = 0;
+
+    // Add enough measurements to fill up the buffer
+    for (; time < 4; time += 0.02) {
+      estimator.updateWithTime(
+          time,
+          Rotation2d.kZero,
+          new SwerveModulePosition[] {
+            new SwerveModulePosition(),
+            new SwerveModulePosition(),
+            new SwerveModulePosition(),
+            new SwerveModulePosition()
+          });
+    }
+
+    var odometryPose = estimator.getEstimatedPosition();
+
+    // Apply a vision measurement made 3 seconds ago
+    // This test passes if this does not cause a ConcurrentModificationException.
+    estimator.addVisionMeasurement(
+        new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)),
+        1,
+        VecBuilder.fill(0.1, 0.1, 0.1));
+
+    assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X");
+    assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y");
+    assertEquals(
+        odometryPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().getRadians(),
+        "Incorrect Final Theta");
+  }
+
+  @Test
+  void testSampleAt() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new SwerveDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            new SwerveModulePosition[] {
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition()
+            },
+            Pose2d.kZero,
+            VecBuilder.fill(1, 1, 1),
+            VecBuilder.fill(1, 1, 1));
+
+    // Returns empty when null
+    assertEquals(Optional.empty(), estimator.sampleAt(1));
+
+    // Add odometry measurements, but don't fill up the buffer
+    // Add a tiny tolerance for the upper bound because of floating point rounding error
+    for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
+      var wheelPositions =
+          new SwerveModulePosition[] {
+            new SwerveModulePosition(time, Rotation2d.kZero),
+            new SwerveModulePosition(time, Rotation2d.kZero),
+            new SwerveModulePosition(time, Rotation2d.kZero),
+            new SwerveModulePosition(time, Rotation2d.kZero)
+          };
+      estimator.updateWithTime(time, Rotation2d.kZero, wheelPositions);
+    }
+
+    // Sample at an added time
+    assertEquals(Optional.of(new Pose2d(1.02, 0, Rotation2d.kZero)), estimator.sampleAt(1.02));
+    // Sample between updates (test interpolation)
+    assertEquals(Optional.of(new Pose2d(1.01, 0, Rotation2d.kZero)), estimator.sampleAt(1.01));
+    // Sampling before the oldest value returns the oldest value
+    assertEquals(Optional.of(new Pose2d(1, 0, Rotation2d.kZero)), estimator.sampleAt(0.5));
+    // Sampling after the newest value returns the newest value
+    assertEquals(Optional.of(new Pose2d(2, 0, Rotation2d.kZero)), estimator.sampleAt(2.5));
+
+    // Add a vision measurement after the odometry measurements (while keeping all of the old
+    // odometry measurements)
+    estimator.addVisionMeasurement(new Pose2d(2, 0, new Rotation2d(1)), 2.2);
+
+    // Make sure nothing changed (except the newest value)
+    assertEquals(Optional.of(new Pose2d(1.02, 0, Rotation2d.kZero)), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose2d(1.01, 0, Rotation2d.kZero)), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose2d(1, 0, Rotation2d.kZero)), estimator.sampleAt(0.5));
+
+    // Add a vision measurement before the odometry measurements that's still in the buffer
+    estimator.addVisionMeasurement(new Pose2d(1, 0.2, Rotation2d.kZero), 0.9);
+
+    // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
+    assertEquals(Optional.of(new Pose2d(1.02, 0.1, Rotation2d.kZero)), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose2d(1.01, 0.1, Rotation2d.kZero)), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose2d(1, 0.1, Rotation2d.kZero)), estimator.sampleAt(0.5));
+    assertEquals(Optional.of(new Pose2d(2, 0.1, Rotation2d.kZero)), estimator.sampleAt(2.5));
+  }
+
+  @Test
+  void testReset() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new SwerveDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            new SwerveModulePosition[] {
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition()
+            },
+            Pose2d.kZero,
+            VecBuilder.fill(1, 1, 1),
+            VecBuilder.fill(1, 1, 1));
+
+    // Test reset position
+    {
+      var modulePosition = new SwerveModulePosition(1, Rotation2d.kZero);
+      estimator.resetPosition(
+          Rotation2d.kZero,
+          new SwerveModulePosition[] {
+            modulePosition, modulePosition, modulePosition, modulePosition
+          },
+          new Pose2d(1, 0, Rotation2d.kZero));
+    }
+
+    assertAll(
+        () -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
+
+    // Test orientation and wheel positions
+    {
+      var modulePosition = new SwerveModulePosition(2, Rotation2d.kZero);
+      estimator.update(
+          Rotation2d.kZero,
+          new SwerveModulePosition[] {
+            modulePosition, modulePosition, modulePosition, modulePosition
+          });
+    }
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
+
+    // Test reset rotation
+    estimator.resetRotation(Rotation2d.kCCW_Pi_2);
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test orientation
+    {
+      var modulePosition = new SwerveModulePosition(3, Rotation2d.kZero);
+      estimator.update(
+          Rotation2d.kZero,
+          new SwerveModulePosition[] {
+            modulePosition, modulePosition, modulePosition, modulePosition
+          });
+    }
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test reset translation
+    estimator.resetTranslation(new Translation2d(-1, -1));
+
+    assertAll(
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test reset pose
+    estimator.resetPose(Pose2d.kZero);
+
+    assertAll(
+        () -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
+  }
+}

wpimath/src/test/java/org/wpilib/math/filter/DebouncerTest.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 static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertSame;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.util.WPIUtilJNI;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class DebouncerTest {
+  @BeforeEach
+  void setUp() {
+    WPIUtilJNI.enableMockTime();
+    WPIUtilJNI.setMockTime(0L);
+  }
+
+  @AfterEach
+  void tearDown() {
+    WPIUtilJNI.setMockTime(0L);
+    WPIUtilJNI.disableMockTime();
+  }
+
+  @Test
+  void debounceRisingTest() {
+    var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kRising);
+
+    debouncer.calculate(false);
+    assertFalse(debouncer.calculate(true));
+
+    WPIUtilJNI.setMockTime(1000000L);
+
+    assertTrue(debouncer.calculate(true));
+  }
+
+  @Test
+  void debounceFallingTest() {
+    var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kFalling);
+
+    debouncer.calculate(true);
+    assertTrue(debouncer.calculate(false));
+
+    WPIUtilJNI.setMockTime(1000000L);
+
+    assertFalse(debouncer.calculate(false));
+
+    WPIUtilJNI.setMockTime(0L);
+  }
+
+  @Test
+  void debounceBothTest() {
+    var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kBoth);
+
+    debouncer.calculate(false);
+    assertFalse(debouncer.calculate(true));
+
+    WPIUtilJNI.setMockTime(1000000L);
+
+    assertTrue(debouncer.calculate(true));
+    assertTrue(debouncer.calculate(false));
+
+    WPIUtilJNI.setMockTime(2000000L);
+
+    assertFalse(debouncer.calculate(false));
+  }
+
+  @Test
+  void debounceParamsTest() {
+    var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kBoth);
+
+    assertEquals(debouncer.getDebounceTime(), 0.02);
+    assertSame(debouncer.getDebounceType(), Debouncer.DebounceType.kBoth);
+
+    debouncer.setDebounceTime(0.1);
+
+    assertEquals(debouncer.getDebounceTime(), 0.1);
+
+    debouncer.setDebounceType(Debouncer.DebounceType.kFalling);
+
+    assertSame(debouncer.getDebounceType(), Debouncer.DebounceType.kFalling);
+    assertTrue(debouncer.calculate(false));
+  }
+}

wpimath/src/test/java/org/wpilib/math/filter/LinearFilterTest.java

@@ -0,0 +1,341 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+
+import java.util.Random;
+import java.util.function.DoubleFunction;
+import java.util.stream.Stream;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+class LinearFilterTest {
+  private static final double kFilterStep = 0.005;
+  private static final double kFilterTime = 2.0;
+  private static final double kSinglePoleIIRTimeConstant = 0.015915;
+  private static final double kHighPassTimeConstant = 0.006631;
+  private static final int kMovAvgTaps = 6;
+
+  private static final double kSinglePoleIIRExpectedOutput = -3.2172003;
+  private static final double kHighPassExpectedOutput = 10.074717;
+  private static final double kMovAvgExpectedOutput = -10.191644;
+
+  private static double getData(double t) {
+    return 100.0 * Math.sin(2.0 * Math.PI * t) + 20.0 * Math.cos(50.0 * Math.PI * t);
+  }
+
+  private static double getPulseData(double t) {
+    if (Math.abs(t - 1.0) < 0.001) {
+      return 1.0;
+    } else {
+      return 0.0;
+    }
+  }
+
+  @Test
+  void illegalTapNumberTest() {
+    assertThrows(IllegalArgumentException.class, () -> LinearFilter.movingAverage(0));
+  }
+
+  /** Test if the filter reduces the noise produced by a signal generator. */
+  @ParameterizedTest
+  @MethodSource("noiseFilterProvider")
+  void noiseReduceTest(final LinearFilter filter) {
+    double noiseGenError = 0.0;
+    double filterError = 0.0;
+
+    final Random gen = new Random();
+    final double kStdDev = 10.0;
+
+    for (double t = 0; t < kFilterTime; t += kFilterStep) {
+      final double theory = getData(t);
+      final double noise = gen.nextGaussian() * kStdDev;
+      filterError += Math.abs(filter.calculate(theory + noise) - theory);
+      noiseGenError += Math.abs(noise - theory);
+    }
+
+    assertTrue(
+        noiseGenError > filterError,
+        "Filter should have reduced noise accumulation from "
+            + noiseGenError
+            + " but failed. The filter error was "
+            + filterError);
+  }
+
+  static Stream<LinearFilter> noiseFilterProvider() {
+    return Stream.of(
+        LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
+        LinearFilter.movingAverage(kMovAvgTaps));
+  }
+
+  /** Test if the linear filters produce consistent output for a given data set. */
+  @ParameterizedTest
+  @MethodSource("outputFilterProvider")
+  void outputTest(
+      final LinearFilter filter, final DoubleFunction<Double> data, final double expectedOutput) {
+    double filterOutput = 0.0;
+    for (double t = 0.0; t < kFilterTime; t += kFilterStep) {
+      filterOutput = filter.calculate(data.apply(t));
+    }
+
+    assertEquals(expectedOutput, filterOutput, 5e-5, "Filter output was incorrect.");
+  }
+
+  static Stream<Arguments> outputFilterProvider() {
+    return Stream.of(
+        arguments(
+            LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
+            (DoubleFunction<Double>) LinearFilterTest::getData,
+            kSinglePoleIIRExpectedOutput),
+        arguments(
+            LinearFilter.highPass(kHighPassTimeConstant, kFilterStep),
+            (DoubleFunction<Double>) LinearFilterTest::getData,
+            kHighPassExpectedOutput),
+        arguments(
+            LinearFilter.movingAverage(kMovAvgTaps),
+            (DoubleFunction<Double>) LinearFilterTest::getData,
+            kMovAvgExpectedOutput),
+        arguments(
+            LinearFilter.movingAverage(kMovAvgTaps),
+            (DoubleFunction<Double>) LinearFilterTest::getPulseData,
+            0.0));
+  }
+
+  /** Test central finite difference. */
+  @Test
+  void centralFiniteDifferenceTest() {
+    double h = 0.005;
+
+    assertCentralResults(
+        1,
+        3,
+        // f(x) = x²
+        (double x) -> x * x,
+        // df/dx = 2x
+        (double x) -> 2.0 * x,
+        h,
+        -20.0,
+        20.0);
+
+    assertCentralResults(
+        1,
+        3,
+        // f(x) = sin(x)
+        Math::sin,
+        // df/dx = cos(x)
+        Math::cos,
+        h,
+        -20.0,
+        20.0);
+
+    assertCentralResults(
+        1,
+        3,
+        // f(x) = ln(x)
+        Math::log,
+        // df/dx = 1/x
+        (double x) -> 1.0 / x,
+        h,
+        1.0,
+        20.0);
+
+    assertCentralResults(
+        2,
+        5,
+        // f(x) = x²
+        (double x) -> x * x,
+        // d²f/dx² = 2
+        (double x) -> 2.0,
+        h,
+        -20.0,
+        20.0);
+
+    assertCentralResults(
+        2,
+        5,
+        // f(x) = sin(x)
+        Math::sin,
+        // d²f/dx² = -sin(x)
+        (double x) -> -Math.sin(x),
+        h,
+        -20.0,
+        20.0);
+
+    assertCentralResults(
+        2,
+        5,
+        // f(x) = ln(x)
+        Math::log,
+        // d²f/dx² = -1/x²
+        (double x) -> -1.0 / (x * x),
+        h,
+        1.0,
+        20.0);
+  }
+
+  /** Test backward finite difference. */
+  @Test
+  void backwardFiniteDifferenceTest() {
+    double h = 0.005;
+
+    assertBackwardResults(
+        1,
+        2,
+        // f(x) = x²
+        (double x) -> x * x,
+        // df/dx = 2x
+        (double x) -> 2.0 * x,
+        h,
+        -20.0,
+        20.0);
+
+    assertBackwardResults(
+        1,
+        2,
+        // f(x) = sin(x)
+        Math::sin,
+        // df/dx = cos(x)
+        Math::cos,
+        h,
+        -20.0,
+        20.0);
+
+    assertBackwardResults(
+        1,
+        2,
+        // f(x) = ln(x)
+        Math::log,
+        // df/dx = 1/x
+        (double x) -> 1.0 / x,
+        h,
+        1.0,
+        20.0);
+
+    assertBackwardResults(
+        2,
+        4,
+        // f(x) = x²
+        (double x) -> x * x,
+        // d²f/dx² = 2
+        (double x) -> 2.0,
+        h,
+        -20.0,
+        20.0);
+
+    assertBackwardResults(
+        2,
+        4,
+        // f(x) = sin(x)
+        Math::sin,
+        // d²f/dx² = -sin(x)
+        (double x) -> -Math.sin(x),
+        h,
+        -20.0,
+        20.0);
+
+    assertBackwardResults(
+        2,
+        4,
+        // f(x) = ln(x)
+        Math::log,
+        // d²f/dx² = -1/x²
+        (double x) -> -1.0 / (x * x),
+        h,
+        1.0,
+        20.0);
+  }
+
+  /**
+   * Helper for checking results of central finite difference.
+   *
+   * @param derivative The order of the derivative.
+   * @param samples The number of sample points.
+   * @param f Function of which to take derivative.
+   * @param dfdx Derivative of f.
+   * @param h Sample period in seconds.
+   * @param min Minimum of f's domain to test.
+   * @param max Maximum of f's domain to test.
+   */
+  void assertCentralResults(
+      int derivative,
+      int samples,
+      DoubleFunction<Double> f,
+      DoubleFunction<Double> dfdx,
+      double h,
+      double min,
+      double max) {
+    if (samples % 2 == 0) {
+      throw new IllegalArgumentException("Number of samples must be odd.");
+    }
+
+    // Generate stencil points from -(samples - 1)/2 to (samples - 1)/2
+    int[] stencil = new int[samples];
+    for (int i = 0; i < samples; ++i) {
+      stencil[i] = -(samples - 1) / 2 + i;
+    }
+
+    var filter = LinearFilter.finiteDifference(derivative, stencil, h);
+
+    for (int i = (int) (min / h); i < (int) (max / h); ++i) {
+      // Let filter initialize
+      if (i < (int) (min / h) + samples) {
+        filter.calculate(f.apply(i * h));
+        continue;
+      }
+
+      // The order of accuracy is O(h^(N - d)) where N is number of stencil
+      // points and d is order of derivative
+      assertEquals(
+          dfdx.apply((i - samples / 2) * h),
+          filter.calculate(f.apply(i * h)),
+          Math.pow(h, samples - derivative));
+    }
+  }
+
+  /**
+   * Helper for checking results of backward finite difference.
+   *
+   * @param derivative The order of the derivative.
+   * @param samples The number of sample points.
+   * @param f Function of which to take derivative.
+   * @param dfdx Derivative of f.
+   * @param h Sample period in seconds.
+   * @param min Minimum of f's domain to test.
+   * @param max Maximum of f's domain to test.
+   */
+  void assertBackwardResults(
+      int derivative,
+      int samples,
+      DoubleFunction<Double> f,
+      DoubleFunction<Double> dfdx,
+      double h,
+      double min,
+      double max) {
+    var filter = LinearFilter.backwardFiniteDifference(derivative, samples, h);
+
+    for (int i = (int) (min / h); i < (int) (max / h); ++i) {
+      // Let filter initialize
+      if (i < (int) (min / h) + samples) {
+        filter.calculate(f.apply(i * h));
+        continue;
+      }
+
+      // For central finite difference, the derivative computed at this point is
+      // half the window size in the past.
+      // The order of accuracy is O(h^(N - d)) where N is number of stencil
+      // points and d is order of derivative
+      assertEquals(
+          dfdx.apply(i * h),
+          filter.calculate(f.apply(i * h)),
+          10.0 * Math.pow(h, samples - derivative));
+    }
+  }
+}

wpimath/src/test/java/org/wpilib/math/filter/MedianFilterTest.java

@@ -0,0 +1,61 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.filter;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class MedianFilterTest {
+  @Test
+  void medianFilterNotFullTestEven() {
+    MedianFilter filter = new MedianFilter(10);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(4);
+
+    assertEquals(3.5, filter.calculate(1000));
+  }
+
+  @Test
+  void medianFilterNotFullTestOdd() {
+    MedianFilter filter = new MedianFilter(10);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(4);
+    filter.calculate(7);
+
+    assertEquals(4, filter.calculate(1000));
+  }
+
+  @Test
+  void medianFilterFullTestEven() {
+    MedianFilter filter = new MedianFilter(6);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(0);
+    filter.calculate(5);
+    filter.calculate(4);
+    filter.calculate(1000);
+
+    assertEquals(4.5, filter.calculate(99));
+  }
+
+  @Test
+  void medianFilterFullTestOdd() {
+    MedianFilter filter = new MedianFilter(5);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(5);
+    filter.calculate(4);
+    filter.calculate(1000);
+
+    assertEquals(5, filter.calculate(99));
+  }
+}

wpimath/src/test/java/org/wpilib/math/filter/SlewRateLimiterTest.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.filter;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.util.WPIUtilJNI;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class SlewRateLimiterTest {
+  @BeforeEach
+  void setUp() {
+    WPIUtilJNI.enableMockTime();
+    WPIUtilJNI.setMockTime(0L);
+  }
+
+  @AfterEach
+  void tearDown() {
+    WPIUtilJNI.setMockTime(0L);
+    WPIUtilJNI.disableMockTime();
+  }
+
+  @Test
+  void slewRateLimitTest() {
+    var limiter = new SlewRateLimiter(1);
+    WPIUtilJNI.setMockTime(1000000L);
+    assertTrue(limiter.calculate(2) < 2);
+  }
+
+  @Test
+  void slewRateNoLimitTest() {
+    var limiter = new SlewRateLimiter(1);
+    WPIUtilJNI.setMockTime(1000000L);
+    assertEquals(limiter.calculate(0.5), 0.5);
+  }
+
+  @Test
+  void slewRatePositiveNegativeTest() {
+    var limiter = new SlewRateLimiter(1, -0.5, 0);
+    WPIUtilJNI.setMockTime(1000000L);
+    assertEquals(limiter.calculate(2), 1);
+    WPIUtilJNI.setMockTime(2000000L);
+    assertEquals(limiter.calculate(0), 0.5);
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/CoordinateSystemTest.java

@@ -0,0 +1,235 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class CoordinateSystemTest {
+  private void checkPose3dConvert(
+      Pose3d poseFrom, Pose3d poseTo, CoordinateSystem coordFrom, CoordinateSystem coordTo) {
+    // "from" to "to"
+    assertEquals(
+        poseTo.getTranslation(),
+        CoordinateSystem.convert(poseFrom.getTranslation(), coordFrom, coordTo));
+    assertEquals(
+        poseTo.getRotation(), CoordinateSystem.convert(poseFrom.getRotation(), coordFrom, coordTo));
+    assertEquals(poseTo, CoordinateSystem.convert(poseFrom, coordFrom, coordTo));
+
+    // "to" to "from"
+    assertEquals(
+        poseFrom.getTranslation(),
+        CoordinateSystem.convert(poseTo.getTranslation(), coordTo, coordFrom));
+    assertEquals(
+        poseFrom.getRotation(), CoordinateSystem.convert(poseTo.getRotation(), coordTo, coordFrom));
+    assertEquals(poseFrom, CoordinateSystem.convert(poseTo, coordTo, coordFrom));
+  }
+
+  private void checkTransform3dConvert(
+      Transform3d transformFrom,
+      Transform3d transformTo,
+      CoordinateSystem coordFrom,
+      CoordinateSystem coordTo) {
+    // "from" to "to"
+    assertEquals(
+        transformTo.getTranslation(),
+        CoordinateSystem.convert(transformFrom.getTranslation(), coordFrom, coordTo));
+    assertEquals(transformTo, CoordinateSystem.convert(transformFrom, coordFrom, coordTo));
+
+    // "to" to "from"
+    assertEquals(
+        transformFrom.getTranslation(),
+        CoordinateSystem.convert(transformTo.getTranslation(), coordTo, coordFrom));
+    assertEquals(transformFrom, CoordinateSystem.convert(transformTo, coordTo, coordFrom));
+  }
+
+  @Test
+  void testPose3dEDNtoNWU() {
+    // No rotation from EDN to NWU
+    checkPose3dConvert(
+        new Pose3d(1.0, 2.0, 3.0, Rotation3d.kZero),
+        new Pose3d(
+            3.0,
+            -1.0,
+            -2.0,
+            new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+
+    // 45° roll from EDN to NWU
+    checkPose3dConvert(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
+        new Pose3d(
+            3.0,
+            -1.0,
+            -2.0,
+            new Rotation3d(Units.degreesToRadians(-45.0), 0.0, Units.degreesToRadians(-90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+
+    // 45° pitch from EDN to NWU
+    checkPose3dConvert(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
+        new Pose3d(
+            3.0,
+            -1.0,
+            -2.0,
+            new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-135.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+
+    // 45° yaw from EDN to NWU
+    checkPose3dConvert(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
+        new Pose3d(
+            3.0,
+            -1.0,
+            -2.0,
+            new Rotation3d(
+                Units.degreesToRadians(-90.0),
+                Units.degreesToRadians(45.0),
+                Units.degreesToRadians(-90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+  }
+
+  @Test
+  void testPose3dEDNtoNED() {
+    // No rotation from EDN to NED
+    checkPose3dConvert(
+        new Pose3d(1.0, 2.0, 3.0, Rotation3d.kZero),
+        new Pose3d(
+            3.0,
+            1.0,
+            2.0,
+            new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+
+    // 45° roll from EDN to NED
+    checkPose3dConvert(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
+        new Pose3d(
+            3.0,
+            1.0,
+            2.0,
+            new Rotation3d(Units.degreesToRadians(135.0), 0.0, Units.degreesToRadians(90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+
+    // 45° pitch from EDN to NED
+    checkPose3dConvert(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
+        new Pose3d(
+            3.0,
+            1.0,
+            2.0,
+            new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(135.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+
+    // 45° yaw from EDN to NED
+    checkPose3dConvert(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
+        new Pose3d(
+            3.0,
+            1.0,
+            2.0,
+            new Rotation3d(
+                Units.degreesToRadians(90.0),
+                Units.degreesToRadians(-45.0),
+                Units.degreesToRadians(90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+  }
+
+  @Test
+  void testTransform3dEDNtoNWU() {
+    // No rotation from EDN to NWU
+    checkTransform3dConvert(
+        new Transform3d(new Translation3d(1.0, 2.0, 3.0), Rotation3d.kZero),
+        new Transform3d(new Translation3d(3.0, -1.0, -2.0), Rotation3d.kZero),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+
+    // 45° roll from EDN to NWU
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
+        new Transform3d(
+            new Translation3d(3.0, -1.0, -2.0),
+            new Rotation3d(0.0, Units.degreesToRadians(-45.0), 0.0)),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+
+    // 45° pitch from EDN to NWU
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
+        new Transform3d(
+            new Translation3d(3.0, -1.0, -2.0),
+            new Rotation3d(0.0, 0.0, Units.degreesToRadians(-45.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+
+    // 45° yaw from EDN to NWU
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
+        new Transform3d(
+            new Translation3d(3.0, -1.0, -2.0),
+            new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+  }
+
+  @Test
+  void testTransform3dEDNtoNED() {
+    // No rotation from EDN to NED
+    checkTransform3dConvert(
+        new Transform3d(new Translation3d(1.0, 2.0, 3.0), Rotation3d.kZero),
+        new Transform3d(new Translation3d(3.0, 1.0, 2.0), Rotation3d.kZero),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+
+    // 45° roll from EDN to NED
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
+        new Transform3d(
+            new Translation3d(3.0, 1.0, 2.0),
+            new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+
+    // 45° pitch from EDN to NED
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
+        new Transform3d(
+            new Translation3d(3.0, 1.0, 2.0),
+            new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+
+    // 45° yaw from EDN to NED
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
+        new Transform3d(
+            new Translation3d(3.0, 1.0, 2.0),
+            new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/Ellipse2dTest.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.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+
+class Ellipse2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testGetFocalPoints() {
+    var center = new Pose2d(1, 2, new Rotation2d());
+    var ellipse = new Ellipse2d(center, 5.0, 4.0);
+
+    var pair = ellipse.getFocalPoints();
+    var a = pair.getFirst();
+    var b = pair.getSecond();
+
+    assertAll(
+        () -> assertEquals(new Translation2d(-2.0, 2.0), a),
+        () -> assertEquals(new Translation2d(4.0, 2.0), b));
+  }
+
+  @Test
+  void testIntersects() {
+    var center = new Pose2d(1.0, 2.0, new Rotation2d());
+    var ellipse = new Ellipse2d(center, 2.0, 1.0);
+
+    var pointA = new Translation2d(1.0, 3.0);
+    var pointB = new Translation2d(0.0, 3.0);
+
+    assertAll(
+        () -> assertTrue(ellipse.intersects(pointA)),
+        () -> assertFalse(ellipse.intersects(pointB)));
+  }
+
+  @Test
+  void testContains() {
+    var center = new Pose2d(-1.0, -2.0, Rotation2d.fromDegrees(45.0));
+    var ellipse = new Ellipse2d(center, 2.0, 1.0);
+
+    var pointA = new Translation2d(0.0, -1.0);
+    var pointB = new Translation2d(0.5, -2.0);
+
+    assertAll(
+        () -> assertTrue(ellipse.contains(pointA)), () -> assertFalse(ellipse.contains(pointB)));
+  }
+
+  @Test
+  void testDistance() {
+    var center = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(270.0));
+    var ellipse = new Ellipse2d(center, 1.0, 2.0);
+
+    var point1 = new Translation2d(2.5, 2.0);
+    assertEquals(0.0, ellipse.getDistance(point1), kEpsilon);
+
+    var point2 = new Translation2d(1.0, 2.0);
+    assertEquals(0.0, ellipse.getDistance(point2), kEpsilon);
+
+    var point3 = new Translation2d(1.0, 1.0);
+    assertEquals(0.0, ellipse.getDistance(point3), kEpsilon);
+
+    var point4 = new Translation2d(-1.0, 2.5);
+    assertEquals(0.19210128384806818, ellipse.getDistance(point4), kEpsilon);
+  }
+
+  @Test
+  void testNearest() {
+    var center = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(270.0));
+    var ellipse = new Ellipse2d(center, 1.0, 2.0);
+
+    var point1 = new Translation2d(2.5, 2.0);
+    var nearestPoint1 = ellipse.nearest(point1);
+    assertAll(
+        () -> assertEquals(2.5, nearestPoint1.getX(), kEpsilon),
+        () -> assertEquals(2.0, nearestPoint1.getY(), kEpsilon));
+
+    var point2 = new Translation2d(1.0, 2.0);
+    var nearestPoint2 = ellipse.nearest(point2);
+    assertAll(
+        () -> assertEquals(1.0, nearestPoint2.getX(), kEpsilon),
+        () -> assertEquals(2.0, nearestPoint2.getY(), kEpsilon));
+
+    var point3 = new Translation2d(1.0, 1.0);
+    var nearestPoint3 = ellipse.nearest(point3);
+    assertAll(
+        () -> assertEquals(1.0, nearestPoint3.getX(), kEpsilon),
+        () -> assertEquals(1.0, nearestPoint3.getY(), kEpsilon));
+
+    var point4 = new Translation2d(-1.0, 2.5);
+    var nearestPoint4 = ellipse.nearest(point4);
+    assertAll(
+        () -> assertEquals(-0.8512799937611617, nearestPoint4.getX(), kEpsilon),
+        () -> assertEquals(2.378405333174535, nearestPoint4.getY(), kEpsilon));
+  }
+
+  @Test
+  void testEquals() {
+    var center1 = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(90.0));
+    var ellipse1 = new Ellipse2d(center1, 2.0, 3.0);
+
+    var center2 = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(90.0));
+    var ellipse2 = new Ellipse2d(center2, 2.0, 3.0);
+
+    var center3 = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(90.0));
+    var ellipse3 = new Ellipse2d(center3, 3.0, 2.0);
+
+    assertAll(
+        () -> assertEquals(ellipse1, ellipse2),
+        () -> assertNotEquals(ellipse1, ellipse3),
+        () -> assertNotEquals(ellipse3, ellipse2));
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/Pose2dTest.java

@@ -0,0 +1,165 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+import edu.wpi.first.units.Units;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class Pose2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testNewWithMeasures() {
+    var pose = new Pose2d(Units.Inches.of(6), Units.Inches.of(8), Rotation2d.fromDegrees(45));
+
+    assertEquals(0.1524, pose.getX(), kEpsilon);
+    assertEquals(0.2032, pose.getY(), kEpsilon);
+    assertEquals(Math.PI / 4, pose.getRotation().getRadians(), kEpsilon);
+  }
+
+  @Test
+  void testRotateBy() {
+    final double x = 1.0;
+    final double y = 2.0;
+    var initial = new Pose2d(new Translation2d(x, y), Rotation2d.fromDegrees(45.0));
+
+    var rotation = Rotation2d.fromDegrees(5.0);
+    var rotated = initial.rotateBy(rotation);
+
+    // Translation is rotated by CCW rotation matrix
+    double c = rotation.getCos();
+    double s = rotation.getSin();
+    assertAll(
+        () -> assertEquals(c * x - s * y, rotated.getX(), kEpsilon),
+        () -> assertEquals(s * x + c * y, rotated.getY(), kEpsilon),
+        () ->
+            assertEquals(
+                initial.getRotation().getDegrees() + rotation.getDegrees(),
+                rotated.getRotation().getDegrees(),
+                kEpsilon));
+  }
+
+  @Test
+  void testTransformBy() {
+    var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
+    var transformation = new Transform2d(new Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0));
+
+    var transformed = initial.plus(transformation);
+
+    assertAll(
+        () -> assertEquals(1.0 + 5.0 / Math.sqrt(2.0), transformed.getX(), kEpsilon),
+        () -> assertEquals(2.0 + 5.0 / Math.sqrt(2.0), transformed.getY(), kEpsilon),
+        () -> assertEquals(50.0, transformed.getRotation().getDegrees(), kEpsilon));
+  }
+
+  @Test
+  void testRelativeTo() {
+    var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
+    var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
+
+    var finalRelativeToInitial = last.relativeTo(initial);
+
+    assertAll(
+        () -> assertEquals(5.0 * Math.sqrt(2.0), finalRelativeToInitial.getX(), kEpsilon),
+        () -> assertEquals(0.0, finalRelativeToInitial.getY(), kEpsilon),
+        () -> assertEquals(0.0, finalRelativeToInitial.getRotation().getDegrees(), kEpsilon));
+  }
+
+  @Test
+  void testRotateAround() {
+    var initial = new Pose2d(5, 0, Rotation2d.kZero);
+    var point = Translation2d.kZero;
+
+    var rotated = initial.rotateAround(point, Rotation2d.kPi);
+
+    assertAll(
+        () -> assertEquals(-5.0, rotated.getX(), kEpsilon),
+        () -> assertEquals(0.0, rotated.getY(), kEpsilon),
+        () -> assertEquals(180.0, rotated.getRotation().getDegrees(), kEpsilon));
+  }
+
+  @Test
+  void testEquality() {
+    var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
+    var two = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
+    var two = new Pose2d(0.0, 1.524, Rotation2d.fromDegrees(43.0));
+    assertNotEquals(one, two);
+  }
+
+  @Test
+  void testToMatrix() {
+    var before = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(20.0));
+    var after = new Pose2d(before.toMatrix());
+
+    assertEquals(before, after);
+  }
+
+  @Test
+  void testMinus() {
+    var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
+    var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
+
+    final var transform = last.minus(initial);
+
+    assertAll(
+        () -> assertEquals(5.0 * Math.sqrt(2.0), transform.getX(), kEpsilon),
+        () -> assertEquals(0.0, transform.getY(), kEpsilon),
+        () -> assertEquals(0.0, transform.getRotation().getDegrees(), kEpsilon));
+  }
+
+  @Test
+  void testNearest() {
+    var origin = Pose2d.kZero;
+
+    // Distance sort
+    // each poseX is X units away from the origin at a random angle.
+    final var pose1 =
+        new Pose2d(new Translation2d(1, Rotation2d.fromDegrees(45)), Rotation2d.kZero);
+    final var pose2 = new Pose2d(new Translation2d(2, Rotation2d.kCCW_Pi_2), Rotation2d.kZero);
+    final var pose3 =
+        new Pose2d(new Translation2d(3, Rotation2d.fromDegrees(135)), Rotation2d.kZero);
+    final var pose4 = new Pose2d(new Translation2d(4, Rotation2d.kPi), Rotation2d.kZero);
+    final var pose5 = new Pose2d(new Translation2d(5, Rotation2d.kCW_Pi_2), Rotation2d.kZero);
+
+    assertEquals(pose3, origin.nearest(List.of(pose5, pose3, pose4)));
+    assertEquals(pose1, origin.nearest(List.of(pose1, pose2, pose3)));
+    assertEquals(pose2, origin.nearest(List.of(pose4, pose2, pose3)));
+
+    // Rotation component sort (when distance is the same)
+    // Use the same translation because using different angles at the same distance can cause
+    // rounding error.
+    final var translation = new Translation2d(1, Rotation2d.kZero);
+
+    final var poseA = new Pose2d(translation, Rotation2d.kZero);
+    final var poseB = new Pose2d(translation, Rotation2d.fromDegrees(30));
+    final var poseC = new Pose2d(translation, Rotation2d.fromDegrees(120));
+    final var poseD = new Pose2d(translation, Rotation2d.kCCW_Pi_2);
+    final var poseE = new Pose2d(translation, Rotation2d.fromDegrees(-180));
+
+    assertEquals(
+        poseA, new Pose2d(0, 0, Rotation2d.fromDegrees(360)).nearest(List.of(poseA, poseB, poseD)));
+    assertEquals(
+        poseB,
+        new Pose2d(0, 0, Rotation2d.fromDegrees(-335)).nearest(List.of(poseB, poseC, poseD)));
+    assertEquals(
+        poseC,
+        new Pose2d(0, 0, Rotation2d.fromDegrees(-120)).nearest(List.of(poseB, poseC, poseD)));
+    assertEquals(
+        poseD, new Pose2d(0, 0, Rotation2d.fromDegrees(85)).nearest(List.of(poseA, poseC, poseD)));
+    assertEquals(
+        poseE, new Pose2d(0, 0, Rotation2d.fromDegrees(170)).nearest(List.of(poseA, poseD, poseE)));
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/Pose3dTest.java

@@ -0,0 +1,394 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.util.Units;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class Pose3dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testRotateBy() {
+    final double x = 1.0;
+    final double y = 2.0;
+    var initial =
+        new Pose3d(
+            new Translation3d(x, y, 0.0),
+            new Rotation3d(
+                Units.degreesToRadians(0.0),
+                Units.degreesToRadians(0.0),
+                Units.degreesToRadians(45.0)));
+
+    double yaw = Units.degreesToRadians(5.0);
+    var rotation = new Rotation3d(Units.degreesToRadians(0.0), Units.degreesToRadians(0.0), yaw);
+    var rotated = initial.rotateBy(rotation);
+
+    // Translation is rotated by CCW rotation matrix
+    double c = Math.cos(yaw);
+    double s = Math.sin(yaw);
+    assertAll(
+        () -> assertEquals(c * x - s * y, rotated.getX(), kEpsilon),
+        () -> assertEquals(s * x + c * y, rotated.getY(), kEpsilon),
+        () -> assertEquals(0.0, rotated.getZ(), kEpsilon),
+        () -> assertEquals(0.0, rotated.getRotation().getX(), kEpsilon),
+        () -> assertEquals(0.0, rotated.getRotation().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                initial.getRotation().getZ() + rotation.getZ(),
+                rotated.getRotation().getZ(),
+                kEpsilon));
+  }
+
+  @Test
+  void testTransformByRotations() {
+    var initialPose = Pose3d.kZero;
+
+    var transform1 =
+        new Transform3d(
+            Translation3d.kZero,
+            new Rotation3d(
+                Units.degreesToRadians(90.0),
+                Units.degreesToRadians(45.0),
+                Units.degreesToRadians(0.0)));
+
+    var transform2 =
+        new Transform3d(
+            Translation3d.kZero,
+            new Rotation3d(
+                Units.degreesToRadians(-90.0),
+                Units.degreesToRadians(0.0),
+                Units.degreesToRadians(0.0)));
+
+    var transform3 =
+        new Transform3d(
+            Translation3d.kZero,
+            new Rotation3d(
+                Units.degreesToRadians(0.0),
+                Units.degreesToRadians(-45.0),
+                Units.degreesToRadians(0.0)));
+
+    // This sequence of rotations should diverge from the origin and eventually
+    // return to it. When
+    // each rotation occurs, it should be performed intrinsicly, i.e. 'from the
+    // viewpoint' of and
+    // with
+    // the axes of the pose that is being transformed, just like how the translation
+    // is done 'from
+    // the
+    // viewpoint' of the pose that is being transformed.
+    var finalPose =
+        initialPose.transformBy(transform1).transformBy(transform2).transformBy(transform3);
+
+    assertAll(
+        () ->
+            assertEquals(
+                finalPose.getRotation().getX(), initialPose.getRotation().getX(), kEpsilon),
+        () ->
+            assertEquals(
+                finalPose.getRotation().getY(), initialPose.getRotation().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                finalPose.getRotation().getZ(), initialPose.getRotation().getZ(), kEpsilon));
+  }
+
+  @Test
+  void testTransformBy() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var initial =
+        new Pose3d(
+            new Translation3d(1.0, 2.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+    var transformation =
+        new Transform3d(
+            new Translation3d(5.0, 0.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0)));
+
+    var transformed = initial.plus(transformation);
+
+    assertAll(
+        () -> assertEquals(1.0 + 5.0 / Math.sqrt(2.0), transformed.getX(), kEpsilon),
+        () -> assertEquals(2.0 + 5.0 / Math.sqrt(2.0), transformed.getY(), kEpsilon),
+        () ->
+            assertEquals(Units.degreesToRadians(50.0), transformed.getRotation().getZ(), kEpsilon));
+  }
+
+  @Test
+  void testRelativeTo() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var initial = new Pose3d(0.0, 0.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+    var last = new Pose3d(5.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+
+    var finalRelativeToInitial = last.relativeTo(initial);
+
+    assertAll(
+        () -> assertEquals(5.0 * Math.sqrt(2.0), finalRelativeToInitial.getX(), kEpsilon),
+        () -> assertEquals(0.0, finalRelativeToInitial.getY(), kEpsilon),
+        () -> assertEquals(0.0, finalRelativeToInitial.getRotation().getZ(), kEpsilon));
+  }
+
+  @Test
+  void testRotateAround() {
+    var initial = new Pose3d(new Translation3d(5, 0, 0), Rotation3d.kZero);
+    var point = Translation3d.kZero;
+
+    var rotated = initial.rotateAround(point, new Rotation3d(0, 0, Math.PI));
+
+    assertAll(
+        () -> assertEquals(-5.0, rotated.getX(), kEpsilon),
+        () -> assertEquals(0.0, rotated.getY(), kEpsilon),
+        () -> assertEquals(Math.PI, rotated.getRotation().getZ(), kEpsilon));
+  }
+
+  @Test
+  void testEquality() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var one = new Pose3d(0.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0)));
+    var two = new Pose3d(0.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0)));
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var one = new Pose3d(0.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0)));
+    var two = new Pose3d(0.0, 1.524, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0)));
+    assertNotEquals(one, two);
+  }
+
+  @Test
+  void testMinus() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var initial = new Pose3d(0.0, 0.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+    var last = new Pose3d(5.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+
+    final var transform = last.minus(initial);
+
+    assertAll(
+        () -> assertEquals(5.0 * Math.sqrt(2.0), transform.getX(), kEpsilon),
+        () -> assertEquals(0.0, transform.getY(), kEpsilon),
+        () -> assertEquals(0.0, transform.getRotation().getZ(), kEpsilon));
+  }
+
+  @Test
+  void testToMatrix() {
+    var before =
+        new Pose3d(
+            1.0,
+            2.0,
+            3.0,
+            new Rotation3d(
+                Units.degreesToRadians(20.0),
+                Units.degreesToRadians(30.0),
+                Units.degreesToRadians(40.0)));
+    var after = new Pose3d(before.toMatrix());
+
+    assertEquals(before, after);
+  }
+
+  @Test
+  void testToPose2d() {
+    var pose =
+        new Pose3d(
+            1.0,
+            2.0,
+            3.0,
+            new Rotation3d(
+                Units.degreesToRadians(20.0),
+                Units.degreesToRadians(30.0),
+                Units.degreesToRadians(40.0)));
+    var expected = new Pose2d(1.0, 2.0, new Rotation2d(Units.degreesToRadians(40.0)));
+
+    assertEquals(expected, pose.toPose2d());
+  }
+
+  @Test
+  void testComplexTwists() {
+    var initial_poses =
+        List.of(
+            new Pose3d(
+                new Translation3d(0.698303, -0.959096, 0.271076),
+                new Rotation3d(new Quaternion(0.86403, -0.076866, 0.147234, 0.475254))),
+            new Pose3d(
+                new Translation3d(0.634892, -0.765209, 0.117543),
+                new Rotation3d(new Quaternion(0.84987, -0.070829, 0.162097, 0.496415))),
+            new Pose3d(
+                new Translation3d(0.584827, -0.590303, -0.02557),
+                new Rotation3d(new Quaternion(0.832743, -0.041991, 0.202188, 0.513708))),
+            new Pose3d(
+                new Translation3d(0.505038, -0.451479, -0.112835),
+                new Rotation3d(new Quaternion(0.816515, -0.002673, 0.226182, 0.531166))),
+            new Pose3d(
+                new Translation3d(0.428178, -0.329692, -0.189707),
+                new Rotation3d(new Quaternion(0.807886, 0.029298, 0.257788, 0.529157))));
+
+    var final_poses =
+        List.of(
+            new Pose3d(
+                new Translation3d(-0.230448, -0.511957, 0.198406),
+                new Rotation3d(new Quaternion(0.753984, 0.347016, 0.409105, 0.379106))),
+            new Pose3d(
+                new Translation3d(-0.088932, -0.343253, 0.095018),
+                new Rotation3d(new Quaternion(0.638738, 0.413016, 0.536281, 0.365833))),
+            new Pose3d(
+                new Translation3d(-0.107908, -0.317552, 0.133946),
+                new Rotation3d(new Quaternion(0.653444, 0.417069, 0.465505, 0.427046))),
+            new Pose3d(
+                new Translation3d(-0.123383, -0.156411, -0.047435),
+                new Rotation3d(new Quaternion(0.652983, 0.40644, 0.431566, 0.47135))),
+            new Pose3d(
+                new Translation3d(-0.084654, -0.019305, -0.030022),
+                new Rotation3d(new Quaternion(0.620243, 0.429104, 0.479384, 0.44873))));
+
+    final var eps = 1E-5;
+    for (int i = 0; i < initial_poses.size(); i++) {
+      var start = initial_poses.get(i);
+      var end = final_poses.get(i);
+
+      var twist = end.minus(start).log();
+      var start_exp = start.plus(twist.exp());
+
+      assertAll(
+          () -> assertEquals(start_exp.getX(), end.getX(), eps),
+          () -> assertEquals(start_exp.getY(), end.getY(), eps),
+          () -> assertEquals(start_exp.getZ(), end.getZ(), eps),
+          () ->
+              assertEquals(
+                  start_exp.getRotation().getQuaternion().getW(),
+                  end.getRotation().getQuaternion().getW(),
+                  eps),
+          () ->
+              assertEquals(
+                  start_exp.getRotation().getQuaternion().getX(),
+                  end.getRotation().getQuaternion().getX(),
+                  eps),
+          () ->
+              assertEquals(
+                  start_exp.getRotation().getQuaternion().getY(),
+                  end.getRotation().getQuaternion().getY(),
+                  eps),
+          () ->
+              assertEquals(
+                  start_exp.getRotation().getQuaternion().getZ(),
+                  end.getRotation().getQuaternion().getZ(),
+                  eps));
+    }
+  }
+
+  @Test
+  void testTwistNaN() {
+    var initial_poses =
+        List.of(
+            new Pose3d(
+                new Translation3d(6.32, 4.12, 0.00),
+                new Rotation3d(
+                    new Quaternion(-0.9999999999999999, 0.0, 0.0, 1.9208309264993548E-8))),
+            new Pose3d(
+                new Translation3d(3.75, 2.95, 0.00),
+                new Rotation3d(
+                    new Quaternion(0.9999999999999793, 0.0, 0.0, 2.0352360299846772E-7))));
+    var final_poses =
+        List.of(
+            new Pose3d(
+                new Translation3d(6.33, 4.15, 0.00),
+                new Rotation3d(
+                    new Quaternion(-0.9999999999999999, 0.0, 0.0, 2.416890209039172E-8))),
+            new Pose3d(
+                new Translation3d(3.66, 2.93, 0.00),
+                new Rotation3d(
+                    new Quaternion(0.9999999999999782, 0.0, 0.0, 2.0859477994905617E-7))));
+
+    for (int i = 0; i < initial_poses.size(); i++) {
+      var start = initial_poses.get(i);
+      var end = final_poses.get(i);
+
+      var twist = end.minus(start).log();
+      assertAll(
+          () -> assertFalse(((Double) twist.dx).isNaN()),
+          () -> assertFalse(((Double) twist.dy).isNaN()),
+          () -> assertFalse(((Double) twist.dz).isNaN()),
+          () -> assertFalse(((Double) twist.rx).isNaN()),
+          () -> assertFalse(((Double) twist.ry).isNaN()),
+          () -> assertFalse(((Double) twist.rz).isNaN()));
+    }
+  }
+
+  @Test
+  void testNearest() {
+    var origin = Pose3d.kZero;
+
+    // Distance sort
+    // poses are in order of closest to farthest away from the origin at various positions in 3D
+    // space.
+    final var pose1 = new Pose3d(1, 0, 0, Rotation3d.kZero);
+    final var pose2 = new Pose3d(0, 2, 0, Rotation3d.kZero);
+    final var pose3 = new Pose3d(0, 0, 3, Rotation3d.kZero);
+    final var pose4 = new Pose3d(2, 2, 2, Rotation3d.kZero);
+    final var pose5 = new Pose3d(3, 3, 3, Rotation3d.kZero);
+
+    assertEquals(pose3, origin.nearest(List.of(pose5, pose3, pose4)));
+    assertEquals(pose1, origin.nearest(List.of(pose1, pose2, pose3)));
+    assertEquals(pose2, origin.nearest(List.of(pose4, pose2, pose3)));
+
+    // Rotation component sort (when distance is the same)
+    // Use the same translation to avoid distance differences
+    final var translation = new Translation3d(1, 0, 0);
+
+    final var poseA = new Pose3d(translation, Rotation3d.kZero); // No rotation
+    final var poseB = new Pose3d(translation, new Rotation3d(Math.toRadians(30), 0, 0));
+    final var poseC = new Pose3d(translation, new Rotation3d(0, Math.toRadians(45), 0));
+    final var poseD = new Pose3d(translation, new Rotation3d(0, 0, Math.toRadians(90)));
+    final var poseE = new Pose3d(translation, new Rotation3d(Math.toRadians(180), 0, 0));
+
+    assertEquals(
+        poseA, new Pose3d(0, 0, 0, Rotation3d.kZero).nearest(List.of(poseA, poseB, poseD)));
+    assertEquals(
+        poseB,
+        new Pose3d(0, 0, 0, new Rotation3d(Math.toRadians(25), 0, 0))
+            .nearest(List.of(poseB, poseC, poseD)));
+    assertEquals(
+        poseC,
+        new Pose3d(0, 0, 0, new Rotation3d(0, Math.toRadians(50), 0))
+            .nearest(List.of(poseB, poseC, poseD)));
+    assertEquals(
+        poseD,
+        new Pose3d(0, 0, 0, new Rotation3d(0, 0, Math.toRadians(85)))
+            .nearest(List.of(poseA, poseC, poseD)));
+    assertEquals(
+        poseE,
+        new Pose3d(0, 0, 0, new Rotation3d(Math.toRadians(170), 0, 0))
+            .nearest(List.of(poseA, poseD, poseE)));
+
+    // Test with complex 3D rotations (combining roll, pitch, yaw)
+    final var complexPose1 =
+        new Pose3d(
+            translation,
+            new Rotation3d(Math.toRadians(45), Math.toRadians(30), Math.toRadians(60)));
+    final var complexPose2 =
+        new Pose3d(
+            translation,
+            new Rotation3d(Math.toRadians(90), Math.toRadians(45), Math.toRadians(90)));
+    final var complexPose3 =
+        new Pose3d(
+            translation,
+            new Rotation3d(Math.toRadians(10), Math.toRadians(15), Math.toRadians(20)));
+
+    assertEquals(
+        complexPose3,
+        new Pose3d(
+                0, 0, 0, new Rotation3d(Math.toRadians(5), Math.toRadians(10), Math.toRadians(15)))
+            .nearest(List.of(complexPose1, complexPose2, complexPose3)));
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/QuaternionTest.java

@@ -0,0 +1,245 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class QuaternionTest {
+  @Test
+  void testInit() {
+    // Identity
+    var q1 = new Quaternion();
+    assertAll(
+        () -> assertEquals(1.0, q1.getW()),
+        () -> assertEquals(0.0, q1.getX()),
+        () -> assertEquals(0.0, q1.getY()),
+        () -> assertEquals(0.0, q1.getZ()));
+
+    // Normalized
+    var q2 = new Quaternion(0.5, 0.5, 0.5, 0.5);
+    assertAll(
+        () -> assertEquals(0.5, q2.getW()),
+        () -> assertEquals(0.5, q2.getX()),
+        () -> assertEquals(0.5, q2.getY()),
+        () -> assertEquals(0.5, q2.getZ()));
+
+    // Unnormalized
+    var q3 = new Quaternion(0.75, 0.3, 0.4, 0.5);
+    assertAll(
+        () -> assertEquals(0.75, q3.getW()),
+        () -> assertEquals(0.3, q3.getX()),
+        () -> assertEquals(0.4, q3.getY()),
+        () -> assertEquals(0.5, q3.getZ()));
+
+    var q3_norm = q3.normalize();
+    double norm = Math.sqrt(0.75 * 0.75 + 0.3 * 0.3 + 0.4 * 0.4 + 0.5 * 0.5);
+    assertAll(
+        () -> assertEquals(0.75 / norm, q3_norm.getW()),
+        () -> assertEquals(0.3 / norm, q3_norm.getX()),
+        () -> assertEquals(0.4 / norm, q3_norm.getY()),
+        () -> assertEquals(0.5 / norm, q3_norm.getZ()),
+        () -> assertEquals(1.0, q3_norm.dot(q3_norm)));
+  }
+
+  @Test
+  void testAddition() {
+    var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
+    var p = new Quaternion(0.5, 0.6, 0.7, 0.8);
+
+    var sum = q.plus(p);
+    assertAll(
+        () -> assertEquals(q.getW() + p.getW(), sum.getW()),
+        () -> assertEquals(q.getX() + p.getX(), sum.getX()),
+        () -> assertEquals(q.getY() + p.getY(), sum.getY()),
+        () -> assertEquals(q.getZ() + p.getZ(), sum.getZ()));
+  }
+
+  @Test
+  void testSubtraction() {
+    var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
+    var p = new Quaternion(0.5, 0.6, 0.7, 0.8);
+
+    var difference = q.minus(p);
+
+    assertAll(
+        () -> assertEquals(q.getW() - p.getW(), difference.getW()),
+        () -> assertEquals(q.getX() - p.getX(), difference.getX()),
+        () -> assertEquals(q.getY() - p.getY(), difference.getY()),
+        () -> assertEquals(q.getZ() - p.getZ(), difference.getZ()));
+  }
+
+  @Test
+  void testScalarMultiplication() {
+    var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
+    var scalar = 2;
+
+    var product = q.times(scalar);
+
+    assertAll(
+        () -> assertEquals(q.getW() * scalar, product.getW()),
+        () -> assertEquals(q.getX() * scalar, product.getX()),
+        () -> assertEquals(q.getY() * scalar, product.getY()),
+        () -> assertEquals(q.getZ() * scalar, product.getZ()));
+  }
+
+  @Test
+  void testScalarDivision() {
+    var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
+    var scalar = 2;
+
+    var product = q.divide(scalar);
+
+    assertAll(
+        () -> assertEquals(q.getW() / scalar, product.getW()),
+        () -> assertEquals(q.getX() / scalar, product.getX()),
+        () -> assertEquals(q.getY() / scalar, product.getY()),
+        () -> assertEquals(q.getZ() / scalar, product.getZ()));
+  }
+
+  @Test
+  void testTimes() {
+    // 90° CCW rotations around each axis
+    double c = Math.cos(Units.degreesToRadians(90.0) / 2.0);
+    double s = Math.sin(Units.degreesToRadians(90.0) / 2.0);
+    var xRot = new Quaternion(c, s, 0.0, 0.0);
+    var yRot = new Quaternion(c, 0.0, s, 0.0);
+    var zRot = new Quaternion(c, 0.0, 0.0, s);
+
+    // 90° CCW X rotation, 90° CCW Y rotation, and 90° CCW Z rotation should
+    // produce a 90° CCW Y rotation
+    var expected = yRot;
+    final var actual = zRot.times(yRot).times(xRot);
+    assertAll(
+        () -> assertEquals(expected.getW(), actual.getW(), 1e-9),
+        () -> assertEquals(expected.getX(), actual.getX(), 1e-9),
+        () -> assertEquals(expected.getY(), actual.getY(), 1e-9),
+        () -> assertEquals(expected.getZ(), actual.getZ(), 1e-9));
+
+    // Identity
+    var q =
+        new Quaternion(
+            0.7276068751089989, 0.29104275004359953, 0.38805700005813276, 0.48507125007266594);
+    final var actual2 = q.times(q.inverse());
+    assertAll(
+        () -> assertEquals(1.0, actual2.getW()),
+        () -> assertEquals(0.0, actual2.getX()),
+        () -> assertEquals(0.0, actual2.getY()),
+        () -> assertEquals(0.0, actual2.getZ()));
+  }
+
+  @Test
+  void testConjugate() {
+    var q = new Quaternion(0.75, 0.3, 0.4, 0.5);
+    var inv = q.conjugate();
+
+    assertAll(
+        () -> assertEquals(q.getW(), inv.getW()),
+        () -> assertEquals(-q.getX(), inv.getX()),
+        () -> assertEquals(-q.getY(), inv.getY()),
+        () -> assertEquals(-q.getZ(), inv.getZ()));
+  }
+
+  @Test
+  void testInverse() {
+    var q = new Quaternion(0.75, 0.3, 0.4, 0.5);
+    var inv = q.inverse();
+    var norm = q.norm();
+
+    assertAll(
+        () -> assertEquals(q.getW() / (norm * norm), inv.getW(), 1e-10),
+        () -> assertEquals(-q.getX() / (norm * norm), inv.getX(), 1e-10),
+        () -> assertEquals(-q.getY() / (norm * norm), inv.getY(), 1e-10),
+        () -> assertEquals(-q.getZ() / (norm * norm), inv.getZ(), 1e-10));
+  }
+
+  @Test
+  void testNorm() {
+    var q = new Quaternion(3, 4, 12, 84);
+
+    // pythagorean triples (3, 4, 5), (5, 12, 13), (13, 84, 85)
+    assertEquals(q.norm(), 85, 1e-10);
+  }
+
+  @Test
+  void testExponential() {
+    var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+    var q_exp =
+        new Quaternion(
+            2.81211398529184, -0.392521193481878, -0.588781790222817, -0.785042386963756);
+
+    assertEquals(q_exp, q.exp());
+  }
+
+  @Test
+  void testLogarithm() {
+    var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+    var q_log =
+        new Quaternion(1.7959088706354, 0.515190292664085, 0.772785438996128, 1.03038058532817);
+
+    assertEquals(q_log, q.log());
+
+    var zero = new Quaternion(0, 0, 0, 0);
+    var one = new Quaternion();
+
+    assertEquals(zero, one.log());
+
+    var i = new Quaternion(0, 1, 0, 0);
+    assertEquals(i.times(Math.PI / 2), i.log());
+
+    var j = new Quaternion(0, 0, 1, 0);
+    assertEquals(j.times(Math.PI / 2), j.log());
+
+    var k = new Quaternion(0, 0, 0, 1);
+    assertEquals(k.times(Math.PI / 2), k.log());
+    assertEquals(i.times(-Math.PI), one.times(-1).log());
+
+    var ln_half = Math.log(0.5);
+    assertEquals(new Quaternion(ln_half, -Math.PI, 0, 0), one.times(-0.5).log());
+  }
+
+  @Test
+  void testLogarithmIsInverseOfExponential() {
+    var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+
+    // These operations are order-dependent: ln(exp(q)) is congruent
+    // but not necessarily equal to exp(ln(q)) due to the multi-valued nature of the complex
+    // logarithm.
+
+    var q_log_exp = q.log().exp();
+
+    assertEquals(q, q_log_exp);
+
+    var start = new Quaternion(1, 2, 3, 4);
+    var expect = new Quaternion(5, 6, 7, 8);
+
+    var twist = start.log(expect);
+    var actual = start.exp(twist);
+
+    assertEquals(expect, actual);
+  }
+
+  @Test
+  void testDotProduct() {
+    var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+    var p = new Quaternion(5.5, 6.6, 7.7, 8.8);
+
+    assertEquals(
+        q.getW() * p.getW() + q.getX() * p.getX() + q.getY() * p.getY() + q.getZ() * p.getZ(),
+        q.dot(p));
+  }
+
+  @Test
+  void testDotProductAsEquality() {
+    var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+    var q_conj = q.conjugate();
+
+    assertAll(() -> assertEquals(q, q), () -> assertNotEquals(q, q_conj));
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/Rectangle2dTest.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.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+
+class Rectangle2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testNewWithCorners() {
+    var cornerA = new Translation2d(1.0, 2.0);
+    var cornerB = new Translation2d(4.0, 6.0);
+
+    var rect = new Rectangle2d(cornerA, cornerB);
+
+    assertAll(
+        () -> assertEquals(3.0, rect.getXWidth()),
+        () -> assertEquals(4.0, rect.getYWidth()),
+        () -> assertEquals(2.5, rect.getCenter().getX()),
+        () -> assertEquals(4.0, rect.getCenter().getY()));
+  }
+
+  @Test
+  void testIntersects() {
+    var center = new Pose2d(4.0, 3.0, Rotation2d.fromDegrees(90.0));
+    var rect = new Rectangle2d(center, 2.0, 3.0);
+
+    assertAll(
+        () -> assertTrue(rect.intersects(new Translation2d(5.5, 4.0))),
+        () -> assertTrue(rect.intersects(new Translation2d(3.0, 2.0))),
+        () -> assertFalse(rect.intersects(new Translation2d(4.0, 1.5))),
+        () -> assertFalse(rect.intersects(new Translation2d(4.0, 3.5))));
+  }
+
+  @Test
+  void testContains() {
+    var center = new Pose2d(2.0, 3.0, Rotation2d.fromDegrees(45.0));
+    var rect = new Rectangle2d(center, 3.0, 1.0);
+
+    assertAll(
+        () -> assertTrue(rect.contains(new Translation2d(2.0, 3.0))),
+        () -> assertTrue(rect.contains(new Translation2d(3.0, 4.0))),
+        () -> assertFalse(rect.contains(new Translation2d(3.0, 3.0))));
+  }
+
+  @Test
+  void testDistance() {
+    var center = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(270.0));
+    var rect = new Rectangle2d(center, 1.0, 2.0);
+
+    var point1 = new Translation2d(2.5, 2.0);
+    assertEquals(0.5, rect.getDistance(point1), kEpsilon);
+
+    var point2 = new Translation2d(1.0, 2.0);
+    assertEquals(0.0, rect.getDistance(point2), kEpsilon);
+
+    var point3 = new Translation2d(1.0, 1.0);
+    assertEquals(0.5, rect.getDistance(point3), kEpsilon);
+
+    var point4 = new Translation2d(-1.0, 2.5);
+    assertEquals(1.0, rect.getDistance(point4), kEpsilon);
+  }
+
+  @Test
+  void testNearest() {
+    var center = new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(90.0));
+    var rect = new Rectangle2d(center, 3.0, 4.0);
+
+    var point1 = new Translation2d(1.0, 3.0);
+    var nearestPoint1 = rect.nearest(point1);
+    assertAll(
+        () -> assertEquals(1.0, nearestPoint1.getX(), kEpsilon),
+        () -> assertEquals(2.5, nearestPoint1.getY(), kEpsilon));
+
+    var point2 = new Translation2d(0.0, 0.0);
+    var nearestPoint2 = rect.nearest(point2);
+    assertAll(
+        () -> assertEquals(0.0, nearestPoint2.getX(), kEpsilon),
+        () -> assertEquals(0.0, nearestPoint2.getY(), kEpsilon));
+  }
+
+  @Test
+  void testEquals() {
+    var center1 = new Pose2d(2.0, 3.0, new Rotation2d());
+    var rect1 = new Rectangle2d(center1, 5.0, 3.0);
+
+    var center2 = new Pose2d(2.0, 3.0, new Rotation2d());
+    var rect2 = new Rectangle2d(center2, 5.0, 3.0);
+
+    var center3 = new Pose2d(2.0, 3.0, new Rotation2d());
+    var rect3 = new Rectangle2d(center3, 3.0, 3.0);
+
+    assertAll(() -> assertEquals(rect1, rect2), () -> assertNotEquals(rect2, rect3));
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/Rotation2dTest.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.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+import edu.wpi.first.units.Units;
+import org.junit.jupiter.api.Test;
+
+class Rotation2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testNewWithMeasures() {
+    var rot = new Rotation2d(Units.Degrees.of(45));
+
+    assertEquals(Math.PI / 4, rot.getRadians(), kEpsilon);
+  }
+
+  @Test
+  void testRadiansToDegrees() {
+    var rot1 = Rotation2d.fromRadians(Math.PI / 3);
+    var rot2 = Rotation2d.fromRadians(Math.PI / 4);
+
+    assertAll(
+        () -> assertEquals(60.0, rot1.getDegrees(), kEpsilon),
+        () -> assertEquals(45.0, rot2.getDegrees(), kEpsilon));
+  }
+
+  @Test
+  void testRadiansAndDegrees() {
+    var rot1 = Rotation2d.fromDegrees(45.0);
+    var rot2 = Rotation2d.fromDegrees(30.0);
+
+    assertAll(
+        () -> assertEquals(Math.PI / 4.0, rot1.getRadians(), kEpsilon),
+        () -> assertEquals(Math.PI / 6.0, rot2.getRadians(), kEpsilon));
+  }
+
+  @Test
+  void testRotateByFromZero() {
+    var zero = Rotation2d.kZero;
+    var rotated = zero.rotateBy(Rotation2d.kCCW_Pi_2);
+
+    assertAll(
+        () -> assertEquals(Math.PI / 2.0, rotated.getRadians(), kEpsilon),
+        () -> assertEquals(90.0, rotated.getDegrees(), kEpsilon));
+  }
+
+  @Test
+  void testRotateByNonZero() {
+    var rot = Rotation2d.kCCW_Pi_2;
+    rot = rot.plus(Rotation2d.fromDegrees(30.0));
+
+    assertEquals(120.0, rot.getDegrees(), kEpsilon);
+  }
+
+  @Test
+  void testMinus() {
+    var rot1 = Rotation2d.fromDegrees(70.0);
+    var rot2 = Rotation2d.fromDegrees(30.0);
+
+    assertEquals(40.0, rot1.minus(rot2).getDegrees(), kEpsilon);
+  }
+
+  @Test
+  void testUnaryMinus() {
+    var rot = Rotation2d.fromDegrees(20.0);
+
+    assertEquals(-20.0, rot.unaryMinus().getDegrees(), kEpsilon);
+  }
+
+  @Test
+  void testMultiply() {
+    var rot = Rotation2d.fromDegrees(10.0);
+
+    assertEquals(30.0, rot.times(3.0).getDegrees(), kEpsilon);
+    assertEquals(50.0, rot.times(41.0).getDegrees(), kEpsilon);
+  }
+
+  @Test
+  void testEquality() {
+    var rot1 = Rotation2d.fromDegrees(43.0);
+    var rot2 = Rotation2d.fromDegrees(43.0);
+    assertEquals(rot1, rot2);
+
+    rot1 = Rotation2d.fromDegrees(-180.0);
+    rot2 = Rotation2d.fromDegrees(180.0);
+    assertEquals(rot1, rot2);
+  }
+
+  @Test
+  void testInequality() {
+    var rot1 = Rotation2d.fromDegrees(43.0);
+    var rot2 = Rotation2d.fromDegrees(43.5);
+    assertNotEquals(rot1, rot2);
+  }
+
+  @Test
+  void testToMatrix() {
+    var before = Rotation2d.fromDegrees(20.0);
+    var after = new Rotation2d(before.toMatrix());
+
+    assertEquals(before, after);
+  }
+
+  @Test
+  void testInterpolate() {
+    // 50 + (70 - 50) * 0.5 = 60
+    var rot1 = Rotation2d.fromDegrees(50);
+    var rot2 = Rotation2d.fromDegrees(70);
+    var interpolated = rot1.interpolate(rot2, 0.5);
+    assertEquals(60.0, interpolated.getDegrees(), kEpsilon);
+
+    // -160 minus half distance between 170 and -160 (15) = -175
+    rot1 = Rotation2d.fromDegrees(170);
+    rot2 = Rotation2d.fromDegrees(-160);
+    interpolated = rot1.interpolate(rot2, 0.5);
+    assertEquals(-175.0, interpolated.getDegrees(), kEpsilon);
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/Rotation3dTest.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 static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class Rotation3dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testGimbalLockAccuracy() {
+    var rot1 = new Rotation3d(0, 0, Math.PI / 2);
+    var rot2 = new Rotation3d(Math.PI, 0, 0);
+    var rot3 = new Rotation3d(-Math.PI / 2, 0, 0);
+    final var result1 = rot1.plus(rot2).plus(rot3);
+    final var expected1 = new Rotation3d(0, -Math.PI / 2, Math.PI / 2);
+    assertAll(
+        () -> assertEquals(expected1, result1),
+        () -> assertEquals(Math.PI / 2, result1.getX() + result1.getZ(), kEpsilon),
+        () -> assertEquals(-Math.PI / 2, result1.getY(), kEpsilon));
+
+    rot1 = new Rotation3d(0, 0, Math.PI / 2);
+    rot2 = new Rotation3d(-Math.PI, 0, 0);
+    rot3 = new Rotation3d(Math.PI / 2, 0, 0);
+    final var result2 = rot1.plus(rot2).plus(rot3);
+    final var expected2 = new Rotation3d(0, Math.PI / 2, Math.PI / 2);
+    assertAll(
+        () -> assertEquals(expected2, result2),
+        () -> assertEquals(Math.PI / 2, result2.getZ() - result2.getX(), kEpsilon),
+        () -> assertEquals(Math.PI / 2, result2.getY(), kEpsilon));
+
+    rot1 = new Rotation3d(0, 0, Math.PI / 2);
+    rot2 = new Rotation3d(0, Math.PI / 3, 0);
+    rot3 = new Rotation3d(-Math.PI / 2, 0, 0);
+    final var result3 = rot1.plus(rot2).plus(rot3);
+    final var expected3 = new Rotation3d(0, Math.PI / 2, Math.PI / 6);
+    assertAll(
+        () -> assertEquals(expected3, result3),
+        () -> assertEquals(Math.PI / 6, result3.getZ() - result3.getX(), kEpsilon),
+        () -> assertEquals(Math.PI / 2, result3.getY(), kEpsilon));
+  }
+
+  @Test
+  void testInitAxisAngleAndRollPitchYaw() {
+    final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+    final var rot1 = new Rotation3d(xAxis, Math.PI / 3);
+    final var rot2 = new Rotation3d(Math.PI / 3, 0.0, 0.0);
+    final var rvec1 = new Rotation3d(xAxis.times(Math.PI / 3));
+    assertEquals(rot1, rot2);
+    assertEquals(rot1, rvec1);
+
+    final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+    final var rot3 = new Rotation3d(yAxis, Math.PI / 3);
+    final var rot4 = new Rotation3d(0.0, Math.PI / 3, 0.0);
+    final var rvec2 = new Rotation3d(yAxis.times(Math.PI / 3));
+    assertEquals(rot3, rot4);
+    assertEquals(rot3, rvec2);
+
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+    final var rot5 = new Rotation3d(zAxis, Math.PI / 3);
+    final var rot6 = new Rotation3d(0.0, 0.0, Math.PI / 3);
+    final var rvec3 = new Rotation3d(zAxis.times(Math.PI / 3));
+    assertEquals(rot5, rot6);
+    assertEquals(rot5, rvec3);
+  }
+
+  @Test
+  void testInitRotationMatrix() {
+    // No rotation
+    final var R1 = Matrix.eye(Nat.N3());
+    final var rot1 = new Rotation3d(R1);
+    assertEquals(Rotation3d.kZero, rot1);
+
+    // 90 degree CCW rotation around z-axis
+    final var R2 = new Matrix<>(Nat.N3(), Nat.N3());
+    R2.assignBlock(0, 0, VecBuilder.fill(0.0, 1.0, 0.0));
+    R2.assignBlock(0, 1, VecBuilder.fill(-1.0, 0.0, 0.0));
+    R2.assignBlock(0, 2, VecBuilder.fill(0.0, 0.0, 1.0));
+    final var rot2 = new Rotation3d(R2);
+    final var expected2 = new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0));
+    assertEquals(expected2, rot2);
+
+    // Matrix that isn't orthogonal
+    final var R3 = MatBuilder.fill(Nat.N3(), Nat.N3(), 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0);
+    assertThrows(IllegalArgumentException.class, () -> new Rotation3d(R3));
+
+    // Matrix that's orthogonal but not special orthogonal
+    final var R4 = Matrix.eye(Nat.N3()).times(2.0);
+    assertThrows(IllegalArgumentException.class, () -> new Rotation3d(R4));
+  }
+
+  @Test
+  void testInitTwoVector() {
+    final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+    final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    // 90 degree CW rotation around y-axis
+    final var rot1 = new Rotation3d(xAxis, zAxis);
+    final var expected1 = new Rotation3d(yAxis, -Math.PI / 2.0);
+    assertEquals(expected1, rot1);
+
+    // 45 degree CCW rotation around z-axis
+    final var rot2 = new Rotation3d(xAxis, VecBuilder.fill(1.0, 1.0, 0.0));
+    final var expected2 = new Rotation3d(zAxis, Math.PI / 4.0);
+    assertEquals(expected2, rot2);
+
+    // 0 degree rotation of x-axes
+    final var rot3 = new Rotation3d(xAxis, xAxis);
+    assertEquals(Rotation3d.kZero, rot3);
+
+    // 0 degree rotation of y-axes
+    final var rot4 = new Rotation3d(yAxis, yAxis);
+    assertEquals(Rotation3d.kZero, rot4);
+
+    // 0 degree rotation of z-axes
+    final var rot5 = new Rotation3d(zAxis, zAxis);
+    assertEquals(Rotation3d.kZero, rot5);
+
+    // 180 degree rotation tests. For 180 degree rotations, any quaternion with
+    // an orthogonal rotation axis is acceptable. The rotation axis and initial
+    // vector are orthogonal if their dot product is zero.
+
+    // 180 degree rotation of x-axes
+    final var rot6 = new Rotation3d(xAxis, xAxis.times(-1.0));
+    final var q6 = rot6.getQuaternion();
+    assertEquals(0.0, q6.getW());
+    assertEquals(
+        0.0,
+        q6.getX() * xAxis.get(0, 0) + q6.getY() * xAxis.get(1, 0) + q6.getZ() * xAxis.get(2, 0));
+
+    // 180 degree rotation of y-axes
+    final var rot7 = new Rotation3d(yAxis, yAxis.times(-1.0));
+    final var q7 = rot7.getQuaternion();
+    assertEquals(0.0, q7.getW());
+    assertEquals(
+        0.0,
+        q7.getX() * yAxis.get(0, 0) + q7.getY() * yAxis.get(1, 0) + q7.getZ() * yAxis.get(2, 0));
+
+    // 180 degree rotation of z-axes
+    final var rot8 = new Rotation3d(zAxis, zAxis.times(-1.0));
+    final var q8 = rot8.getQuaternion();
+    assertEquals(0.0, q8.getW());
+    assertEquals(
+        0.0,
+        q8.getX() * zAxis.get(0, 0) + q8.getY() * zAxis.get(1, 0) + q8.getZ() * zAxis.get(2, 0));
+  }
+
+  @Test
+  void testRadiansToDegrees() {
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var rot1 = new Rotation3d(zAxis, Math.PI / 3);
+    assertAll(
+        () -> assertEquals(Units.degreesToRadians(0.0), rot1.getX(), kEpsilon),
+        () -> assertEquals(Units.degreesToRadians(0.0), rot1.getY(), kEpsilon),
+        () -> assertEquals(Units.degreesToRadians(60.0), rot1.getZ(), kEpsilon));
+
+    var rot2 = new Rotation3d(zAxis, Math.PI / 4);
+    assertAll(
+        () -> assertEquals(Units.degreesToRadians(0.0), rot2.getX(), kEpsilon),
+        () -> assertEquals(Units.degreesToRadians(0.0), rot2.getY(), kEpsilon),
+        () -> assertEquals(Units.degreesToRadians(45.0), rot2.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testRadiansAndDegrees() {
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(45.0));
+    assertAll(
+        () -> assertEquals(0.0, rot1.getX(), kEpsilon),
+        () -> assertEquals(0.0, rot1.getY(), kEpsilon),
+        () -> assertEquals(Math.PI / 4.0, rot1.getZ(), kEpsilon));
+
+    var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(30.0));
+    assertAll(
+        () -> assertEquals(0.0, rot2.getX(), kEpsilon),
+        () -> assertEquals(0.0, rot2.getY(), kEpsilon),
+        () -> assertEquals(Math.PI / 6.0, rot2.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testRotationLoop() {
+    var rot = Rotation3d.kZero;
+
+    rot = rot.plus(new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0));
+    var expected = new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0);
+    assertEquals(expected, rot);
+
+    rot = rot.plus(new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0));
+    expected =
+        new Rotation3d(
+            VecBuilder.fill(1.0 / Math.sqrt(3), 1.0 / Math.sqrt(3), -1.0 / Math.sqrt(3)),
+            Units.degreesToRadians(120.0));
+    assertEquals(expected, rot);
+
+    rot = rot.plus(new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0)));
+    expected = new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0);
+    assertEquals(expected, rot);
+
+    rot = rot.plus(new Rotation3d(0.0, Units.degreesToRadians(-90.0), 0.0));
+    assertEquals(Rotation3d.kZero, rot);
+  }
+
+  @Test
+  void testRotateByFromZeroX() {
+    final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+
+    final var zero = Rotation3d.kZero;
+    var rotated = zero.rotateBy(new Rotation3d(xAxis, Units.degreesToRadians(90.0)));
+
+    var expected = new Rotation3d(xAxis, Units.degreesToRadians(90.0));
+    assertEquals(expected, rotated);
+  }
+
+  @Test
+  void testRotateByFromZeroY() {
+    final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+
+    final var zero = Rotation3d.kZero;
+    var rotated = zero.rotateBy(new Rotation3d(yAxis, Units.degreesToRadians(90.0)));
+
+    var expected = new Rotation3d(yAxis, Units.degreesToRadians(90.0));
+    assertEquals(expected, rotated);
+  }
+
+  @Test
+  void testRotateByFromZeroZ() {
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    final var zero = Rotation3d.kZero;
+    var rotated = zero.rotateBy(new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
+
+    var expected = new Rotation3d(zAxis, Units.degreesToRadians(90.0));
+    assertEquals(expected, rotated);
+  }
+
+  @Test
+  void testRotateByNonZeroX() {
+    final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+
+    var rot = new Rotation3d(xAxis, Units.degreesToRadians(90.0));
+    rot = rot.plus(new Rotation3d(xAxis, Units.degreesToRadians(30.0)));
+
+    var expected = new Rotation3d(xAxis, Units.degreesToRadians(120.0));
+    assertEquals(expected, rot);
+  }
+
+  @Test
+  void testRotateByNonZeroY() {
+    final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+
+    var rot = new Rotation3d(yAxis, Units.degreesToRadians(90.0));
+    rot = rot.plus(new Rotation3d(yAxis, Units.degreesToRadians(30.0)));
+
+    var expected = new Rotation3d(yAxis, Units.degreesToRadians(120.0));
+    assertEquals(expected, rot);
+  }
+
+  @Test
+  void testRotateByNonZeroZ() {
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var rot = new Rotation3d(zAxis, Units.degreesToRadians(90.0));
+    rot = rot.plus(new Rotation3d(zAxis, Units.degreesToRadians(30.0)));
+
+    var expected = new Rotation3d(zAxis, Units.degreesToRadians(120.0));
+    assertEquals(expected, rot);
+  }
+
+  @Test
+  void testMinus() {
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(70.0));
+    var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(30.0));
+
+    assertEquals(rot1.minus(rot2).getZ(), Units.degreesToRadians(40.0), kEpsilon);
+  }
+
+  @Test
+  void testEquality() {
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(43.0));
+    var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(43.0));
+    assertEquals(rot1, rot2);
+
+    rot1 = new Rotation3d(zAxis, Units.degreesToRadians(-180.0));
+    rot2 = new Rotation3d(zAxis, Units.degreesToRadians(180.0));
+    assertEquals(rot1, rot2);
+  }
+
+  @Test
+  void testAxisAngle() {
+    final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+    final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var rot1 = new Rotation3d(xAxis, Units.degreesToRadians(90.0));
+    assertEquals(xAxis, rot1.getAxis());
+    assertEquals(Math.PI / 2.0, rot1.getAngle(), 1e-9);
+
+    var rot2 = new Rotation3d(yAxis, Units.degreesToRadians(45.0));
+    assertEquals(yAxis, rot2.getAxis());
+    assertEquals(Math.PI / 4.0, rot2.getAngle(), 1e-9);
+
+    var rot3 = new Rotation3d(zAxis, Units.degreesToRadians(60.0));
+    assertEquals(zAxis, rot3.getAxis());
+    assertEquals(Math.PI / 3.0, rot3.getAngle(), 1e-9);
+  }
+
+  @Test
+  void testToRotation2d() {
+    var rotation =
+        new Rotation3d(
+            Units.degreesToRadians(20.0),
+            Units.degreesToRadians(30.0),
+            Units.degreesToRadians(40.0));
+    var expected = new Rotation2d(Units.degreesToRadians(40.0));
+
+    assertEquals(expected, rotation.toRotation2d());
+  }
+
+  @Test
+  void testInequality() {
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(43.0));
+    var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(43.5));
+    assertNotEquals(rot1, rot2);
+  }
+
+  @Test
+  void testToMatrix() {
+    var before =
+        new Rotation3d(
+            Units.degreesToRadians(10.0),
+            Units.degreesToRadians(20.0),
+            Units.degreesToRadians(30.0));
+    var after = new Rotation3d(before.toMatrix());
+
+    assertEquals(before, after);
+  }
+
+  @Test
+  void testInterpolate() {
+    final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+    final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    // 50 + (70 - 50) * 0.5 = 60
+    var rot1 = new Rotation3d(xAxis, Units.degreesToRadians(50));
+    var rot2 = new Rotation3d(xAxis, Units.degreesToRadians(70));
+    var interpolated = rot1.interpolate(rot2, 0.5);
+    assertEquals(Units.degreesToRadians(60.0), interpolated.getX(), kEpsilon);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getZ(), kEpsilon);
+
+    // -160 minus half distance between 170 and -160 (15) = -175
+    rot1 = new Rotation3d(xAxis, Units.degreesToRadians(170));
+    rot2 = new Rotation3d(xAxis, Units.degreesToRadians(-160));
+    interpolated = rot1.interpolate(rot2, 0.5);
+    assertEquals(Units.degreesToRadians(-175.0), interpolated.getX(), kEpsilon);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getZ(), kEpsilon);
+
+    // 50 + (70 - 50) * 0.5 = 60
+    rot1 = new Rotation3d(yAxis, Units.degreesToRadians(50));
+    rot2 = new Rotation3d(yAxis, Units.degreesToRadians(70));
+    interpolated = rot1.interpolate(rot2, 0.5);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon);
+    assertEquals(Units.degreesToRadians(60.0), interpolated.getY(), kEpsilon);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getZ(), kEpsilon);
+
+    // -160 minus half distance between 170 and -160 (165) = 5
+    rot1 = new Rotation3d(yAxis, Units.degreesToRadians(170));
+    rot2 = new Rotation3d(yAxis, Units.degreesToRadians(-160));
+    interpolated = rot1.interpolate(rot2, 0.5);
+    assertEquals(Units.degreesToRadians(180.0), interpolated.getX(), kEpsilon);
+    assertEquals(Units.degreesToRadians(-5.0), interpolated.getY(), kEpsilon);
+    assertEquals(Units.degreesToRadians(180.0), interpolated.getZ(), kEpsilon);
+
+    // 50 + (70 - 50) * 0.5 = 60
+    rot1 = new Rotation3d(zAxis, Units.degreesToRadians(50));
+    rot2 = new Rotation3d(zAxis, Units.degreesToRadians(70));
+    interpolated = rot1.interpolate(rot2, 0.5);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
+    assertEquals(Units.degreesToRadians(60.0), interpolated.getZ(), kEpsilon);
+
+    // -160 minus half distance between 170 and -160 (15) = -175
+    rot1 = new Rotation3d(zAxis, Units.degreesToRadians(170));
+    rot2 = new Rotation3d(zAxis, Units.degreesToRadians(-160));
+    interpolated = rot1.interpolate(rot2, 0.5);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
+    assertEquals(Units.degreesToRadians(-175.0), interpolated.getZ(), kEpsilon);
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/Transform2dTest.java

@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.units.Units;
+import org.junit.jupiter.api.Test;
+
+class Transform2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testNewWithMeasures() {
+    var transform =
+        new Transform2d(Units.Inches.of(6), Units.Inches.of(8), Rotation2d.fromDegrees(45));
+
+    assertEquals(0.1524, transform.getX(), kEpsilon);
+    assertEquals(0.2032, transform.getY(), kEpsilon);
+    assertEquals(Math.PI / 4, transform.getRotation().getRadians(), kEpsilon);
+  }
+
+  @Test
+  void testToMatrix() {
+    var before = new Transform2d(1.0, 2.0, Rotation2d.fromDegrees(20.0));
+    var after = new Transform2d(before.toMatrix());
+
+    assertEquals(before, after);
+  }
+
+  @Test
+  void testInverse() {
+    var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
+    var transform = new Transform2d(new Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0));
+
+    var transformed = initial.plus(transform);
+    var untransformed = transformed.plus(transform.inverse());
+
+    assertEquals(initial, untransformed);
+  }
+
+  @Test
+  void testComposition() {
+    var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
+    var transform1 = new Transform2d(new Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0));
+    var transform2 = new Transform2d(new Translation2d(0.0, 2.0), Rotation2d.fromDegrees(5.0));
+
+    var transformedSeparate = initial.plus(transform1).plus(transform2);
+    var transformedCombined = initial.plus(transform1.plus(transform2));
+
+    assertEquals(transformedSeparate, transformedCombined);
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/Transform3dTest.java

@@ -0,0 +1,66 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class Transform3dTest {
+  @Test
+  void testToMatrix() {
+    var before =
+        new Transform3d(
+            1.0,
+            2.0,
+            3.0,
+            new Rotation3d(
+                Units.degreesToRadians(20.0),
+                Units.degreesToRadians(30.0),
+                Units.degreesToRadians(40.0)));
+    var after = new Transform3d(before.toMatrix());
+
+    assertEquals(before, after);
+  }
+
+  @Test
+  void testInverse() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var initial =
+        new Pose3d(
+            new Translation3d(1.0, 2.0, 3.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+    var transform =
+        new Transform3d(
+            new Translation3d(5.0, 4.0, 3.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0)));
+
+    var transformed = initial.plus(transform);
+    var untransformed = transformed.plus(transform.inverse());
+
+    assertEquals(initial, untransformed);
+  }
+
+  @Test
+  void testComposition() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var initial =
+        new Pose3d(
+            new Translation3d(1.0, 2.0, 3.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+    var transform1 =
+        new Transform3d(
+            new Translation3d(5.0, 0.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0)));
+    var transform2 =
+        new Transform3d(
+            new Translation3d(0.0, 2.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0)));
+
+    var transformedSeparate = initial.plus(transform1).plus(transform2);
+    var transformedCombined = initial.plus(transform1.plus(transform2));
+
+    assertEquals(transformedSeparate, transformedCombined);
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/Translation2dTest.java

@@ -0,0 +1,184 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+import edu.wpi.first.math.VecBuilder;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class Translation2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testSum() {
+    var one = new Translation2d(1.0, 3.0);
+    var two = new Translation2d(2.0, 5.0);
+
+    var sum = one.plus(two);
+
+    assertAll(
+        () -> assertEquals(3.0, sum.getX(), kEpsilon),
+        () -> assertEquals(8.0, sum.getY(), kEpsilon));
+  }
+
+  @Test
+  void testDifference() {
+    var one = new Translation2d(1.0, 3.0);
+    var two = new Translation2d(2.0, 5.0);
+
+    var difference = one.minus(two);
+
+    assertAll(
+        () -> assertEquals(-1.0, difference.getX(), kEpsilon),
+        () -> assertEquals(-2.0, difference.getY(), kEpsilon));
+  }
+
+  @Test
+  void testRotateBy() {
+    var another = new Translation2d(3.0, 0.0);
+    var rotated = another.rotateBy(Rotation2d.kCCW_Pi_2);
+
+    assertAll(
+        () -> assertEquals(0.0, rotated.getX(), kEpsilon),
+        () -> assertEquals(3.0, rotated.getY(), kEpsilon));
+  }
+
+  @Test
+  void testRotateAround() {
+    var original = new Translation2d(2.0, 1.0);
+    var other = new Translation2d(3.0, 2.0);
+    var rotated = original.rotateAround(other, Rotation2d.fromDegrees(180.0));
+
+    assertAll(
+        () -> assertEquals(4.0, rotated.getX(), kEpsilon),
+        () -> assertEquals(3.0, rotated.getY(), kEpsilon));
+  }
+
+  @Test
+  void testMultiplication() {
+    var original = new Translation2d(3.0, 5.0);
+    var mult = original.times(3);
+
+    assertAll(
+        () -> assertEquals(9.0, mult.getX(), kEpsilon),
+        () -> assertEquals(15.0, mult.getY(), kEpsilon));
+  }
+
+  @Test
+  void testDivision() {
+    var original = new Translation2d(3.0, 5.0);
+    var div = original.div(2);
+
+    assertAll(
+        () -> assertEquals(1.5, div.getX(), kEpsilon),
+        () -> assertEquals(2.5, div.getY(), kEpsilon));
+  }
+
+  @Test
+  void testNorm() {
+    var one = new Translation2d(3.0, 5.0);
+    assertEquals(Math.hypot(3.0, 5.0), one.getNorm(), kEpsilon);
+  }
+
+  @Test
+  void testSquaredNorm() {
+    var one = new Translation2d(3.0, 5.0);
+    assertEquals(34.0, one.getSquaredNorm(), kEpsilon);
+  }
+
+  @Test
+  void testDistance() {
+    var one = new Translation2d(1, 1);
+    var two = new Translation2d(6, 6);
+    assertEquals(5.0 * Math.sqrt(2.0), one.getDistance(two), kEpsilon);
+  }
+
+  @Test
+  void testSquaredDistance() {
+    var one = new Translation2d(1, 1);
+    var two = new Translation2d(6, 6);
+    assertEquals(50.0, one.getSquaredDistance(two), kEpsilon);
+  }
+
+  @Test
+  void testUnaryMinus() {
+    var original = new Translation2d(-4.5, 7);
+    var inverted = original.unaryMinus();
+
+    assertAll(
+        () -> assertEquals(4.5, inverted.getX(), kEpsilon),
+        () -> assertEquals(-7.0, inverted.getY(), kEpsilon));
+  }
+
+  @Test
+  void testEquality() {
+    var one = new Translation2d(9, 5.5);
+    var two = new Translation2d(9, 5.5);
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = new Translation2d(9, 5.5);
+    var two = new Translation2d(9, 5.7);
+    assertNotEquals(one, two);
+  }
+
+  @Test
+  void testPolarConstructor() {
+    var one = new Translation2d(Math.sqrt(2), Rotation2d.fromDegrees(45.0));
+    var two = new Translation2d(2, Rotation2d.fromDegrees(60.0));
+    assertAll(
+        () -> assertEquals(1.0, one.getX(), kEpsilon),
+        () -> assertEquals(1.0, one.getY(), kEpsilon),
+        () -> assertEquals(1.0, two.getX(), kEpsilon),
+        () -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon));
+  }
+
+  @Test
+  void testNearest() {
+    var origin = Translation2d.kZero;
+
+    // each translationX is X units away from the origin at a random angle.
+    var translation1 = new Translation2d(1, Rotation2d.fromDegrees(45));
+    var translation2 = new Translation2d(2, Rotation2d.kCCW_Pi_2);
+    var translation3 = new Translation2d(3, Rotation2d.fromDegrees(135));
+    var translation4 = new Translation2d(4, Rotation2d.kPi);
+    var translation5 = new Translation2d(5, Rotation2d.kCW_Pi_2);
+
+    assertEquals(origin.nearest(List.of(translation5, translation3, translation4)), translation3);
+    assertEquals(origin.nearest(List.of(translation1, translation2, translation3)), translation1);
+    assertEquals(origin.nearest(List.of(translation4, translation2, translation3)), translation2);
+  }
+
+  @Test
+  void testToVector() {
+    var vec = VecBuilder.fill(1.0, 2.0);
+    var translation = new Translation2d(vec);
+
+    assertEquals(vec.get(0), translation.getX());
+    assertEquals(vec.get(1), translation.getY());
+
+    assertEquals(vec, translation.toVector());
+  }
+
+  @Test
+  void testDot() {
+    var one = new Translation2d(2.0, 3.0);
+    var two = new Translation2d(3.0, 4.0);
+    assertEquals(18.0, one.dot(two), kEpsilon);
+  }
+
+  @Test
+  void testCross() {
+    var one = new Translation2d(2.0, 3.0);
+    var two = new Translation2d(3.0, 4.0);
+    assertEquals(-1.0, one.cross(two), kEpsilon);
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/Translation3dTest.java

@@ -0,0 +1,260 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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.Inches;
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.util.Units;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class Translation3dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testNewWithMeasures() {
+    var translation = new Translation3d(Inches.of(6), Inches.of(8), Inches.of(16));
+
+    assertEquals(0.1524, translation.getX(), kEpsilon);
+    assertEquals(0.2032, translation.getY(), kEpsilon);
+    assertEquals(0.4064, translation.getZ(), kEpsilon);
+  }
+
+  @Test
+  void testSum() {
+    var one = new Translation3d(1.0, 3.0, 5.0);
+    var two = new Translation3d(2.0, 5.0, 8.0);
+
+    var sum = one.plus(two);
+
+    assertAll(
+        () -> assertEquals(3.0, sum.getX(), kEpsilon),
+        () -> assertEquals(8.0, sum.getY(), kEpsilon),
+        () -> assertEquals(13.0, sum.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testDifference() {
+    var one = new Translation3d(1.0, 3.0, 5.0);
+    var two = new Translation3d(2.0, 5.0, 8.0);
+
+    var difference = one.minus(two);
+
+    assertAll(
+        () -> assertEquals(-1.0, difference.getX(), kEpsilon),
+        () -> assertEquals(-2.0, difference.getY(), kEpsilon),
+        () -> assertEquals(-3.0, difference.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testRotateBy() {
+    var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+    var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var translation = new Translation3d(1.0, 2.0, 3.0);
+
+    var rotated1 = translation.rotateBy(new Rotation3d(xAxis, Units.degreesToRadians(90.0)));
+    assertAll(
+        () -> assertEquals(1.0, rotated1.getX(), kEpsilon),
+        () -> assertEquals(-3.0, rotated1.getY(), kEpsilon),
+        () -> assertEquals(2.0, rotated1.getZ(), kEpsilon));
+
+    var rotated2 = translation.rotateBy(new Rotation3d(yAxis, Units.degreesToRadians(90.0)));
+    assertAll(
+        () -> assertEquals(3.0, rotated2.getX(), kEpsilon),
+        () -> assertEquals(2.0, rotated2.getY(), kEpsilon),
+        () -> assertEquals(-1.0, rotated2.getZ(), kEpsilon));
+
+    var rotated3 = translation.rotateBy(new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
+    assertAll(
+        () -> assertEquals(-2.0, rotated3.getX(), kEpsilon),
+        () -> assertEquals(1.0, rotated3.getY(), kEpsilon),
+        () -> assertEquals(3.0, rotated3.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testRotateAround() {
+    var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+    var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var translation = new Translation3d(1.0, 2.0, 3.0);
+    var around = new Translation3d(3.0, 2.0, 1.0);
+
+    var rotated1 =
+        translation.rotateAround(around, new Rotation3d(xAxis, Units.degreesToRadians(90.0)));
+
+    assertAll(
+        () -> assertEquals(1.0, rotated1.getX(), kEpsilon),
+        () -> assertEquals(0.0, rotated1.getY(), kEpsilon),
+        () -> assertEquals(1.0, rotated1.getZ(), kEpsilon));
+
+    var rotated2 =
+        translation.rotateAround(around, new Rotation3d(yAxis, Units.degreesToRadians(90.0)));
+
+    assertAll(
+        () -> assertEquals(5.0, rotated2.getX(), kEpsilon),
+        () -> assertEquals(2.0, rotated2.getY(), kEpsilon),
+        () -> assertEquals(3.0, rotated2.getZ(), kEpsilon));
+
+    var rotated3 =
+        translation.rotateAround(around, new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
+
+    assertAll(
+        () -> assertEquals(3.0, rotated3.getX(), kEpsilon),
+        () -> assertEquals(0.0, rotated3.getY(), kEpsilon),
+        () -> assertEquals(3.0, rotated3.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testToTranslation2d() {
+    var translation = new Translation3d(1.0, 2.0, 3.0);
+    var expected = new Translation2d(1.0, 2.0);
+
+    assertEquals(expected, translation.toTranslation2d());
+  }
+
+  @Test
+  void testMultiplication() {
+    var original = new Translation3d(3.0, 5.0, 7.0);
+    var mult = original.times(3);
+
+    assertAll(
+        () -> assertEquals(9.0, mult.getX(), kEpsilon),
+        () -> assertEquals(15.0, mult.getY(), kEpsilon),
+        () -> assertEquals(21.0, mult.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testDivision() {
+    var original = new Translation3d(3.0, 5.0, 7.0);
+    var div = original.div(2);
+
+    assertAll(
+        () -> assertEquals(1.5, div.getX(), kEpsilon),
+        () -> assertEquals(2.5, div.getY(), kEpsilon),
+        () -> assertEquals(3.5, div.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testNorm() {
+    var one = new Translation3d(3.0, 5.0, 7.0);
+    assertEquals(Math.sqrt(83.0), one.getNorm(), kEpsilon);
+  }
+
+  @Test
+  void testSquaredNorm() {
+    var one = new Translation3d(3.0, 5.0, 7.0);
+    assertEquals(83.0, one.getSquaredNorm(), kEpsilon);
+  }
+
+  @Test
+  void testDistance() {
+    var one = new Translation3d(1.0, 1.0, 1.0);
+    var two = new Translation3d(6.0, 6.0, 6.0);
+    assertEquals(5.0 * Math.sqrt(3.0), one.getDistance(two), kEpsilon);
+  }
+
+  @Test
+  void testSquaredDistance() {
+    var one = new Translation3d(1.0, 1.0, 1.0);
+    var two = new Translation3d(6.0, 6.0, 6.0);
+    assertEquals(75.0, one.getSquaredDistance(two), kEpsilon);
+  }
+
+  @Test
+  void testUnaryMinus() {
+    var original = new Translation3d(-4.5, 7.0, 9.0);
+    var inverted = original.unaryMinus();
+
+    assertAll(
+        () -> assertEquals(4.5, inverted.getX(), kEpsilon),
+        () -> assertEquals(-7.0, inverted.getY(), kEpsilon),
+        () -> assertEquals(-9.0, inverted.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testEquality() {
+    var one = new Translation3d(9, 5.5, 3.5);
+    var two = new Translation3d(9, 5.5, 3.5);
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = new Translation3d(9, 5.5, 3.5);
+    var two = new Translation3d(9, 5.7, 3.5);
+    assertNotEquals(one, two);
+  }
+
+  @Test
+  void testPolarConstructor() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var one = new Translation3d(Math.sqrt(2), new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+    var two = new Translation3d(2, new Rotation3d(zAxis, Units.degreesToRadians(60.0)));
+    assertAll(
+        () -> assertEquals(1.0, one.getX(), kEpsilon),
+        () -> assertEquals(1.0, one.getY(), kEpsilon),
+        () -> assertEquals(0.0, one.getZ(), kEpsilon),
+        () -> assertEquals(1.0, two.getX(), kEpsilon),
+        () -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon),
+        () -> assertEquals(0.0, two.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testToVector() {
+    var vec = VecBuilder.fill(1.0, 2.0, 3.0);
+    var translation = new Translation3d(vec);
+
+    assertEquals(vec.get(0), translation.getX());
+    assertEquals(vec.get(1), translation.getY());
+    assertEquals(vec.get(2), translation.getZ());
+
+    assertEquals(vec, translation.toVector());
+  }
+
+  @Test
+  void testNearest() {
+    var origin = Translation3d.kZero;
+
+    // Distance sort
+    // translations are in order of closest to farthest away from the origin at various positions
+    // in 3D space.
+    final var translation1 = new Translation3d(1, 0, 0);
+    final var translation2 = new Translation3d(0, 2, 0);
+    final var translation3 = new Translation3d(0, 0, 3);
+    final var translation4 = new Translation3d(2, 2, 2);
+    final var translation5 = new Translation3d(3, 3, 3);
+
+    assertEquals(translation3, origin.nearest(List.of(translation5, translation3, translation4)));
+    assertEquals(translation1, origin.nearest(List.of(translation1, translation2, translation3)));
+    assertEquals(translation2, origin.nearest(List.of(translation4, translation2, translation3)));
+  }
+
+  @Test
+  void testDot() {
+    var one = new Translation3d(1.0, 2.0, 3.0);
+    var two = new Translation3d(4.0, 5.0, 6.0);
+    assertEquals(32.0, one.dot(two));
+  }
+
+  @Test
+  void testCross() {
+    var one = new Translation3d(1.0, 2.0, 3.0);
+    var two = new Translation3d(4.0, 5.0, 6.0);
+
+    var cross = one.cross(two);
+    assertAll(
+        () -> assertEquals(-3.0, cross.get(0, 0), kEpsilon),
+        () -> assertEquals(6.0, cross.get(1, 0), kEpsilon),
+        () -> assertEquals(-3.0, cross.get(2, 0), kEpsilon));
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/Twist2dTest.java

@@ -0,0 +1,68 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+import org.junit.jupiter.api.Test;
+
+class Twist2dTest {
+  @Test
+  void testStraight() {
+    var straight = new Twist2d(5.0, 0.0, 0.0);
+    var straightPose = straight.exp();
+
+    var expected = new Transform2d(5.0, 0.0, Rotation2d.kZero);
+    assertEquals(expected, straightPose);
+  }
+
+  @Test
+  void testQuarterCircle() {
+    var quarterCircle = new Twist2d(5.0 / 2.0 * Math.PI, 0, Math.PI / 2.0);
+    var quarterCirclePose = quarterCircle.exp();
+
+    var expected = new Transform2d(5.0, 5.0, Rotation2d.kCCW_Pi_2);
+    assertEquals(expected, quarterCirclePose);
+  }
+
+  @Test
+  void testDiagonalNoDtheta() {
+    var diagonal = new Twist2d(2.0, 2.0, 0.0);
+    var diagonalPose = diagonal.exp();
+
+    var expected = new Transform2d(2.0, 2.0, Rotation2d.kZero);
+    assertEquals(expected, diagonalPose);
+  }
+
+  @Test
+  void testEquality() {
+    var one = new Twist2d(5, 1, 3);
+    var two = new Twist2d(5, 1, 3);
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = new Twist2d(5, 1, 3);
+    var two = new Twist2d(5, 1.2, 3);
+    assertNotEquals(one, two);
+  }
+
+  @Test
+  void testPose2dLog() {
+    final var start = Pose2d.kZero;
+    final var end = new Pose2d(5.0, 5.0, Rotation2d.kCCW_Pi_2);
+
+    final var twist = end.minus(start).log();
+
+    var expected = new Twist2d(5.0 / 2.0 * Math.PI, 0.0, Math.PI / 2.0);
+    assertEquals(expected, twist);
+
+    // Make sure computed twist gives back original end pose
+    final var reapplied = start.plus(twist.exp());
+    assertEquals(end, reapplied);
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/Twist3dTest.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.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class Twist3dTest {
+  @Test
+  void testStraightX() {
+    var straight = new Twist3d(5.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+    var straightPose = straight.exp();
+
+    var expected = new Transform3d(5.0, 0.0, 0.0, Rotation3d.kZero);
+    assertEquals(expected, straightPose);
+  }
+
+  @Test
+  void testStraightY() {
+    var straight = new Twist3d(0.0, 5.0, 0.0, 0.0, 0.0, 0.0);
+    var straightPose = straight.exp();
+
+    var expected = new Transform3d(0.0, 5.0, 0.0, Rotation3d.kZero);
+    assertEquals(expected, straightPose);
+  }
+
+  @Test
+  void testStraightZ() {
+    var straight = new Twist3d(0.0, 0.0, 5.0, 0.0, 0.0, 0.0);
+    var straightPose = straight.exp();
+
+    var expected = new Transform3d(0.0, 0.0, 5.0, Rotation3d.kZero);
+    assertEquals(expected, straightPose);
+  }
+
+  @Test
+  void testQuarterCircle() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var quarterCircle = new Twist3d(5.0 / 2.0 * Math.PI, 0.0, 0.0, 0.0, 0.0, Math.PI / 2.0);
+    var quarterCirclePose = quarterCircle.exp();
+
+    var expected =
+        new Transform3d(5.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
+    assertEquals(expected, quarterCirclePose);
+  }
+
+  @Test
+  void testDiagonalNoDtheta() {
+    var diagonal = new Twist3d(2.0, 2.0, 0.0, 0.0, 0.0, 0.0);
+    var diagonalPose = diagonal.exp();
+
+    var expected = new Transform3d(2.0, 2.0, 0.0, Rotation3d.kZero);
+    assertEquals(expected, diagonalPose);
+  }
+
+  @Test
+  void testEquality() {
+    var one = new Twist3d(5, 1, 0, 0.0, 0.0, 3.0);
+    var two = new Twist3d(5, 1, 0, 0.0, 0.0, 3.0);
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = new Twist3d(5, 1, 0, 0.0, 0.0, 3.0);
+    var two = new Twist3d(5, 1.2, 0, 0.0, 0.0, 3.0);
+    assertNotEquals(one, two);
+  }
+
+  @Test
+  void testPose3dLogX() {
+    final var start = Pose3d.kZero;
+    final var end =
+        new Pose3d(0.0, 5.0, 5.0, new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0));
+
+    final var twist = end.minus(start).log();
+
+    var expected =
+        new Twist3d(0.0, 5.0 / 2.0 * Math.PI, 0.0, Units.degreesToRadians(90.0), 0.0, 0.0);
+    assertEquals(expected, twist);
+
+    // Make sure computed twist gives back original end pose
+    final var reapplied = start.plus(twist.exp());
+    assertEquals(end, reapplied);
+  }
+
+  @Test
+  void testPose3dLogY() {
+    final var start = Pose3d.kZero;
+    final var end =
+        new Pose3d(5.0, 0.0, 5.0, new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0));
+
+    final var twist = end.minus(start).log();
+
+    var expected = new Twist3d(0.0, 0.0, 5.0 / 2.0 * Math.PI, 0.0, Math.PI / 2.0, 0.0);
+    assertEquals(expected, twist);
+
+    // Make sure computed twist gives back original end pose
+    final var reapplied = start.plus(twist.exp());
+    assertEquals(end, reapplied);
+  }
+
+  @Test
+  void testPose3dLogZ() {
+    final var start = Pose3d.kZero;
+    final var end =
+        new Pose3d(5.0, 5.0, 0.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0)));
+
+    final var twist = end.minus(start).log();
+
+    var expected = new Twist3d(5.0 / 2.0 * Math.PI, 0.0, 0.0, 0.0, 0.0, Math.PI / 2.0);
+    assertEquals(expected, twist);
+
+    // Make sure computed twist gives back original end pose
+    final var reapplied = start.plus(twist.exp());
+    assertEquals(end, reapplied);
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/proto/Ellipse2dProtoTest.java

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Ellipse2d;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufEllipse2d;
+import org.junit.jupiter.api.Test;
+
+class Ellipse2dProtoTest {
+  private static final Ellipse2d DATA =
+      new Ellipse2d(new Pose2d(1.0, 3.6, new Rotation2d(4.1)), 2.0, 1.0);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufEllipse2d proto = Ellipse2d.proto.createMessage();
+    Ellipse2d.proto.pack(proto, DATA);
+
+    Ellipse2d data = Ellipse2d.proto.unpack(proto);
+    assertEquals(DATA.getCenter(), data.getCenter());
+    assertEquals(DATA.getXSemiAxis(), data.getXSemiAxis());
+    assertEquals(DATA.getYSemiAxis(), data.getYSemiAxis());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/proto/Pose2dProtoTest.java

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d;
+import org.junit.jupiter.api.Test;
+
+class Pose2dProtoTest {
+  private static final Pose2d DATA =
+      new Pose2d(new Translation2d(0.191, 2.2), new Rotation2d(22.9));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufPose2d proto = Pose2d.proto.createMessage();
+    Pose2d.proto.pack(proto, DATA);
+
+    Pose2d data = Pose2d.proto.unpack(proto);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/proto/Pose3dProtoTest.java

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d;
+import org.junit.jupiter.api.Test;
+
+class Pose3dProtoTest {
+  private static final Pose3d DATA =
+      new Pose3d(
+          new Translation3d(1.1, 2.2, 1.1),
+          new Rotation3d(new Quaternion(1.91, 0.3504, 3.3, 1.74)));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufPose3d proto = Pose3d.proto.createMessage();
+    Pose3d.proto.pack(proto, DATA);
+
+    Pose3d data = Pose3d.proto.unpack(proto);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/proto/QuaternionProtoTest.java

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion;
+import org.junit.jupiter.api.Test;
+
+class QuaternionProtoTest {
+  private static final Quaternion DATA = new Quaternion(1.1, 0.191, 35.04, 19.1);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufQuaternion proto = Quaternion.proto.createMessage();
+    Quaternion.proto.pack(proto, DATA);
+
+    Quaternion data = Quaternion.proto.unpack(proto);
+    assertEquals(DATA.getW(), data.getW());
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+    assertEquals(DATA.getZ(), data.getZ());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/proto/Rectangle2dProtoTest.java

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rectangle2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufRectangle2d;
+import org.junit.jupiter.api.Test;
+
+class Rectangle2dProtoTest {
+  private static final Rectangle2d DATA =
+      new Rectangle2d(new Pose2d(0.1, 0.2, new Rotation2d()), 4.0, 3.0);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufRectangle2d proto = Rectangle2d.proto.createMessage();
+    Rectangle2d.proto.pack(proto, DATA);
+
+    Rectangle2d data = Rectangle2d.proto.unpack(proto);
+    assertEquals(DATA.getCenter(), data.getCenter());
+    assertEquals(DATA.getXWidth(), data.getXWidth());
+    assertEquals(DATA.getYWidth(), data.getYWidth());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/proto/Rotation2dProtoTest.java

@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d;
+import org.junit.jupiter.api.Test;
+
+class Rotation2dProtoTest {
+  private static final Rotation2d DATA = new Rotation2d(1.91);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufRotation2d proto = Rotation2d.proto.createMessage();
+    Rotation2d.proto.pack(proto, DATA);
+
+    Rotation2d data = Rotation2d.proto.unpack(proto);
+    assertEquals(DATA.getRadians(), data.getRadians());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/proto/Rotation3dProtoTest.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.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufRotation3d;
+import org.junit.jupiter.api.Test;
+
+class Rotation3dProtoTest {
+  private static final Rotation3d DATA = new Rotation3d(new Quaternion(2.29, 0.191, 0.191, 17.4));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufRotation3d proto = Rotation3d.proto.createMessage();
+    Rotation3d.proto.pack(proto, DATA);
+
+    Rotation3d data = Rotation3d.proto.unpack(proto);
+    assertEquals(DATA.getQuaternion(), data.getQuaternion());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/proto/Transform2dProtoTest.java

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+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 org.junit.jupiter.api.Test;
+
+class Transform2dProtoTest {
+  private static final Transform2d DATA =
+      new Transform2d(new Translation2d(0.191, 2.2), new Rotation2d(4.4));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTransform2d proto = Transform2d.proto.createMessage();
+    Transform2d.proto.pack(proto, DATA);
+
+    Transform2d data = Transform2d.proto.unpack(proto);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/proto/Transform3dProtoTest.java

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+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 org.junit.jupiter.api.Test;
+
+class Transform3dProtoTest {
+  private static final Transform3d DATA =
+      new Transform3d(
+          new Translation3d(0.3504, 22.9, 3.504),
+          new Rotation3d(new Quaternion(0.3504, 35.04, 2.29, 0.3504)));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTransform3d proto = Transform3d.proto.createMessage();
+    Transform3d.proto.pack(proto, DATA);
+
+    Transform3d data = Transform3d.proto.unpack(proto);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/proto/Translation2dProtoTest.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.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d;
+import org.junit.jupiter.api.Test;
+
+class Translation2dProtoTest {
+  private static final Translation2d DATA = new Translation2d(3.504, 22.9);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTranslation2d proto = Translation2d.proto.createMessage();
+    Translation2d.proto.pack(proto, DATA);
+
+    Translation2d data = Translation2d.proto.unpack(proto);
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/proto/Translation3dProtoTest.java

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d;
+import org.junit.jupiter.api.Test;
+
+class Translation3dProtoTest {
+  private static final Translation3d DATA = new Translation3d(35.04, 22.9, 3.504);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTranslation3d proto = Translation3d.proto.createMessage();
+    Translation3d.proto.pack(proto, DATA);
+
+    Translation3d data = Translation3d.proto.unpack(proto);
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+    assertEquals(DATA.getZ(), data.getZ());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/proto/Twist2dProtoTest.java

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTwist2d;
+import org.junit.jupiter.api.Test;
+
+class Twist2dProtoTest {
+  private static final Twist2d DATA = new Twist2d(2.29, 35.04, 35.04);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTwist2d proto = Twist2d.proto.createMessage();
+    Twist2d.proto.pack(proto, DATA);
+
+    Twist2d data = Twist2d.proto.unpack(proto);
+    assertEquals(DATA.dx, data.dx);
+    assertEquals(DATA.dy, data.dy);
+    assertEquals(DATA.dtheta, data.dtheta);
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/proto/Twist3dProtoTest.java

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Twist3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTwist3d;
+import org.junit.jupiter.api.Test;
+
+class Twist3dProtoTest {
+  private static final Twist3d DATA = new Twist3d(1.1, 2.29, 35.04, 0.174, 19.1, 4.4);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTwist3d proto = Twist3d.proto.createMessage();
+    Twist3d.proto.pack(proto, DATA);
+
+    Twist3d data = Twist3d.proto.unpack(proto);
+    assertEquals(DATA.dx, data.dx);
+    assertEquals(DATA.dy, data.dy);
+    assertEquals(DATA.dz, data.dz);
+    assertEquals(DATA.rx, data.rx);
+    assertEquals(DATA.ry, data.ry);
+    assertEquals(DATA.rz, data.rz);
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/struct/Ellipse2dStructTest.java

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Ellipse2d;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Ellipse2dStructTest {
+  private static final Ellipse2d DATA =
+      new Ellipse2d(new Pose2d(0.0, 1.0, new Rotation2d(2.4)), 3.0, 4.0);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Ellipse2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Ellipse2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Ellipse2d data = Ellipse2d.struct.unpack(buffer);
+    assertEquals(DATA.getCenter(), data.getCenter());
+    assertEquals(DATA.getXSemiAxis(), data.getXSemiAxis());
+    assertEquals(DATA.getYSemiAxis(), data.getYSemiAxis());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/struct/Pose2dStructTest.java

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Pose2dStructTest {
+  private static final Pose2d DATA =
+      new Pose2d(new Translation2d(0.191, 2.2), new Rotation2d(22.9));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Pose2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Pose2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Pose2d data = Pose2d.struct.unpack(buffer);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/struct/Pose3dStructTest.java

@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Pose3dStructTest {
+  private static final Pose3d DATA =
+      new Pose3d(
+          new Translation3d(1.1, 2.2, 1.1),
+          new Rotation3d(new Quaternion(1.91, 0.3504, 3.3, 1.74)));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Pose3d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Pose3d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Pose3d data = Pose3d.struct.unpack(buffer);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/struct/QuaternionStructTest.java

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class QuaternionStructTest {
+  private static final Quaternion DATA = new Quaternion(1.1, 0.191, 35.04, 19.1);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Quaternion.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Quaternion.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Quaternion data = Quaternion.struct.unpack(buffer);
+    assertEquals(DATA.getW(), data.getW());
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+    assertEquals(DATA.getZ(), data.getZ());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/struct/Rectangle2dStructTest.java

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rectangle2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Rectangle2dStructTest {
+  private static final Rectangle2d DATA =
+      new Rectangle2d(new Pose2d(1.0, 2.0, new Rotation2d(3.1)), 5.0, 3.0);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Rectangle2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Rectangle2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Rectangle2d data = Rectangle2d.struct.unpack(buffer);
+    assertEquals(DATA.getCenter(), data.getCenter());
+    assertEquals(DATA.getXWidth(), data.getXWidth());
+    assertEquals(DATA.getYWidth(), data.getYWidth());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/struct/Rotation2dStructTest.java

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Rotation2dStructTest {
+  private static final Rotation2d DATA = new Rotation2d(1.91);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Rotation2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Rotation2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Rotation2d data = Rotation2d.struct.unpack(buffer);
+    assertEquals(DATA.getRadians(), data.getRadians());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/struct/Rotation3dStructTest.java

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Rotation3dStructTest {
+  private static final Rotation3d DATA = new Rotation3d(new Quaternion(2.29, 0.191, 0.191, 17.4));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Rotation3d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Rotation3d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Rotation3d data = Rotation3d.struct.unpack(buffer);
+    assertEquals(DATA.getQuaternion(), data.getQuaternion());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/struct/Transform2dStructTest.java

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Transform2dStructTest {
+  private static final Transform2d DATA =
+      new Transform2d(new Translation2d(0.191, 2.2), new Rotation2d(4.4));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Transform2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Transform2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Transform2d data = Transform2d.struct.unpack(buffer);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/struct/Transform3dStructTest.java

@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Transform3dStructTest {
+  private static final Transform3d DATA =
+      new Transform3d(
+          new Translation3d(0.3504, 22.9, 3.504),
+          new Rotation3d(new Quaternion(0.3504, 35.04, 2.29, 0.3504)));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Transform3d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Transform3d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Transform3d data = Transform3d.struct.unpack(buffer);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/struct/Translation2dStructTest.java

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Translation2dStructTest {
+  private static final Translation2d DATA = new Translation2d(3.504, 22.9);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Translation2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Translation2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Translation2d data = Translation2d.struct.unpack(buffer);
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/struct/Translation3dStructTest.java

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Translation3dStructTest {
+  private static final Translation3d DATA = new Translation3d(35.04, 22.9, 3.504);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Translation3d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Translation3d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Translation3d data = Translation3d.struct.unpack(buffer);
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+    assertEquals(DATA.getZ(), data.getZ());
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/struct/Twist2dStructTest.java

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Twist2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Twist2dStructTest {
+  private static final Twist2d DATA = new Twist2d(2.29, 35.04, 35.04);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Twist2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Twist2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Twist2d data = Twist2d.struct.unpack(buffer);
+    assertEquals(DATA.dx, data.dx);
+    assertEquals(DATA.dy, data.dy);
+    assertEquals(DATA.dtheta, data.dtheta);
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/struct/Twist3dStructTest.java

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Twist3d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Twist3dStructTest {
+  private static final Twist3d DATA = new Twist3d(1.1, 2.29, 35.04, 0.174, 19.1, 4.4);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Twist3d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Twist3d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Twist3d data = Twist3d.struct.unpack(buffer);
+    assertEquals(DATA.dx, data.dx);
+    assertEquals(DATA.dy, data.dy);
+    assertEquals(DATA.dz, data.dz);
+    assertEquals(DATA.rx, data.rx);
+    assertEquals(DATA.ry, data.ry);
+    assertEquals(DATA.rz, data.rz);
+  }
+}

wpimath/src/test/java/org/wpilib/math/interpolation/InterpolatingDoubleTreeMapTest.java

@@ -0,0 +1,76 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class InterpolatingDoubleTreeMapTest {
+  @Test
+  void testInterpolationDouble() {
+    InterpolatingDoubleTreeMap table = new InterpolatingDoubleTreeMap();
+
+    table.put(125.0, 450.0);
+    table.put(200.0, 510.0);
+    table.put(268.0, 525.0);
+    table.put(312.0, 550.0);
+    table.put(326.0, 650.0);
+
+    // Key below minimum gives the smallest value
+    assertEquals(450.0, table.get(100.0));
+
+    // Minimum key gives exact value
+    assertEquals(450.0, table.get(125.0));
+
+    // Key gives interpolated value
+    assertEquals(480.0, table.get(162.5));
+
+    // Key at right of interpolation range gives exact value
+    assertEquals(510.0, table.get(200.0));
+
+    // Maximum key gives exact value
+    assertEquals(650.0, table.get(326.0));
+
+    // Key above maximum gives largest value
+    assertEquals(650.0, table.get(400.0));
+  }
+
+  @Test
+  void testInterpolationClear() {
+    InterpolatingDoubleTreeMap table = new InterpolatingDoubleTreeMap();
+
+    table.put(125.0, 450.0);
+    table.put(200.0, 510.0);
+    table.put(268.0, 525.0);
+    table.put(312.0, 550.0);
+    table.put(326.0, 650.0);
+
+    // Key below minimum gives the smallest value
+    assertEquals(450.0, table.get(100.0));
+
+    // Minimum key gives exact value
+    assertEquals(450.0, table.get(125.0));
+
+    // Key gives interpolated value
+    assertEquals(480.0, table.get(162.5));
+
+    // Key at right of interpolation range gives exact value
+    assertEquals(510.0, table.get(200.0));
+
+    // Maximum key gives exact value
+    assertEquals(650.0, table.get(326.0));
+
+    // Key above maximum gives largest value
+    assertEquals(650.0, table.get(400.0));
+
+    table.clear();
+
+    table.put(100.0, 250.0);
+    table.put(200.0, 500.0);
+
+    assertEquals(375.0, table.get(150.0));
+  }
+}

wpimath/src/test/java/org/wpilib/math/interpolation/InterpolatingTreeMapTest.java

@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class InterpolatingTreeMapTest {
+  @Test
+  void testInterpolation() {
+    InterpolatingTreeMap<Double, Double> table =
+        new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Interpolator.forDouble());
+
+    table.put(125.0, 450.0);
+    table.put(200.0, 510.0);
+    table.put(268.0, 525.0);
+    table.put(312.0, 550.0);
+    table.put(326.0, 650.0);
+
+    // Key below minimum gives the smallest value
+    assertEquals(450.0, table.get(100.0));
+
+    // Minimum key gives exact value
+    assertEquals(450.0, table.get(125.0));
+
+    // Key gives interpolated value
+    assertEquals(480.0, table.get(162.5));
+
+    // Key at right of interpolation range gives exact value
+    assertEquals(510.0, table.get(200.0));
+
+    // Maximum key gives exact value
+    assertEquals(650.0, table.get(326.0));
+
+    // Key above maximum gives largest value
+    assertEquals(650.0, table.get(400.0));
+
+    table.clear();
+
+    table.put(100.0, 250.0);
+    table.put(200.0, 500.0);
+
+    assertEquals(375.0, table.get(150.0));
+  }
+}

wpimath/src/test/java/org/wpilib/math/interpolation/TimeInterpolatableBufferTest.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.interpolation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.junit.jupiter.api.Test;
+
+class TimeInterpolatableBufferTest {
+  @Test
+  void testAddSample() {
+    TimeInterpolatableBuffer<Rotation2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
+
+    // No entries
+    buffer.addSample(1.0, Rotation2d.kZero);
+    assertEquals(0.0, buffer.getSample(1.0).get().getRadians(), 0.001);
+
+    // New entry at start of container
+    buffer.addSample(0.0, new Rotation2d(1.0));
+    assertEquals(1.0, buffer.getSample(0.0).get().getRadians(), 0.001);
+
+    // New entry in middle of container
+    buffer.addSample(0.5, new Rotation2d(0.5));
+    assertEquals(0.5, buffer.getSample(0.5).get().getRadians(), 0.001);
+
+    // Override sample
+    buffer.addSample(0.5, new Rotation2d(1.0));
+    assertEquals(1.0, buffer.getSample(0.5).get().getRadians(), 0.001);
+  }
+
+  @Test
+  void testInterpolation() {
+    TimeInterpolatableBuffer<Rotation2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
+
+    buffer.addSample(0.0, Rotation2d.kZero);
+    assertEquals(0.0, buffer.getSample(0.0).get().getRadians(), 0.001);
+    buffer.addSample(1.0, new Rotation2d(1.0));
+    assertEquals(0.5, buffer.getSample(0.5).get().getRadians(), 0.001);
+    assertEquals(1.0, buffer.getSample(1.0).get().getRadians(), 0.001);
+    buffer.addSample(3.0, new Rotation2d(2.0));
+    assertEquals(1.5, buffer.getSample(2.0).get().getRadians(), 0.001);
+
+    buffer.addSample(10.5, new Rotation2d(2));
+    assertEquals(new Rotation2d(1.0), buffer.getSample(0.0).get());
+  }
+
+  @Test
+  void testPose2d() {
+    TimeInterpolatableBuffer<Pose2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
+
+    // We expect to be at (1 - 1/Math.sqrt(2), 1/Math.sqrt(2), 45deg) at t=0.5
+    buffer.addSample(0.0, new Pose2d(0.0, 0.0, Rotation2d.kCCW_Pi_2));
+    buffer.addSample(1.0, new Pose2d(1.0, 1.0, Rotation2d.kZero));
+    Pose2d sample = buffer.getSample(0.5).get();
+
+    assertEquals(1.0 - 1.0 / Math.sqrt(2.0), sample.getTranslation().getX(), 0.01);
+    assertEquals(1.0 / Math.sqrt(2.0), sample.getTranslation().getY(), 0.01);
+    assertEquals(45.0, sample.getRotation().getDegrees(), 0.01);
+  }
+}

wpimath/src/test/java/org/wpilib/math/jni/ArmFeedforwardJNITest.java

@@ -0,0 +1,16 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.jni;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+import org.junit.jupiter.api.Test;
+
+public class ArmFeedforwardJNITest {
+  @Test
+  public void testLink() {
+    assertDoesNotThrow(ArmFeedforwardJNI::forceLoad);
+  }
+}

wpimath/src/test/java/org/wpilib/math/jni/DAREJNITest.java

@@ -0,0 +1,16 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.jni;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+import org.junit.jupiter.api.Test;
+
+public class DAREJNITest {
+  @Test
+  public void testLink() {
+    assertDoesNotThrow(DAREJNI::forceLoad);
+  }
+}

wpimath/src/test/java/org/wpilib/math/jni/EigenJNITest.java

@@ -0,0 +1,16 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.jni;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+import org.junit.jupiter.api.Test;
+
+public class EigenJNITest {
+  @Test
+  public void testLink() {
+    assertDoesNotThrow(EigenJNI::forceLoad);
+  }
+}

wpimath/src/test/java/org/wpilib/math/jni/Ellipse2dJNITest.java

@@ -0,0 +1,16 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.jni;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+import org.junit.jupiter.api.Test;
+
+public class Ellipse2dJNITest {
+  @Test
+  public void testLink() {
+    assertDoesNotThrow(Ellipse2dJNI::forceLoad);
+  }
+}

wpimath/src/test/java/org/wpilib/math/jni/StateSpaceUtilJNITest.java

@@ -0,0 +1,16 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.jni;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+import org.junit.jupiter.api.Test;
+
+public class StateSpaceUtilJNITest {
+  @Test
+  public void testLink() {
+    assertDoesNotThrow(StateSpaceUtilJNI::forceLoad);
+  }
+}

wpimath/src/test/java/org/wpilib/math/jni/Transform3dJNITest.java

@@ -0,0 +1,16 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.jni;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+import org.junit.jupiter.api.Test;
+
+public class Transform3dJNITest {
+  @Test
+  public void testLink() {
+    assertDoesNotThrow(Transform3dJNI::forceLoad);
+  }
+}

wpimath/src/test/java/org/wpilib/math/jni/Twist3dJNITest.java

@@ -0,0 +1,16 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.jni;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+import org.junit.jupiter.api.Test;
+
+public class Twist3dJNITest {
+  @Test
+  public void testLink() {
+    assertDoesNotThrow(Twist3dJNI::forceLoad);
+  }
+}

wpimath/src/test/java/org/wpilib/math/kinematics/ChassisSpeedsTest.java

@@ -0,0 +1,130 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import static edu.wpi.first.units.Units.InchesPerSecond;
+import static edu.wpi.first.units.Units.RPM;
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import org.junit.jupiter.api.Test;
+
+class ChassisSpeedsTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testDiscretize() {
+    final var target = new ChassisSpeeds(1.0, 0.0, 0.5);
+    final var duration = 1.0;
+    final var dt = 0.01;
+
+    final var speeds = target.discretize(duration);
+    final var twist = new Twist2d(speeds.vx * dt, speeds.vy * dt, speeds.omega * dt);
+
+    var pose = Pose2d.kZero;
+    for (double time = 0; time < duration; time += dt) {
+      pose = pose.plus(twist.exp());
+    }
+
+    final var result = pose; // For lambda capture
+    assertAll(
+        () -> assertEquals(target.vx * duration, result.getX(), kEpsilon),
+        () -> assertEquals(target.vy * duration, result.getY(), kEpsilon),
+        () -> assertEquals(target.omega * duration, result.getRotation().getRadians(), kEpsilon));
+  }
+
+  @Test
+  void testMeasureConstructor() {
+    var vx = InchesPerSecond.of(14.52);
+    var vy = InchesPerSecond.of(0);
+    var omega = RPM.of(0.02);
+    var speeds = new ChassisSpeeds(vx, vy, omega);
+
+    assertAll(
+        () -> assertEquals(0.368808, speeds.vx, kEpsilon),
+        () -> assertEquals(0, speeds.vy, kEpsilon),
+        () -> assertEquals(0.002094395102, speeds.omega, kEpsilon));
+  }
+
+  @Test
+  void testToRobotRelative() {
+    final var chassisSpeeds = new ChassisSpeeds(1.0, 0.0, 0.5).toRobotRelative(Rotation2d.kCW_Pi_2);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon),
+        () -> assertEquals(1.0, chassisSpeeds.vy, kEpsilon),
+        () -> assertEquals(0.5, chassisSpeeds.omega, kEpsilon));
+  }
+
+  @Test
+  void testToFieldRelative() {
+    final var chassisSpeeds =
+        new ChassisSpeeds(1.0, 0.0, 0.5).toFieldRelative(Rotation2d.fromDegrees(45.0));
+
+    assertAll(
+        () -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vx, kEpsilon),
+        () -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vy, kEpsilon),
+        () -> assertEquals(0.5, chassisSpeeds.omega, kEpsilon));
+  }
+
+  @Test
+  void testPlus() {
+    final var left = new ChassisSpeeds(1.0, 0.5, 0.75);
+    final var right = new ChassisSpeeds(2.0, 1.5, 0.25);
+
+    final var chassisSpeeds = left.plus(right);
+
+    assertAll(
+        () -> assertEquals(3.0, chassisSpeeds.vx),
+        () -> assertEquals(2.0, chassisSpeeds.vy),
+        () -> assertEquals(1.0, chassisSpeeds.omega));
+  }
+
+  @Test
+  void testMinus() {
+    final var left = new ChassisSpeeds(1.0, 0.5, 0.75);
+    final var right = new ChassisSpeeds(2.0, 0.5, 0.25);
+
+    final var chassisSpeeds = left.minus(right);
+
+    assertAll(
+        () -> assertEquals(-1.0, chassisSpeeds.vx),
+        () -> assertEquals(0.0, chassisSpeeds.vy),
+        () -> assertEquals(0.5, chassisSpeeds.omega));
+  }
+
+  @Test
+  void testUnaryMinus() {
+    final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).unaryMinus();
+
+    assertAll(
+        () -> assertEquals(-1.0, chassisSpeeds.vx),
+        () -> assertEquals(-0.5, chassisSpeeds.vy),
+        () -> assertEquals(-0.75, chassisSpeeds.omega));
+  }
+
+  @Test
+  void testMultiplication() {
+    final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).times(2.0);
+
+    assertAll(
+        () -> assertEquals(2.0, chassisSpeeds.vx),
+        () -> assertEquals(1.0, chassisSpeeds.vy),
+        () -> assertEquals(1.5, chassisSpeeds.omega));
+  }
+
+  @Test
+  void testDivision() {
+    final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).div(2.0);
+
+    assertAll(
+        () -> assertEquals(0.5, chassisSpeeds.vx),
+        () -> assertEquals(0.25, chassisSpeeds.vy),
+        () -> assertEquals(0.375, chassisSpeeds.omega));
+  }
+}

wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveKinematicsTest.java

@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveKinematicsTest {
+  private static final double kEpsilon = 1E-9;
+  private final DifferentialDriveKinematics m_kinematics =
+      new DifferentialDriveKinematics(0.381 * 2);
+
+  @Test
+  void testInverseKinematicsForZeros() {
+    var chassisSpeeds = new ChassisSpeeds();
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+
+    assertAll(
+        () -> assertEquals(0.0, wheelSpeeds.left, kEpsilon),
+        () -> assertEquals(0.0, wheelSpeeds.right, kEpsilon));
+  }
+
+  @Test
+  void testForwardKinematicsForZeros() {
+    var wheelSpeeds = new DifferentialDriveWheelSpeeds();
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.omega, kEpsilon));
+  }
+
+  @Test
+  void testInverseKinematicsForStraightLine() {
+    var chassisSpeeds = new ChassisSpeeds(3, 0, 0);
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+
+    assertAll(
+        () -> assertEquals(3.0, wheelSpeeds.left, kEpsilon),
+        () -> assertEquals(3.0, wheelSpeeds.right, kEpsilon));
+  }
+
+  @Test
+  void testForwardKinematicsForStraightLine() {
+    var wheelSpeeds = new DifferentialDriveWheelSpeeds(3, 3);
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(3.0, chassisSpeeds.vx, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.omega, kEpsilon));
+  }
+
+  @Test
+  void testInverseKinematicsForRotateInPlace() {
+    var chassisSpeeds = new ChassisSpeeds(0, 0, Math.PI);
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+
+    assertAll(
+        () -> assertEquals(-0.381 * Math.PI, wheelSpeeds.left, kEpsilon),
+        () -> assertEquals(+0.381 * Math.PI, wheelSpeeds.right, kEpsilon));
+  }
+
+  @Test
+  void testForwardKinematicsForRotateInPlace() {
+    var wheelSpeeds = new DifferentialDriveWheelSpeeds(+0.381 * Math.PI, -0.381 * Math.PI);
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon),
+        () -> assertEquals(-Math.PI, chassisSpeeds.omega, kEpsilon));
+  }
+}