SCRIPT Move cc files

wpimath/src/main/native/include/wpi/math/controller/ArmFeedforward.hpp

@@ -0,0 +1,322 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <cstdlib>
+
+#include <wpi/MathExtras.h>
+#include <wpi/SymbolExports.h>
+
+#include "units/angle.h"
+#include "units/angular_velocity.h"
+#include "units/math.h"
+#include "units/voltage.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+/**
+ * A helper class that computes feedforward outputs for a simple arm (modeled as
+ * a motor acting against the force of gravity on a beam suspended at an angle).
+ */
+class WPILIB_DLLEXPORT ArmFeedforward {
+ public:
+  using Angle = units::radians;
+  using Velocity = units::radians_per_second;
+  using Acceleration = units::compound_unit<units::radians_per_second,
+                                            units::inverse<units::second>>;
+  using kv_unit =
+      units::compound_unit<units::volts,
+                           units::inverse<units::radians_per_second>>;
+  using ka_unit =
+      units::compound_unit<units::volts, units::inverse<Acceleration>>;
+
+  /**
+   * Creates a new ArmFeedforward with the specified gains.
+   *
+   * @param kS The static gain, in volts.
+   * @param kG The gravity gain, in volts.
+   * @param kV The velocity gain, in volt seconds per radian.
+   * @param kA The acceleration gain, in volt seconds² per radian.
+   * @param dt The period in seconds.
+   * @throws IllegalArgumentException for kv &lt; zero.
+   * @throws IllegalArgumentException for ka &lt; zero.
+   * @throws IllegalArgumentException for period &le; zero.
+   */
+  constexpr ArmFeedforward(
+      units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
+      units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0),
+      units::second_t dt = 20_ms)
+      : kS(kS), kG(kG), kV(kV), kA(kA), m_dt(dt) {
+    if (kV.value() < 0) {
+      wpi::math::MathSharedStore::ReportError(
+          "kV must be a non-negative number, got {}!", kV.value());
+      this->kV = units::unit_t<kv_unit>{0};
+      wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
+    }
+    if (kA.value() < 0) {
+      wpi::math::MathSharedStore::ReportError(
+          "kA must be a non-negative number, got {}!", kA.value());
+      this->kA = units::unit_t<ka_unit>{0};
+      wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;");
+    }
+    if (dt <= 0_ms) {
+      wpi::math::MathSharedStore::ReportError(
+          "period must be a positive number, got {}!", dt.value());
+      this->m_dt = 20_ms;
+      wpi::math::MathSharedStore::ReportWarning("period defaulted to 20 ms.");
+    }
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints assuming continuous
+   * control.
+   *
+   * @param angle        The angle setpoint, in radians. This angle should be
+   *                     measured from the horizontal (i.e. if the provided
+   *                     angle is 0, the arm should be parallel to the floor).
+   *                     If your encoder does not follow this convention, an
+   *                     offset should be added.
+   * @param velocity     The velocity setpoint.
+   * @param acceleration The acceleration setpoint.
+   * @return The computed feedforward, in volts.
+   */
+  [[deprecated("Use the current/next velocity overload instead.")]]
+  constexpr units::volt_t Calculate(
+      units::unit_t<Angle> angle, units::unit_t<Velocity> velocity,
+      units::unit_t<Acceleration> acceleration) const {
+    return kS * wpi::sgn(velocity) + kG * units::math::cos(angle) +
+           kV * velocity + kA * acceleration;
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints assuming continuous
+   * control.
+   *
+   * @param currentAngle The current angle in radians. This angle should be
+   *   measured from the horizontal (i.e. if the provided angle is 0, the arm
+   *   should be parallel to the floor). If your encoder does not follow this
+   *   convention, an offset should be added.
+   * @param currentVelocity The current velocity setpoint.
+   * @param nextVelocity The next velocity setpoint.
+   * @param dt Time between velocity setpoints in seconds.
+   * @return The computed feedforward in volts.
+   */
+  [[deprecated("Use the current/next velocity overload instead.")]]
+  units::volt_t Calculate(units::unit_t<Angle> currentAngle,
+                          units::unit_t<Velocity> currentVelocity,
+                          units::unit_t<Velocity> nextVelocity,
+                          units::second_t dt) const {
+    return Calculate(currentAngle, currentVelocity, nextVelocity);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoint assuming discrete
+   * control. Use this method when the velocity does not change.
+   *
+   * @param currentAngle The current angle. This angle should be measured from
+   * the horizontal (i.e. if the provided angle is 0, the arm should be parallel
+   * to the floor). If your encoder does not follow this convention, an offset
+   * should be added.
+   * @param currentVelocity The current velocity.
+   * @return The computed feedforward in volts.
+   */
+  constexpr units::volt_t Calculate(
+      units::unit_t<Angle> currentAngle,
+      units::unit_t<Velocity> currentVelocity) const {
+    return kS * wpi::sgn(currentVelocity) +
+           kG * units::math::cos(currentAngle) + kV * currentVelocity;
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints assuming discrete
+   * control.
+   *
+   * @param currentAngle The current angle. This angle should be measured from
+   * the horizontal (i.e. if the provided angle is 0, the arm should be parallel
+   * to the floor). If your encoder does not follow this convention, an offset
+   * should be added.
+   * @param currentVelocity The current velocity.
+   * @param nextVelocity    The next velocity.
+   * @return The computed feedforward in volts.
+   */
+  units::volt_t Calculate(units::unit_t<Angle> currentAngle,
+                          units::unit_t<Velocity> currentVelocity,
+                          units::unit_t<Velocity> nextVelocity) const;
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply,
+   * a position, and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage   The maximum voltage that can be supplied to the arm.
+   * @param angle        The angle of the arm. This angle should be measured
+   *                     from the horizontal (i.e. if the provided angle is 0,
+   *                     the arm should be parallel to the floor). If your
+   *                     encoder does not follow this convention, an offset
+   *                     should be added.
+   * @param acceleration The acceleration of the arm.
+   * @return The maximum possible velocity at the given acceleration and angle.
+   */
+  constexpr units::unit_t<Velocity> MaxAchievableVelocity(
+      units::volt_t maxVoltage, units::unit_t<Angle> angle,
+      units::unit_t<Acceleration> acceleration) {
+    // Assume max velocity is positive
+    return (maxVoltage - kS - kG * units::math::cos(angle) -
+            kA * acceleration) /
+           kV;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply,
+   * a position, and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage   The maximum voltage that can be supplied to the arm.
+   * @param angle        The angle of the arm. This angle should be measured
+   *                     from the horizontal (i.e. if the provided angle is 0,
+   *                     the arm should be parallel to the floor). If your
+   *                     encoder does not follow this convention, an offset
+   *                     should be added.
+   * @param acceleration The acceleration of the arm.
+   * @return The minimum possible velocity at the given acceleration and angle.
+   */
+  constexpr units::unit_t<Velocity> MinAchievableVelocity(
+      units::volt_t maxVoltage, units::unit_t<Angle> angle,
+      units::unit_t<Acceleration> acceleration) {
+    // Assume min velocity is negative, ks flips sign
+    return (-maxVoltage + kS - kG * units::math::cos(angle) -
+            kA * acceleration) /
+           kV;
+  }
+
+  /**
+   * Calculates the maximum achievable acceleration given a maximum voltage
+   * supply, a position, and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle      The angle of the arm. This angle should be measured
+   *                   from the horizontal (i.e. if the provided angle is 0,
+   *                   the arm should be parallel to the floor). If your
+   *                   encoder does not follow this convention, an offset
+   *                   should be added.
+   * @param velocity   The velocity of the arm.
+   * @return The maximum possible acceleration at the given velocity and angle.
+   */
+  constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
+      units::volt_t maxVoltage, units::unit_t<Angle> angle,
+      units::unit_t<Velocity> velocity) {
+    return (maxVoltage - kS * wpi::sgn(velocity) -
+            kG * units::math::cos(angle) - kV * velocity) /
+           kA;
+  }
+
+  /**
+   * Calculates the minimum achievable acceleration given a maximum voltage
+   * supply, a position, and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle      The angle of the arm. This angle should be measured
+   *                   from the horizontal (i.e. if the provided angle is 0,
+   *                   the arm should be parallel to the floor). If your
+   *                   encoder does not follow this convention, an offset
+   *                   should be added.
+   * @param velocity   The velocity of the arm.
+   * @return The minimum possible acceleration at the given velocity and angle.
+   */
+  constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
+      units::volt_t maxVoltage, units::unit_t<Angle> angle,
+      units::unit_t<Velocity> velocity) {
+    return MaxAchievableAcceleration(-maxVoltage, angle, velocity);
+  }
+
+  /**
+   * Sets the static gain.
+   *
+   * @param kS The static gain.
+   */
+  constexpr void SetKs(units::volt_t kS) { this->kS = kS; }
+
+  /**
+   * Sets the gravity gain.
+   *
+   * @param kG The gravity gain.
+   */
+  constexpr void SetKg(units::volt_t kG) { this->kG = kG; }
+
+  /**
+   * Sets the velocity gain.
+   *
+   * @param kV The velocity gain.
+   */
+  constexpr void SetKv(units::unit_t<kv_unit> kV) { this->kV = kV; }
+
+  /**
+   * Sets the acceleration gain.
+   *
+   * @param kA The acceleration gain.
+   */
+  constexpr void SetKa(units::unit_t<ka_unit> kA) { this->kA = kA; }
+
+  /**
+   * Returns the static gain.
+   *
+   * @return The static gain.
+   */
+  constexpr units::volt_t GetKs() const { return kS; }
+
+  /**
+   * Returns the gravity gain.
+   *
+   * @return The gravity gain.
+   */
+  constexpr units::volt_t GetKg() const { return kG; }
+
+  /**
+   * Returns the velocity gain.
+   *
+   * @return The velocity gain.
+   */
+  constexpr units::unit_t<kv_unit> GetKv() const { return kV; }
+
+  /**
+   * Returns the acceleration gain.
+   *
+   * @return The acceleration gain.
+   */
+  constexpr units::unit_t<ka_unit> GetKa() const { return kA; }
+
+ private:
+  /// The static gain, in volts.
+  units::volt_t kS;
+
+  /// The gravity gain, in volts.
+  units::volt_t kG;
+
+  /// The velocity gain, in V/(rad/s)volt seconds per radian.
+  units::unit_t<kv_unit> kV;
+
+  /// The acceleration gain, in V/(rad/s²).
+  units::unit_t<ka_unit> kA;
+
+  /** The period. */
+  units::second_t m_dt;
+};
+}  // namespace frc
+
+#include "frc/controller/proto/ArmFeedforwardProto.h"
+#include "frc/controller/struct/ArmFeedforwardStruct.h"

wpimath/src/main/native/include/wpi/math/controller/BangBangController.hpp

@@ -0,0 +1,143 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <limits>
+
+#include <gcem.hpp>
+#include <wpi/SymbolExports.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+/**
+ * Implements a bang-bang controller, which outputs either 0 or 1 depending on
+ * whether the measurement is less than the setpoint. This maximally-aggressive
+ * control approach works very well for velocity control of high-inertia
+ * mechanisms, and poorly on most other things.
+ *
+ * <p>Note that this is an *asymmetric* bang-bang controller - it will not exert
+ * any control effort in the reverse direction (e.g. it won't try to slow down
+ * an over-speeding shooter wheel). This asymmetry is *extremely important.*
+ * Bang-bang control is extremely simple, but also potentially hazardous. Always
+ * ensure that your motor controllers are set to "coast" before attempting to
+ * control them with a bang-bang controller.
+ */
+class WPILIB_DLLEXPORT BangBangController
+    : public wpi::Sendable,
+      public wpi::SendableHelper<BangBangController> {
+ public:
+  /**
+   * Creates a new bang-bang controller.
+   *
+   * <p>Always ensure that your motor controllers are set to "coast" before
+   * attempting to control them with a bang-bang controller.
+   *
+   * @param tolerance Tolerance for atSetpoint.
+   */
+  constexpr explicit BangBangController(
+      double tolerance = std::numeric_limits<double>::infinity())
+      : m_tolerance(tolerance) {
+    if (!std::is_constant_evaluated()) {
+      ++instances;
+      wpi::math::MathSharedStore::ReportUsage("BangBangController",
+                                              std::to_string(instances));
+    }
+  }
+
+  /**
+   * Sets the setpoint for the bang-bang controller.
+   *
+   * @param setpoint The desired setpoint.
+   */
+  constexpr void SetSetpoint(double setpoint) { m_setpoint = setpoint; }
+
+  /**
+   * Returns the current setpoint of the bang-bang controller.
+   *
+   * @return The current setpoint.
+   */
+  constexpr double GetSetpoint() const { return m_setpoint; }
+
+  /**
+   * Returns true if the error is within the tolerance of the setpoint.
+   *
+   * @return Whether the error is within the acceptable bounds.
+   */
+  constexpr bool AtSetpoint() const {
+    return gcem::abs(m_setpoint - m_measurement) < m_tolerance;
+  }
+
+  /**
+   * Sets the error within which AtSetpoint will return true.
+   *
+   * @param tolerance Position error which is tolerable.
+   */
+  constexpr void SetTolerance(double tolerance) { m_tolerance = tolerance; }
+
+  /**
+   * Returns the current tolerance of the controller.
+   *
+   * @return The current tolerance.
+   */
+  constexpr double GetTolerance() const { return m_tolerance; }
+
+  /**
+   * Returns the current measurement of the process variable.
+   *
+   * @return The current measurement of the process variable.
+   */
+  constexpr double GetMeasurement() const { return m_measurement; }
+
+  /**
+   * Returns the current error.
+   *
+   * @return The current error.
+   */
+  constexpr double GetError() const { return m_setpoint - m_measurement; }
+
+  /**
+   * Returns the calculated control output.
+   *
+   * <p>Always ensure that your motor controllers are set to "coast" before
+   * attempting to control them with a bang-bang controller.
+   *
+   * @param measurement The most recent measurement of the process variable.
+   * @param setpoint The setpoint for the process variable.
+   * @return The calculated motor output (0 or 1).
+   */
+  constexpr double Calculate(double measurement, double setpoint) {
+    m_measurement = measurement;
+    m_setpoint = setpoint;
+
+    return measurement < setpoint ? 1 : 0;
+  }
+
+  /**
+   * Returns the calculated control output.
+   *
+   * @param measurement The most recent measurement of the process variable.
+   * @return The calculated motor output (0 or 1).
+   */
+  constexpr double Calculate(double measurement) {
+    return Calculate(measurement, m_setpoint);
+  }
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  double m_tolerance;
+
+  double m_setpoint = 0;
+  double m_measurement = 0;
+
+  // Usage reporting instances
+  inline static int instances = 0;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/controller/ControlAffinePlantInversionFeedforward.hpp

@@ -0,0 +1,195 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <array>
+#include <functional>
+
+#include <Eigen/QR>
+
+#include "frc/EigenCore.h"
+#include "frc/system/NumericalJacobian.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Constructs a control-affine plant inversion model-based feedforward from
+ * given model dynamics.
+ *
+ * If given the vector valued function as f(x, u) where x is the state
+ * vector and u is the input vector, the B matrix(continuous input matrix)
+ * is calculated through a NumericalJacobian. In this case f has to be
+ * control-affine (of the form f(x) + Bu).
+ *
+ * The feedforward is calculated as
+ * <strong> u_ff = B<sup>+</sup> (rDot - f(x)) </strong>, where <strong>
+ * B<sup>+</sup> </strong> is the pseudoinverse of B.
+ *
+ * This feedforward does not account for a dynamic B matrix, B is either
+ * determined or supplied when the feedforward is created and remains constant.
+ *
+ * For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ */
+template <int States, int Inputs>
+class ControlAffinePlantInversionFeedforward {
+ public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+
+  /**
+   * Constructs a feedforward with given model dynamics as a function
+   * of state and input.
+   *
+   * @param f  A vector-valued function of x, the state, and
+   *           u, the input, that returns the derivative of
+   *           the state vector. HAS to be control-affine
+   *           (of the form f(x) + Bu).
+   * @param dt The timestep between calls of calculate().
+   */
+  ControlAffinePlantInversionFeedforward(
+      std::function<StateVector(const StateVector&, const InputVector&)> f,
+      units::second_t dt)
+      : m_dt(dt), m_f(f) {
+    m_B = NumericalJacobianU<States, States, Inputs>(f, StateVector::Zero(),
+                                                     InputVector::Zero());
+
+    Reset();
+  }
+
+  /**
+   * Constructs a feedforward with given model dynamics as a function of state,
+   * and the plant's B matrix(continuous input matrix).
+   *
+   * @param f  A vector-valued function of x, the state,
+   *           that returns the derivative of the state vector.
+   * @param B  Continuous input matrix of the plant being controlled.
+   * @param dt The timestep between calls of calculate().
+   */
+  ControlAffinePlantInversionFeedforward(
+      std::function<StateVector(const StateVector&)> f,
+      const Matrixd<States, Inputs>& B, units::second_t dt)
+      : m_B(B), m_dt(dt) {
+    m_f = [=](const StateVector& x, const InputVector& u) -> StateVector {
+      return f(x);
+    };
+
+    Reset();
+  }
+
+  ControlAffinePlantInversionFeedforward(
+      ControlAffinePlantInversionFeedforward&&) = default;
+  ControlAffinePlantInversionFeedforward& operator=(
+      ControlAffinePlantInversionFeedforward&&) = default;
+
+  /**
+   * Returns the previously calculated feedforward as an input vector.
+   *
+   * @return The calculated feedforward.
+   */
+  const InputVector& Uff() const { return m_uff; }
+
+  /**
+   * Returns an element of the previously calculated feedforward.
+   *
+   * @param i Row of uff.
+   *
+   * @return The row of the calculated feedforward.
+   */
+  double Uff(int i) const { return m_uff(i); }
+
+  /**
+   * Returns the current reference vector r.
+   *
+   * @return The current reference vector.
+   */
+  const StateVector& R() const { return m_r; }
+
+  /**
+   * Returns an element of the reference vector r.
+   *
+   * @param i Row of r.
+   *
+   * @return The row of the current reference vector.
+   */
+  double R(int i) const { return m_r(i); }
+
+  /**
+   * Resets the feedforward with a specified initial state vector.
+   *
+   * @param initialState The initial state vector.
+   */
+  void Reset(const StateVector& initialState) {
+    m_r = initialState;
+    m_uff.setZero();
+  }
+
+  /**
+   * Resets the feedforward with a zero initial state vector.
+   */
+  void Reset() {
+    m_r.setZero();
+    m_uff.setZero();
+  }
+
+  /**
+   * Calculate the feedforward with only the desired
+   * future reference. This uses the internally stored "current"
+   * reference.
+   *
+   * If this method is used the initial state of the system is the one set using
+   * Reset(const StateVector&). If the initial state is not
+   * set it defaults to a zero vector.
+   *
+   * @param nextR The reference state of the future timestep (k + dt).
+   *
+   * @return The calculated feedforward.
+   */
+  InputVector Calculate(const StateVector& nextR) {
+    return Calculate(m_r, nextR);
+  }
+
+  /**
+   * Calculate the feedforward with current and future reference vectors.
+   *
+   * @param r     The reference state of the current timestep (k).
+   * @param nextR The reference state of the future timestep (k + dt).
+   *
+   * @return The calculated feedforward.
+   */
+  InputVector Calculate(const StateVector& r, const StateVector& nextR) {
+    StateVector rDot = (nextR - r) / m_dt.value();
+
+    // ṙ = f(r) + Bu
+    // Bu = ṙ − f(r)
+    // u = B⁺(ṙ − f(r))
+    m_uff = m_B.householderQr().solve(rDot - m_f(r, InputVector::Zero()));
+
+    m_r = nextR;
+    return m_uff;
+  }
+
+ private:
+  Matrixd<States, Inputs> m_B;
+
+  units::second_t m_dt;
+
+  /**
+   * The model dynamics.
+   */
+  std::function<StateVector(const StateVector&, const InputVector&)> m_f;
+
+  // Current reference
+  StateVector m_r;
+
+  // Computed feedforward
+  InputVector m_uff;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveAccelerationLimiter.hpp

@@ -0,0 +1,98 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <utility>
+
+#include <Eigen/Core>
+#include <wpi/SymbolExports.h>
+
+#include "frc/controller/DifferentialDriveWheelVoltages.h"
+#include "frc/system/LinearSystem.h"
+#include "units/acceleration.h"
+#include "units/angular_acceleration.h"
+#include "units/length.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+
+namespace frc {
+
+/**
+ * Filters the provided voltages to limit a differential drive's linear and
+ * angular acceleration.
+ *
+ * The differential drive model can be created via the functions in
+ * LinearSystemId.
+ */
+class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter {
+ public:
+  /**
+   * Constructs a DifferentialDriveAccelerationLimiter.
+   *
+   * @param system The differential drive dynamics.
+   * @param trackwidth The distance between the differential drive's left and
+   *                   right wheels.
+   * @param maxLinearAccel The maximum linear acceleration.
+   * @param maxAngularAccel The maximum angular acceleration.
+   */
+  DifferentialDriveAccelerationLimiter(
+      LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
+      units::meters_per_second_squared_t maxLinearAccel,
+      units::radians_per_second_squared_t maxAngularAccel)
+      : DifferentialDriveAccelerationLimiter(system, trackwidth,
+                                             -maxLinearAccel, maxLinearAccel,
+                                             maxAngularAccel) {}
+
+  /**
+   * Constructs a DifferentialDriveAccelerationLimiter.
+   *
+   * @param system The differential drive dynamics.
+   * @param trackwidth The distance between the differential drive's left and
+   *                   right wheels.
+   * @param minLinearAccel The minimum (most negative) linear acceleration.
+   * @param maxLinearAccel The maximum (most positive) linear acceleration.
+   * @param maxAngularAccel The maximum angular acceleration.
+   * @throws std::invalid_argument if minimum linear acceleration is greater
+   * than maximum linear acceleration
+   */
+  DifferentialDriveAccelerationLimiter(
+      LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
+      units::meters_per_second_squared_t minLinearAccel,
+      units::meters_per_second_squared_t maxLinearAccel,
+      units::radians_per_second_squared_t maxAngularAccel)
+      : m_system{std::move(system)},
+        m_trackwidth{trackwidth},
+        m_minLinearAccel{minLinearAccel},
+        m_maxLinearAccel{maxLinearAccel},
+        m_maxAngularAccel{maxAngularAccel} {
+    if (minLinearAccel > maxLinearAccel) {
+      throw std::invalid_argument(
+          "maxLinearAccel must be greater than minLinearAccel");
+    }
+  }
+
+  /**
+   * Returns the next voltage pair subject to acceleration constraints.
+   *
+   * @param leftVelocity The left wheel velocity.
+   * @param rightVelocity The right wheel velocity.
+   * @param leftVoltage The unconstrained left motor voltage.
+   * @param rightVoltage The unconstrained right motor voltage.
+   * @return The constrained wheel voltages.
+   */
+  DifferentialDriveWheelVoltages Calculate(
+      units::meters_per_second_t leftVelocity,
+      units::meters_per_second_t rightVelocity, units::volt_t leftVoltage,
+      units::volt_t rightVoltage);
+
+ private:
+  LinearSystem<2, 2, 2> m_system;
+  units::meter_t m_trackwidth;
+  units::meters_per_second_squared_t m_minLinearAccel;
+  units::meters_per_second_squared_t m_maxLinearAccel;
+  units::radians_per_second_squared_t m_maxAngularAccel;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveFeedforward.hpp

@@ -0,0 +1,103 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/controller/DifferentialDriveWheelVoltages.h"
+#include "frc/system/LinearSystem.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "units/acceleration.h"
+#include "units/angular_acceleration.h"
+#include "units/angular_velocity.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+
+namespace frc {
+/**
+ * A helper class which computes the feedforward outputs for a differential
+ * drive drivetrain.
+ */
+class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
+  frc::LinearSystem<2, 2, 2> m_plant;
+
+ public:
+  /**
+   * Creates a new DifferentialDriveFeedforward with the specified parameters.
+   *
+   * @param kVLinear The linear velocity gain in volts per (meters per second).
+   * @param kALinear The linear acceleration gain in volts per (meters per
+   * second squared).
+   * @param kVAngular The angular velocity gain in volts per (radians per
+   * second).
+   * @param kAAngular The angular acceleration gain in volts per (radians per
+   * second squared).
+   * @param trackwidth The distance between the differential drive's left and
+   * right wheels, in meters.
+   */
+  constexpr DifferentialDriveFeedforward(
+      decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
+      decltype(1_V / 1_rad_per_s) kVAngular,
+      decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth)
+      // See LinearSystemId::IdentifyDrivetrainSystem(decltype(1_V / 1_mps),
+      // decltype(1_V / 1_mps_sq), decltype(1_V / 1_rad_per_s), decltype(1_V /
+      // 1_rad_per_s_sq))
+      : DifferentialDriveFeedforward{kVLinear, kALinear,
+                                     kVAngular * 2.0 / trackwidth * 1_rad,
+                                     kAAngular * 2.0 / trackwidth * 1_rad} {}
+
+  /**
+   * Creates a new DifferentialDriveFeedforward with the specified parameters.
+   *
+   * @param kVLinear The linear velocity gain in volts per (meters per second).
+   * @param kALinear The linear acceleration gain in volts per (meters per
+   * second squared).
+   * @param kVAngular The angular velocity gain in volts per (meters per
+   * second).
+   * @param kAAngular The angular acceleration gain in volts per (meters per
+   * second squared).
+   */
+  constexpr DifferentialDriveFeedforward(decltype(1_V / 1_mps) kVLinear,
+                                         decltype(1_V / 1_mps_sq) kALinear,
+                                         decltype(1_V / 1_mps) kVAngular,
+                                         decltype(1_V / 1_mps_sq) kAAngular)
+      : m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem(
+            kVLinear, kALinear, kVAngular, kAAngular)},
+        m_kVLinear{kVLinear},
+        m_kALinear{kALinear},
+        m_kVAngular{kVAngular},
+        m_kAAngular{kAAngular} {}
+
+  /**
+   * Calculates the differential drive feedforward inputs given velocity
+   * setpoints.
+   *
+   * @param currentLeftVelocity The current left velocity of the differential
+   * drive in meters/second.
+   * @param nextLeftVelocity The next left velocity of the differential drive in
+   * meters/second.
+   * @param currentRightVelocity The current right velocity of the differential
+   * drive in meters/second.
+   * @param nextRightVelocity The next right velocity of the differential drive
+   * in meters/second.
+   * @param dt Discretization timestep.
+   */
+  DifferentialDriveWheelVoltages Calculate(
+      units::meters_per_second_t currentLeftVelocity,
+      units::meters_per_second_t nextLeftVelocity,
+      units::meters_per_second_t currentRightVelocity,
+      units::meters_per_second_t nextRightVelocity, units::second_t dt);
+
+  decltype(1_V / 1_mps) m_kVLinear;
+  decltype(1_V / 1_mps_sq) m_kALinear;
+  decltype(1_V / 1_mps) m_kVAngular;
+  decltype(1_V / 1_mps_sq) m_kAAngular;
+};
+}  // namespace frc
+
+#include "frc/controller/proto/DifferentialDriveFeedforwardProto.h"
+#include "frc/controller/struct/DifferentialDriveFeedforwardStruct.h"

wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveWheelVoltages.hpp

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "units/voltage.h"
+
+namespace frc {
+
+/**
+ * Motor voltages for a differential drive.
+ */
+struct DifferentialDriveWheelVoltages {
+  /// Left wheel voltage.
+  units::volt_t left = 0_V;
+
+  /// Right wheel voltage.
+  units::volt_t right = 0_V;
+};
+
+}  // namespace frc
+
+#include "frc/controller/proto/DifferentialDriveWheelVoltagesProto.h"
+#include "frc/controller/struct/DifferentialDriveWheelVoltagesStruct.h"

wpimath/src/main/native/include/wpi/math/controller/ElevatorFeedforward.hpp

@@ -0,0 +1,294 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/MathExtras.h>
+
+#include "frc/EigenCore.h"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "units/voltage.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+/**
+ * A helper class that computes feedforward outputs for a simple elevator
+ * (modeled as a motor acting against the force of gravity).
+ */
+class ElevatorFeedforward {
+ public:
+  using Distance = units::meters;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Acceleration =
+      units::compound_unit<Velocity, units::inverse<units::seconds>>;
+  using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
+  using ka_unit =
+      units::compound_unit<units::volts, units::inverse<Acceleration>>;
+
+  /**
+   * Creates a new ElevatorFeedforward with the specified gains.
+   *
+   * @param kS The static gain, in volts.
+   * @param kG The gravity gain, in volts.
+   * @param kV The velocity gain, in volt seconds per distance.
+   * @param kA The acceleration gain, in volt seconds² per distance.
+   * @param dt The period in seconds.
+   * @throws IllegalArgumentException for kv &lt; zero.
+   * @throws IllegalArgumentException for ka &lt; zero.
+   * @throws IllegalArgumentException for period &le; zero.
+   */
+  constexpr ElevatorFeedforward(
+      units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
+      units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0),
+      units::second_t dt = 20_ms)
+      : kS(kS), kG(kG), kV(kV), kA(kA), m_dt(dt) {
+    if (kV.value() < 0) {
+      wpi::math::MathSharedStore::ReportError(
+          "kV must be a non-negative number, got {}!", kV.value());
+      this->kV = units::unit_t<kv_unit>{0};
+      wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
+    }
+    if (kA.value() < 0) {
+      wpi::math::MathSharedStore::ReportError(
+          "kA must be a non-negative number, got {}!", kA.value());
+      this->kA = units::unit_t<ka_unit>{0};
+      wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;");
+    }
+    if (dt <= 0_ms) {
+      wpi::math::MathSharedStore::ReportError(
+          "period must be a positive number, got {}!", dt.value());
+      this->m_dt = 20_ms;
+      wpi::math::MathSharedStore::ReportWarning("period defaulted to 20 ms.");
+    }
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints assuming continuous
+   * control.
+   *
+   * @param velocity     The velocity setpoint.
+   * @param acceleration The acceleration setpoint.
+   * @return The computed feedforward, in volts.
+   * @deprecated Use the current/next velocity overload instead.
+   */
+  [[deprecated("Use the current/next velocity overload instead.")]]
+  constexpr units::volt_t Calculate(
+      units::unit_t<Velocity> velocity,
+      units::unit_t<Acceleration> acceleration) const {
+    return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints assuming continuous
+   * control.
+   *
+   * @param currentVelocity The current velocity setpoint.
+   * @param nextVelocity    The next velocity setpoint.
+   * @param dt              Time between velocity setpoints in seconds.
+   * @return The computed feedforward, in volts.
+   */
+  [[deprecated("Use the current/next velocity overload instead.")]]
+  units::volt_t Calculate(units::unit_t<Velocity> currentVelocity,
+                          units::unit_t<Velocity> nextVelocity,
+                          units::second_t dt) const {
+    // See wpimath/algorithms.md#Elevator_feedforward for derivation
+    auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
+    LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
+
+    Vectord<1> r{{currentVelocity.value()}};
+    Vectord<1> nextR{{nextVelocity.value()}};
+
+    return kG + kS * wpi::sgn(currentVelocity.value()) +
+           units::volt_t{feedforward.Calculate(r, nextR)(0)};
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoint assuming discrete
+   * control. Use this method when the setpoint does not change.
+   *
+   * @param currentVelocity The velocity setpoint.
+   * @return The computed feedforward, in volts.
+   */
+  constexpr units::volt_t Calculate(
+      units::unit_t<Velocity> currentVelocity) const {
+    return Calculate(currentVelocity, currentVelocity);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints assuming discrete
+   * control.
+   *
+   * <p>Note this method is inaccurate when the velocity crosses 0.
+   *
+   * @param currentVelocity The current velocity setpoint.
+   * @param nextVelocity    The next velocity setpoint.
+   * @return The computed feedforward, in volts.
+   */
+  constexpr units::volt_t Calculate(
+      units::unit_t<Velocity> currentVelocity,
+      units::unit_t<Velocity> nextVelocity) const {
+    // See wpimath/algorithms.md#Elevator_feedforward for derivation
+    if (kA < decltype(kA)(1e-9)) {
+      return kS * wpi::sgn(nextVelocity) + kG + kV * nextVelocity;
+    } else {
+      double A = -kV.value() / kA.value();
+      double B = 1.0 / kA.value();
+      double A_d = gcem::exp(A * m_dt.value());
+      double B_d = A > -1e-9 ? B * m_dt.value() : 1.0 / A * (A_d - 1.0) * B;
+      return kG + kS * wpi::sgn(currentVelocity) +
+             units::volt_t{
+                 1.0 / B_d *
+                 (nextVelocity.value() - A_d * currentVelocity.value())};
+    }
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param acceleration The acceleration of the elevator.
+   * @return The maximum possible velocity at the given acceleration.
+   */
+  constexpr units::unit_t<Velocity> MaxAchievableVelocity(
+      units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+    // Assume max velocity is positive
+    return (maxVoltage - kS - kG - kA * acceleration) / kV;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param acceleration The acceleration of the elevator.
+   * @return The minimum possible velocity at the given acceleration.
+   */
+  constexpr units::unit_t<Velocity> MinAchievableVelocity(
+      units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+    // Assume min velocity is negative, ks flips sign
+    return (-maxVoltage + kS - kG - kA * acceleration) / kV;
+  }
+
+  /**
+   * Calculates the maximum achievable acceleration given a maximum voltage
+   * supply and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param velocity The velocity of the elevator.
+   * @return The maximum possible acceleration at the given velocity.
+   */
+  constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
+      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+    return (maxVoltage - kS * wpi::sgn(velocity) - kG - kV * velocity) / kA;
+  }
+
+  /**
+   * Calculates the minimum achievable acceleration given a maximum voltage
+   * supply and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param velocity The velocity of the elevator.
+   * @return The minimum possible acceleration at the given velocity.
+   */
+  constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
+      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+    return MaxAchievableAcceleration(-maxVoltage, velocity);
+  }
+
+  /**
+   * Sets the static gain.
+   *
+   * @param kS The static gain.
+   */
+  constexpr void SetKs(units::volt_t kS) { this->kS = kS; }
+
+  /**
+   * Sets the gravity gain.
+   *
+   * @param kG The gravity gain.
+   */
+  constexpr void SetKg(units::volt_t kG) { this->kG = kG; }
+
+  /**
+   * Sets the velocity gain.
+   *
+   * @param kV The velocity gain.
+   */
+  constexpr void SetKv(units::unit_t<kv_unit> kV) { this->kV = kV; }
+
+  /**
+   * Sets the acceleration gain.
+   *
+   * @param kA The acceleration gain.
+   */
+  constexpr void SetKa(units::unit_t<ka_unit> kA) { this->kA = kA; }
+
+  /**
+   * Returns the static gain.
+   *
+   * @return The static gain.
+   */
+  constexpr units::volt_t GetKs() const { return kS; }
+
+  /**
+   * Returns the gravity gain.
+   *
+   * @return The gravity gain.
+   */
+  constexpr units::volt_t GetKg() const { return kG; }
+
+  /**
+   * Returns the velocity gain.
+   *
+   * @return The velocity gain.
+   */
+  constexpr units::unit_t<kv_unit> GetKv() const { return kV; }
+
+  /**
+   * Returns the acceleration gain.
+   *
+   * @return The acceleration gain.
+   */
+  constexpr units::unit_t<ka_unit> GetKa() const { return kA; }
+
+ private:
+  /// The static gain.
+  units::volt_t kS;
+
+  /// The gravity gain.
+  units::volt_t kG;
+
+  /// The velocity gain.
+  units::unit_t<kv_unit> kV;
+
+  /// The acceleration gain.
+  units::unit_t<ka_unit> kA;
+
+  /** The period. */
+  units::second_t m_dt;
+};
+}  // namespace frc
+
+#include "frc/controller/proto/ElevatorFeedforwardProto.h"
+#include "frc/controller/struct/ElevatorFeedforwardStruct.h"

wpimath/src/main/native/include/wpi/math/controller/ImplicitModelFollower.hpp

@@ -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.
+
+#pragma once
+
+#include <frc/system/LinearSystem.h>
+
+#include <Eigen/QR>
+
+#include "frc/EigenCore.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Contains the controller coefficients and logic for an implicit model
+ * follower.
+ *
+ * Implicit model following lets us design a feedback controller that erases the
+ * dynamics of our system and makes it behave like some other system. This can
+ * be used to make a drivetrain more controllable during teleop driving by
+ * making it behave like a slower or more benign drivetrain.
+ *
+ * For more on the underlying math, read appendix B.3 in
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ */
+template <int States, int Inputs>
+class ImplicitModelFollower {
+ public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param plant    The plant being controlled.
+   * @param plantRef The plant whose dynamics should be followed.
+   */
+  template <int Outputs>
+  ImplicitModelFollower(const LinearSystem<States, Inputs, Outputs>& plant,
+                        const LinearSystem<States, Inputs, Outputs>& plantRef)
+      : ImplicitModelFollower<States, Inputs>(plant.A(), plant.B(),
+                                              plantRef.A(), plantRef.B()) {}
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param A    Continuous system matrix of the plant being controlled.
+   * @param B    Continuous input matrix of the plant being controlled.
+   * @param Aref Continuous system matrix whose dynamics should be followed.
+   * @param Bref Continuous input matrix whose dynamics should be followed.
+   */
+  ImplicitModelFollower(const Matrixd<States, States>& A,
+                        const Matrixd<States, Inputs>& B,
+                        const Matrixd<States, States>& Aref,
+                        const Matrixd<States, Inputs>& Bref) {
+    // Find u_imf that makes real model match reference model.
+    //
+    // dx/dt = Ax + Bu_imf
+    // dz/dt = A_ref z + B_ref u
+    //
+    // Let x = z.
+    //
+    // dx/dt = dz/dt
+    // Ax + Bu_imf = Aref x + B_ref u
+    // Bu_imf = A_ref x - Ax + B_ref u
+    // Bu_imf = (A_ref - A)x + B_ref u
+    // u_imf = B⁻¹((A_ref - A)x + Bref u)
+    // u_imf = -B⁻¹(A - A_ref)x + B⁻¹B_ref u
+
+    // The first term makes the open-loop poles that of the reference
+    // system, and the second term makes the input behave like that of the
+    // reference system.
+    m_A = -B.householderQr().solve(A - Aref);
+    m_B = B.householderQr().solve(Bref);
+
+    Reset();
+  }
+
+  /**
+   * Returns the control input vector u.
+   *
+   * @return The control input.
+   */
+  const InputVector& U() const { return m_u; }
+
+  /**
+   * Returns an element of the control input vector u.
+   *
+   * @param i Row of u.
+   *
+   * @return The row of the control input vector.
+   */
+  double U(int i) const { return m_u(i); }
+
+  /**
+   * Resets the controller.
+   */
+  void Reset() { m_u.setZero(); }
+
+  /**
+   * Returns the next output of the controller.
+   *
+   * @param x The current state x.
+   * @param u The current input for the original model.
+   */
+  InputVector Calculate(const StateVector& x, const InputVector& u) {
+    m_u = m_A * x + m_B * u;
+    return m_u;
+  }
+
+ private:
+  // Computed controller output
+  InputVector m_u;
+
+  // State space conversion gain
+  Matrixd<Inputs, States> m_A;
+
+  // Input space conversion gain
+  Matrixd<Inputs, Inputs> m_B;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/controller/LTVDifferentialDriveController.hpp

@@ -0,0 +1,179 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <cmath>
+
+#include <Eigen/Core>
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/controller/DifferentialDriveWheelVoltages.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/system/LinearSystem.h"
+#include "frc/trajectory/Trajectory.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "units/velocity.h"
+
+namespace frc {
+
+/**
+ * The linear time-varying differential drive controller has a similar form to
+ * the LQR, but the model used to compute the controller gain is the nonlinear
+ * differential drive model linearized around the drivetrain's current state. We
+ * precompute gains for important places in our state-space, then interpolate
+ * between them with a lookup table to save computational resources.
+ *
+ * This controller has a flat hierarchy with pose and wheel velocity references
+ * and voltage outputs. This is different from a unicycle controller's nested
+ * hierarchy where the top-level controller has a pose reference and chassis
+ * velocity command outputs, and the low-level controller has wheel velocity
+ * references and voltage outputs. Flat hierarchies are easier to tune in one
+ * shot.
+ *
+ * See section 8.7 in Controls Engineering in FRC for a derivation of the
+ * control law we used shown in theorem 8.7.4.
+ */
+class WPILIB_DLLEXPORT LTVDifferentialDriveController {
+ public:
+  /**
+   * Constructs a linear time-varying differential drive controller.
+   *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
+   * @param plant      The differential drive velocity plant.
+   * @param trackwidth The distance between the differential drive's left and
+   *                   right wheels.
+   * @param Qelems     The maximum desired error tolerance for each state.
+   * @param Relems     The maximum desired control effort for each input.
+   * @param dt         Discretization timestep.
+   */
+  LTVDifferentialDriveController(const frc::LinearSystem<2, 2, 2>& plant,
+                                 units::meter_t trackwidth,
+                                 const wpi::array<double, 5>& Qelems,
+                                 const wpi::array<double, 2>& Relems,
+                                 units::second_t dt)
+      : m_trackwidth{trackwidth},
+        m_A{plant.A()},
+        m_B{plant.B()},
+        m_Q{frc::MakeCostMatrix(Qelems)},
+        m_R{frc::MakeCostMatrix(Relems)},
+        m_dt{dt} {}
+
+  /**
+   * Move constructor.
+   */
+  LTVDifferentialDriveController(LTVDifferentialDriveController&&) = default;
+
+  /**
+   * Move assignment operator.
+   */
+  LTVDifferentialDriveController& operator=(LTVDifferentialDriveController&&) =
+      default;
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   */
+  bool AtReference() const {
+    return std::abs(m_error(0)) < m_tolerance(0) &&
+           std::abs(m_error(1)) < m_tolerance(1) &&
+           std::abs(m_error(2)) < m_tolerance(2) &&
+           std::abs(m_error(3)) < m_tolerance(3) &&
+           std::abs(m_error(4)) < m_tolerance(4);
+  }
+
+  /**
+   * Sets the pose error which is considered tolerable for use with
+   * AtReference().
+   *
+   * @param poseTolerance Pose error which is tolerable.
+   * @param leftVelocityTolerance Left velocity error which is tolerable.
+   * @param rightVelocityTolerance Right velocity error which is tolerable.
+   */
+  void SetTolerance(const Pose2d& poseTolerance,
+                    units::meters_per_second_t leftVelocityTolerance,
+                    units::meters_per_second_t rightVelocityTolerance) {
+    m_tolerance = Eigen::Vector<double, 5>{
+        poseTolerance.X().value(), poseTolerance.Y().value(),
+        poseTolerance.Rotation().Radians().value(),
+        leftVelocityTolerance.value(), rightVelocityTolerance.value()};
+  }
+
+  /**
+   * Returns the left and right output voltages of the LTV controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose      The current pose.
+   * @param leftVelocity     The current left velocity.
+   * @param rightVelocity    The current right velocity.
+   * @param poseRef          The desired pose.
+   * @param leftVelocityRef  The desired left velocity.
+   * @param rightVelocityRef The desired right velocity.
+   */
+  DifferentialDriveWheelVoltages Calculate(
+      const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
+      units::meters_per_second_t rightVelocity, const Pose2d& poseRef,
+      units::meters_per_second_t leftVelocityRef,
+      units::meters_per_second_t rightVelocityRef);
+
+  /**
+   * Returns the left and right output voltages of the LTV controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose  The current pose.
+   * @param leftVelocity The left velocity.
+   * @param rightVelocity The right velocity.
+   * @param desiredState The desired pose, linear velocity, and angular velocity
+   *                     from a trajectory.
+   */
+  DifferentialDriveWheelVoltages Calculate(
+      const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
+      units::meters_per_second_t rightVelocity,
+      const Trajectory::State& desiredState) {
+    // v = (v_r + v_l) / 2     (1)
+    // w = (v_r - v_l) / (2r)  (2)
+    // k = w / v               (3)
+    //
+    // v_l = v - wr
+    // v_l = v - (vk)r
+    // v_l = v(1 - kr)
+    //
+    // v_r = v + wr
+    // v_r = v + (vk)r
+    // v_r = v(1 + kr)
+    return Calculate(
+        currentPose, leftVelocity, rightVelocity, desiredState.pose,
+        desiredState.velocity *
+            (1 - (desiredState.curvature / 1_rad * m_trackwidth / 2.0)),
+        desiredState.velocity *
+            (1 + (desiredState.curvature / 1_rad * m_trackwidth / 2.0)));
+  }
+
+ private:
+  units::meter_t m_trackwidth;
+
+  // Continuous velocity dynamics
+  Eigen::Matrix<double, 2, 2> m_A;
+  Eigen::Matrix<double, 2, 2> m_B;
+
+  // LQR cost matrices
+  Eigen::Matrix<double, 5, 5> m_Q;
+  Eigen::Matrix<double, 2, 2> m_R;
+
+  units::second_t m_dt;
+
+  Eigen::Vector<double, 5> m_error;
+  Eigen::Vector<double, 5> m_tolerance;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/controller/LTVUnicycleController.hpp

@@ -0,0 +1,145 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <Eigen/Core>
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/trajectory/Trajectory.h"
+#include "units/angular_velocity.h"
+#include "units/math.h"
+#include "units/time.h"
+#include "units/velocity.h"
+
+namespace frc {
+
+/**
+ * The linear time-varying unicycle controller has a similar form to the LQR,
+ * but the model used to compute the controller gain is the nonlinear unicycle
+ * model linearized around the drivetrain's current state.
+ *
+ * See section 8.9 in Controls Engineering in FRC for a derivation of the
+ * control law we used shown in theorem 8.9.1.
+ */
+class WPILIB_DLLEXPORT LTVUnicycleController {
+ public:
+  /**
+   * Constructs a linear time-varying unicycle controller with default maximum
+   * desired error tolerances of (x = 0.0625 m, y = 0.125 m, heading = 2 rad)
+   * and default maximum desired control effort of (linear velocity = 1 m/s,
+   * angular velocity = 2 rad/s).
+   *
+   * @param dt Discretization timestep.
+   */
+  explicit LTVUnicycleController(units::second_t dt)
+      : LTVUnicycleController{{0.0625, 0.125, 2.0}, {1.0, 2.0}, dt} {}
+
+  /**
+   * Constructs a linear time-varying unicycle controller.
+   *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
+   * @param Qelems The maximum desired error tolerance for each state (x, y,
+   *               heading).
+   * @param Relems The maximum desired control effort for each input (linear
+   *               velocity, angular velocity).
+   * @param dt     Discretization timestep.
+   */
+  LTVUnicycleController(const wpi::array<double, 3>& Qelems,
+                        const wpi::array<double, 2>& Relems, units::second_t dt)
+      : m_Q{frc::MakeCostMatrix(Qelems)},
+        m_R{frc::MakeCostMatrix(Relems)},
+        m_dt{dt} {}
+
+  /**
+   * Move constructor.
+   */
+  LTVUnicycleController(LTVUnicycleController&&) = default;
+
+  /**
+   * Move assignment operator.
+   */
+  LTVUnicycleController& operator=(LTVUnicycleController&&) = default;
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   */
+  bool AtReference() const {
+    const auto& eTranslate = m_poseError.Translation();
+    const auto& eRotate = m_poseError.Rotation();
+    const auto& tolTranslate = m_poseTolerance.Translation();
+    const auto& tolRotate = m_poseTolerance.Rotation();
+    return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
+           units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
+           units::math::abs(eRotate.Radians()) < tolRotate.Radians();
+  }
+
+  /**
+   * Sets the pose error which is considered tolerable for use with
+   * AtReference().
+   *
+   * @param poseTolerance Pose error which is tolerable.
+   */
+  void SetTolerance(const Pose2d& poseTolerance) {
+    m_poseTolerance = poseTolerance;
+  }
+
+  /**
+   * Returns the linear and angular velocity outputs of the LTV controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose        The current pose.
+   * @param poseRef            The desired pose.
+   * @param linearVelocityRef  The desired linear velocity.
+   * @param angularVelocityRef The desired angular velocity.
+   */
+  ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
+                          units::meters_per_second_t linearVelocityRef,
+                          units::radians_per_second_t angularVelocityRef);
+
+  /**
+   * Returns the linear and angular velocity outputs of the LTV controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose  The current pose.
+   * @param desiredState The desired pose, linear velocity, and angular velocity
+   *                     from a trajectory.
+   */
+  ChassisSpeeds Calculate(const Pose2d& currentPose,
+                          const Trajectory::State& desiredState) {
+    return Calculate(currentPose, desiredState.pose, desiredState.velocity,
+                     desiredState.velocity * desiredState.curvature);
+  }
+
+  /**
+   * Enables and disables the controller for troubleshooting purposes.
+   *
+   * @param enabled If the controller is enabled or not.
+   */
+  void SetEnabled(bool enabled) { m_enabled = enabled; }
+
+ private:
+  // LQR cost matrices
+  Eigen::Matrix<double, 3, 3> m_Q;
+  Eigen::Matrix<double, 2, 2> m_R;
+
+  units::second_t m_dt;
+
+  Pose2d m_poseError;
+  Pose2d m_poseTolerance;
+  bool m_enabled = true;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/controller/LinearPlantInversionFeedforward.hpp

@@ -0,0 +1,162 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <array>
+#include <functional>
+
+#include <Eigen/QR>
+
+#include "frc/EigenCore.h"
+#include "frc/system/Discretization.h"
+#include "frc/system/LinearSystem.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Constructs a plant inversion model-based feedforward from a LinearSystem.
+ *
+ * The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (r_k+1 - A
+ * r_k) </strong>, where <strong> B<sup>+</sup> </strong> is the pseudoinverse
+ * of B.
+ *
+ * For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ */
+template <int States, int Inputs>
+class LinearPlantInversionFeedforward {
+ public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+
+  /**
+   * Constructs a feedforward with the given plant.
+   *
+   * @tparam Outputs Number of outputs.
+   * @param plant The plant being controlled.
+   * @param dt    Discretization timestep.
+   */
+  template <int Outputs>
+  LinearPlantInversionFeedforward(
+      const LinearSystem<States, Inputs, Outputs>& plant, units::second_t dt)
+      : LinearPlantInversionFeedforward(plant.A(), plant.B(), dt) {}
+
+  /**
+   * Constructs a feedforward with the given coefficients.
+   *
+   * @param A  Continuous system matrix of the plant being controlled.
+   * @param B  Continuous input matrix of the plant being controlled.
+   * @param dt Discretization timestep.
+   */
+  LinearPlantInversionFeedforward(const Matrixd<States, States>& A,
+                                  const Matrixd<States, Inputs>& B,
+                                  units::second_t dt)
+      : m_dt(dt) {
+    DiscretizeAB<States, Inputs>(A, B, dt, &m_A, &m_B);
+    Reset();
+  }
+
+  /**
+   * Returns the previously calculated feedforward as an input vector.
+   *
+   * @return The calculated feedforward.
+   */
+  const InputVector& Uff() const { return m_uff; }
+
+  /**
+   * Returns an element of the previously calculated feedforward.
+   *
+   * @param i Row of uff.
+   *
+   * @return The row of the calculated feedforward.
+   */
+  double Uff(int i) const { return m_uff(i); }
+
+  /**
+   * Returns the current reference vector r.
+   *
+   * @return The current reference vector.
+   */
+  const StateVector& R() const { return m_r; }
+
+  /**
+   * Returns an element of the reference vector r.
+   *
+   * @param i Row of r.
+   *
+   * @return The row of the current reference vector.
+   */
+  double R(int i) const { return m_r(i); }
+
+  /**
+   * Resets the feedforward with a specified initial state vector.
+   *
+   * @param initialState The initial state vector.
+   */
+  void Reset(const StateVector& initialState) {
+    m_r = initialState;
+    m_uff.setZero();
+  }
+
+  /**
+   * Resets the feedforward with a zero initial state vector.
+   */
+  void Reset() {
+    m_r.setZero();
+    m_uff.setZero();
+  }
+
+  /**
+   * Calculate the feedforward with only the desired
+   * future reference. This uses the internally stored "current"
+   * reference.
+   *
+   * If this method is used the initial state of the system is the one set using
+   * Reset(const StateVector&). If the initial state is not
+   * set it defaults to a zero vector.
+   *
+   * @param nextR The reference state of the future timestep (k + dt).
+   *
+   * @return The calculated feedforward.
+   */
+  InputVector Calculate(const StateVector& nextR) {
+    return Calculate(m_r, nextR);
+  }
+
+  /**
+   * Calculate the feedforward with current and future reference vectors.
+   *
+   * @param r     The reference state of the current timestep (k).
+   * @param nextR The reference state of the future timestep (k + dt).
+   *
+   * @return The calculated feedforward.
+   */
+  InputVector Calculate(const StateVector& r, const StateVector& nextR) {
+    // rₖ₊₁ = Arₖ + Buₖ
+    // Buₖ = rₖ₊₁ − Arₖ
+    // uₖ = B⁺(rₖ₊₁ − Arₖ)
+    m_uff = m_B.householderQr().solve(nextR - (m_A * r));
+    m_r = nextR;
+    return m_uff;
+  }
+
+ private:
+  Matrixd<States, States> m_A;
+  Matrixd<States, Inputs> m_B;
+
+  units::second_t m_dt;
+
+  // Current reference
+  StateVector m_r;
+
+  // Computed feedforward
+  InputVector m_uff;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/controller/LinearQuadraticRegulator.hpp

@@ -0,0 +1,320 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdexcept>
+#include <string>
+
+#include <Eigen/Cholesky>
+#include <unsupported/Eigen/MatrixFunctions>
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/DARE.h"
+#include "frc/EigenCore.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/fmt/Eigen.h"
+#include "frc/system/Discretization.h"
+#include "frc/system/LinearSystem.h"
+#include "units/time.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+/**
+ * Contains the controller coefficients and logic for a linear-quadratic
+ * regulator (LQR).
+ * LQRs use the control law u = K(r - x).
+ *
+ * For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ */
+template <int States, int Inputs>
+class LinearQuadraticRegulator {
+ public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+
+  using StateArray = wpi::array<double, States>;
+  using InputArray = wpi::array<double, Inputs>;
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
+   * @tparam Outputs Number of outputs.
+   * @param plant  The plant being controlled.
+   * @param Qelems The maximum desired error tolerance for each state.
+   * @param Relems The maximum desired control effort for each input.
+   * @param dt     Discretization timestep.
+   * @throws std::invalid_argument If the system is unstabilizable.
+   */
+  template <int Outputs>
+  LinearQuadraticRegulator(const LinearSystem<States, Inputs, Outputs>& plant,
+                           const StateArray& Qelems, const InputArray& Relems,
+                           units::second_t dt)
+      : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
+   * @param A      Continuous system matrix of the plant being controlled.
+   * @param B      Continuous input matrix of the plant being controlled.
+   * @param Qelems The maximum desired error tolerance for each state.
+   * @param Relems The maximum desired control effort for each input.
+   * @param dt     Discretization timestep.
+   * @throws std::invalid_argument If the system is unstabilizable.
+   */
+  LinearQuadraticRegulator(const Matrixd<States, States>& A,
+                           const Matrixd<States, Inputs>& B,
+                           const StateArray& Qelems, const InputArray& Relems,
+                           units::second_t dt)
+      : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
+                                 MakeCostMatrix(Relems), dt) {}
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param A  Continuous system matrix of the plant being controlled.
+   * @param B  Continuous input matrix of the plant being controlled.
+   * @param Q  The state cost matrix.
+   * @param R  The input cost matrix.
+   * @param dt Discretization timestep.
+   * @throws std::invalid_argument If the system is unstabilizable.
+   */
+  LinearQuadraticRegulator(const Matrixd<States, States>& A,
+                           const Matrixd<States, Inputs>& B,
+                           const Matrixd<States, States>& Q,
+                           const Matrixd<Inputs, Inputs>& R,
+                           units::second_t dt) {
+    Matrixd<States, States> discA;
+    Matrixd<States, Inputs> discB;
+    DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
+
+    if (auto S = DARE<States, Inputs>(discA, discB, Q, R)) {
+      // K = (BᵀSB + R)⁻¹BᵀSA
+      m_K = (discB.transpose() * S.value() * discB + R)
+                .llt()
+                .solve(discB.transpose() * S.value() * discA);
+    } else if (S.error() == DAREError::QNotSymmetric ||
+               S.error() == DAREError::QNotPositiveSemidefinite) {
+      std::string msg = fmt::format("{}\n\nQ =\n{}\n", to_string(S.error()), Q);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    } else if (S.error() == DAREError::RNotSymmetric ||
+               S.error() == DAREError::RNotPositiveDefinite) {
+      std::string msg = fmt::format("{}\n\nR =\n{}\n", to_string(S.error()), R);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    } else if (S.error() == DAREError::ABNotStabilizable) {
+      std::string msg = fmt::format("{}\n\nA =\n{}\nB =\n{}\n",
+                                    to_string(S.error()), discA, discB);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    } else if (S.error() == DAREError::ACNotDetectable) {
+      std::string msg = fmt::format("{}\n\nA =\n{}\nQ =\n{}\n",
+                                    to_string(S.error()), discA, Q);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    }
+
+    Reset();
+  }
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param A  Continuous system matrix of the plant being controlled.
+   * @param B  Continuous input matrix of the plant being controlled.
+   * @param Q  The state cost matrix.
+   * @param R  The input cost matrix.
+   * @param N  The state-input cross-term cost matrix.
+   * @param dt Discretization timestep.
+   * @throws std::invalid_argument If the system is unstabilizable.
+   */
+  LinearQuadraticRegulator(const Matrixd<States, States>& A,
+                           const Matrixd<States, Inputs>& B,
+                           const Matrixd<States, States>& Q,
+                           const Matrixd<Inputs, Inputs>& R,
+                           const Matrixd<States, Inputs>& N,
+                           units::second_t dt) {
+    Matrixd<States, States> discA;
+    Matrixd<States, Inputs> discB;
+    DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
+
+    if (auto S = DARE<States, Inputs>(discA, discB, Q, R, N)) {
+      // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
+      m_K = (discB.transpose() * S.value() * discB + R)
+                .llt()
+                .solve(discB.transpose() * S.value() * discA + N.transpose());
+    } else if (S.error() == DAREError::QNotSymmetric ||
+               S.error() == DAREError::QNotPositiveSemidefinite) {
+      std::string msg = fmt::format("{}\n\nQ =\n{}\n", to_string(S.error()), Q);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    } else if (S.error() == DAREError::RNotSymmetric ||
+               S.error() == DAREError::RNotPositiveDefinite) {
+      std::string msg = fmt::format("{}\n\nR =\n{}\n", to_string(S.error()), R);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    } else if (S.error() == DAREError::ABNotStabilizable) {
+      std::string msg =
+          fmt::format("{}\n\nA =\n{}\nB =\n{}\n", to_string(S.error()),
+                      discA - discB * R.llt().solve(N.transpose()), discB);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    } else if (S.error() == DAREError::ACNotDetectable) {
+      auto R_llt = R.llt();
+      std::string msg =
+          fmt::format("{}\n\nA =\n{}\nQ =\n{}\n", to_string(S.error()),
+                      discA - discB * R_llt.solve(N.transpose()),
+                      Q - N * R_llt.solve(N.transpose()));
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    }
+
+    Reset();
+  }
+
+  LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
+  LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
+
+  /**
+   * Returns the controller matrix K.
+   */
+  const Matrixd<Inputs, States>& K() const { return m_K; }
+
+  /**
+   * Returns an element of the controller matrix K.
+   *
+   * @param i Row of K.
+   * @param j Column of K.
+   */
+  double K(int i, int j) const { return m_K(i, j); }
+
+  /**
+   * Returns the reference vector r.
+   *
+   * @return The reference vector.
+   */
+  const StateVector& R() const { return m_r; }
+
+  /**
+   * Returns an element of the reference vector r.
+   *
+   * @param i Row of r.
+   *
+   * @return The row of the reference vector.
+   */
+  double R(int i) const { return m_r(i); }
+
+  /**
+   * Returns the control input vector u.
+   *
+   * @return The control input.
+   */
+  const InputVector& U() const { return m_u; }
+
+  /**
+   * Returns an element of the control input vector u.
+   *
+   * @param i Row of u.
+   *
+   * @return The row of the control input vector.
+   */
+  double U(int i) const { return m_u(i); }
+
+  /**
+   * Resets the controller.
+   */
+  void Reset() {
+    m_r.setZero();
+    m_u.setZero();
+  }
+
+  /**
+   * Returns the next output of the controller.
+   *
+   * @param x The current state x.
+   */
+  InputVector Calculate(const StateVector& x) {
+    m_u = m_K * (m_r - x);
+    return m_u;
+  }
+
+  /**
+   * Returns the next output of the controller.
+   *
+   * @param x     The current state x.
+   * @param nextR The next reference vector r.
+   */
+  InputVector Calculate(const StateVector& x, const StateVector& nextR) {
+    m_r = nextR;
+    return Calculate(x);
+  }
+
+  /**
+   * Adjusts LQR controller gain to compensate for a pure time delay in the
+   * input.
+   *
+   * Linear-Quadratic regulator controller gains tend to be aggressive. If
+   * sensor measurements are time-delayed too long, the LQR may be unstable.
+   * However, if we know the amount of delay, we can compute the control based
+   * on where the system will be after the time delay.
+   *
+   * See https://file.tavsys.net/control/controls-engineering-in-frc.pdf
+   * appendix C.4 for a derivation.
+   *
+   * @param plant      The plant being controlled.
+   * @param dt         Discretization timestep.
+   * @param inputDelay Input time delay.
+   */
+  template <int Outputs>
+  void LatencyCompensate(const LinearSystem<States, Inputs, Outputs>& plant,
+                         units::second_t dt, units::second_t inputDelay) {
+    Matrixd<States, States> discA;
+    Matrixd<States, Inputs> discB;
+    DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
+
+    m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt);
+  }
+
+ private:
+  // Current reference
+  StateVector m_r;
+
+  // Computed controller output
+  InputVector m_u;
+
+  // Controller gain
+  Matrixd<Inputs, States> m_K;
+};
+
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    LinearQuadraticRegulator<1, 1>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    LinearQuadraticRegulator<2, 1>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    LinearQuadraticRegulator<2, 2>;
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/controller/PIDController.hpp

@@ -0,0 +1,460 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <type_traits>
+
+#include <gcem.hpp>
+#include <wpi/SymbolExports.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/MathUtil.h"
+#include "units/time.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+/**
+ * Implements a PID control loop.
+ */
+class WPILIB_DLLEXPORT PIDController
+    : public wpi::Sendable,
+      public wpi::SendableHelper<PIDController> {
+ public:
+  /**
+   * Allocates a PIDController with the given constants for Kp, Ki, and Kd.
+   *
+   * @param Kp     The proportional coefficient. Must be >= 0.
+   * @param Ki     The integral coefficient. Must be >= 0.
+   * @param Kd     The derivative coefficient. Must be >= 0.
+   * @param period The period between controller updates in seconds. The
+   *               default is 20 milliseconds. Must be positive.
+   */
+  constexpr PIDController(double Kp, double Ki, double Kd,
+                          units::second_t period = 20_ms)
+      : m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
+    bool invalidGains = false;
+    if (Kp < 0.0) {
+      wpi::math::MathSharedStore::ReportError(
+          "Kp must be a non-negative number, got {}!", Kp);
+      invalidGains = true;
+    }
+    if (Ki < 0.0) {
+      wpi::math::MathSharedStore::ReportError(
+          "Ki must be a non-negative number, got {}!", Ki);
+      invalidGains = true;
+    }
+    if (Kd < 0.0) {
+      wpi::math::MathSharedStore::ReportError(
+          "Kd must be a non-negative number, got {}!", Kd);
+      invalidGains = true;
+    }
+    if (invalidGains) {
+      m_Kp = 0.0;
+      m_Ki = 0.0;
+      m_Kd = 0.0;
+      wpi::math::MathSharedStore::ReportWarning("PID gains defaulted to 0.");
+    }
+
+    if (period <= 0_s) {
+      wpi::math::MathSharedStore::ReportError(
+          "Controller period must be a positive number, got {}!",
+          period.value());
+      m_period = 20_ms;
+      wpi::math::MathSharedStore::ReportWarning(
+          "Controller period defaulted to 20ms.");
+    }
+    if (!std::is_constant_evaluated()) {
+      ++instances;
+
+      wpi::math::MathSharedStore::ReportUsage("PIDController",
+                                              std::to_string(instances));
+      wpi::SendableRegistry::Add(this, "PIDController", instances);
+    }
+  }
+
+  constexpr ~PIDController() override = default;
+
+  constexpr PIDController(const PIDController&) = default;
+  constexpr PIDController& operator=(const PIDController&) = default;
+  constexpr PIDController(PIDController&&) = default;
+  constexpr PIDController& operator=(PIDController&&) = default;
+
+  /**
+   * Sets the PID Controller gain parameters.
+   *
+   * Sets the proportional, integral, and differential coefficients.
+   *
+   * @param Kp The proportional coefficient. Must be >= 0.
+   * @param Ki The integral coefficient. Must be >= 0.
+   * @param Kd The differential coefficient. Must be >= 0.
+   */
+  constexpr void SetPID(double Kp, double Ki, double Kd) {
+    m_Kp = Kp;
+    m_Ki = Ki;
+    m_Kd = Kd;
+  }
+
+  /**
+   * Sets the proportional coefficient of the PID controller gain.
+   *
+   * @param Kp The proportional coefficient. Must be >= 0.
+   */
+  constexpr void SetP(double Kp) { m_Kp = Kp; }
+
+  /**
+   * Sets the integral coefficient of the PID controller gain.
+   *
+   * @param Ki The integral coefficient. Must be >= 0.
+   */
+  constexpr void SetI(double Ki) { m_Ki = Ki; }
+
+  /**
+   * Sets the differential coefficient of the PID controller gain.
+   *
+   * @param Kd The differential coefficient. Must be >= 0.
+   */
+  constexpr void SetD(double Kd) { m_Kd = Kd; }
+
+  /**
+   * Sets the IZone range. When the absolute value of the position error is
+   * greater than IZone, the total accumulated error will reset to zero,
+   * disabling integral gain until the absolute value of the position error is
+   * less than IZone. This is used to prevent integral windup. Must be
+   * non-negative. Passing a value of zero will effectively disable integral
+   * gain. Passing a value of infinity disables IZone functionality.
+   *
+   * @param iZone Maximum magnitude of error to allow integral control. Must be
+   *   >= 0.
+   */
+  constexpr void SetIZone(double iZone) {
+    if (std::is_constant_evaluated() && iZone < 0) {
+      wpi::math::MathSharedStore::ReportError(
+          "IZone must be a non-negative number, got {}!", iZone);
+    }
+    m_iZone = iZone;
+  }
+
+  /**
+   * Gets the proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  constexpr double GetP() const { return m_Kp; }
+
+  /**
+   * Gets the integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  constexpr double GetI() const { return m_Ki; }
+
+  /**
+   * Gets the differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  constexpr double GetD() const { return m_Kd; }
+
+  /**
+   * Get the IZone range.
+   *
+   * @return Maximum magnitude of error to allow integral control.
+   */
+  constexpr double GetIZone() const { return m_iZone; }
+
+  /**
+   * Gets the period of this controller.
+   *
+   * @return The period of the controller.
+   */
+  constexpr units::second_t GetPeriod() const { return m_period; }
+
+  /**
+   * Gets the error tolerance of this controller. Defaults to 0.05.
+   *
+   * @return The error tolerance of the controller.
+   */
+  constexpr double GetErrorTolerance() const { return m_errorTolerance; }
+
+  /**
+   * Gets the error derivative tolerance of this controller. Defaults to ∞.
+   *
+   * @return The error derivative tolerance of the controller.
+   */
+  constexpr double GetErrorDerivativeTolerance() const {
+    return m_errorDerivativeTolerance;
+  }
+
+  /**
+   * Gets the position tolerance of this controller.
+   *
+   * @return The position tolerance of the controller.
+   * @deprecated Use GetErrorTolerance() instead.
+   */
+  [[deprecated("Use the GetErrorTolerance method instead.")]]
+  constexpr double GetPositionTolerance() const {
+    return m_errorTolerance;
+  }
+
+  /**
+   * Gets the velocity tolerance of this controller.
+   *
+   * @return The velocity tolerance of the controller.
+   * @deprecated Use GetErrorDerivativeTolerance() instead.
+   */
+  [[deprecated("Use the GetErrorDerivativeTolerance method instead.")]]
+  constexpr double GetVelocityTolerance() const {
+    return m_errorDerivativeTolerance;
+  }
+
+  /**
+   * Gets the accumulated error used in the integral calculation of this
+   * controller.
+   *
+   * @return The accumulated error of this controller.
+   */
+  constexpr double GetAccumulatedError() const { return m_totalError; }
+
+  /**
+   * Sets the setpoint for the PIDController.
+   *
+   * @param setpoint The desired setpoint.
+   */
+  constexpr void SetSetpoint(double setpoint) {
+    m_setpoint = setpoint;
+    m_haveSetpoint = true;
+
+    if (m_continuous) {
+      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+      m_error =
+          InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+    } else {
+      m_error = m_setpoint - m_measurement;
+    }
+
+    m_errorDerivative = (m_error - m_prevError) / m_period.value();
+  }
+
+  /**
+   * Returns the current setpoint of the PIDController.
+   *
+   * @return The current setpoint.
+   */
+  constexpr double GetSetpoint() const { return m_setpoint; }
+
+  /**
+   * Returns true if the error is within the tolerance of the setpoint.
+   * The error tolerance defauls to 0.05, and the error derivative tolerance
+   * defaults to ∞.
+   *
+   * This will return false until at least one input value has been computed.
+   */
+  constexpr bool AtSetpoint() const {
+    return m_haveMeasurement && m_haveSetpoint &&
+           gcem::abs(m_error) < m_errorTolerance &&
+           gcem::abs(m_errorDerivative) < m_errorDerivativeTolerance;
+  }
+
+  /**
+   * Enables continuous input.
+   *
+   * Rather then using the max and min input range as constraints, it considers
+   * them to be the same point and automatically calculates the shortest route
+   * to the setpoint.
+   *
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  constexpr void EnableContinuousInput(double minimumInput,
+                                       double maximumInput) {
+    m_continuous = true;
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
+  }
+
+  /**
+   * Disables continuous input.
+   */
+  constexpr void DisableContinuousInput() { m_continuous = false; }
+
+  /**
+   * Returns true if continuous input is enabled.
+   */
+  constexpr bool IsContinuousInputEnabled() const { return m_continuous; }
+
+  /**
+   * Sets the minimum and maximum contributions of the integral term.
+   *
+   * The internal integrator is clamped so that the integral term's contribution
+   * to the output stays between minimumIntegral and maximumIntegral. This
+   * prevents integral windup.
+   *
+   * @param minimumIntegral The minimum contribution of the integral term.
+   * @param maximumIntegral The maximum contribution of the integral term.
+   */
+  constexpr void SetIntegratorRange(double minimumIntegral,
+                                    double maximumIntegral) {
+    m_minimumIntegral = minimumIntegral;
+    m_maximumIntegral = maximumIntegral;
+  }
+
+  /**
+   * Sets the error which is considered tolerable for use with AtSetpoint().
+   *
+   * @param errorTolerance error which is tolerable.
+   * @param errorDerivativeTolerance error derivative which is tolerable.
+   */
+  constexpr void SetTolerance(double errorTolerance,
+                              double errorDerivativeTolerance =
+                                  std::numeric_limits<double>::infinity()) {
+    m_errorTolerance = errorTolerance;
+    m_errorDerivativeTolerance = errorDerivativeTolerance;
+  }
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   */
+  constexpr double GetError() const { return m_error; }
+
+  /**
+   * Returns the error derivative.
+   */
+  constexpr double GetErrorDerivative() const { return m_errorDerivative; }
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   * @deprecated Use GetError() instead.
+   */
+  [[deprecated("Use GetError method instead.")]]
+  constexpr double GetPositionError() const {
+    return m_error;
+  }
+
+  /**
+   * Returns the velocity error.
+   * @deprecated Use GetErrorDerivative() instead.
+   */
+  [[deprecated("Use GetErrorDerivative method instead.")]]
+  constexpr double GetVelocityError() const {
+    return m_errorDerivative;
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   */
+  constexpr double Calculate(double measurement) {
+    m_measurement = measurement;
+    m_prevError = m_error;
+    m_haveMeasurement = true;
+
+    if (m_continuous) {
+      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+      m_error =
+          InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+    } else {
+      m_error = m_setpoint - m_measurement;
+    }
+
+    m_errorDerivative = (m_error - m_prevError) / m_period.value();
+
+    // If the absolute value of the position error is outside of IZone, reset
+    // the total error
+    if (gcem::abs(m_error) > m_iZone) {
+      m_totalError = 0;
+    } else if (m_Ki != 0) {
+      m_totalError =
+          std::clamp(m_totalError + m_error * m_period.value(),
+                     m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
+    }
+
+    return m_Kp * m_error + m_Ki * m_totalError + m_Kd * m_errorDerivative;
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param setpoint The new setpoint of the controller.
+   */
+  constexpr double Calculate(double measurement, double setpoint) {
+    m_setpoint = setpoint;
+    m_haveSetpoint = true;
+    return Calculate(measurement);
+  }
+
+  /**
+   * Reset the previous error, the integral term, and disable the controller.
+   */
+  constexpr void Reset() {
+    m_error = 0;
+    m_prevError = 0;
+    m_totalError = 0;
+    m_errorDerivative = 0;
+    m_haveMeasurement = false;
+  }
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  // Factor for "proportional" control
+  double m_Kp;
+
+  // Factor for "integral" control
+  double m_Ki;
+
+  // Factor for "derivative" control
+  double m_Kd;
+
+  // The error range where "integral" control applies
+  double m_iZone = std::numeric_limits<double>::infinity();
+
+  // The period (in seconds) of the control loop running this controller
+  units::second_t m_period;
+
+  double m_maximumIntegral = 1.0;
+
+  double m_minimumIntegral = -1.0;
+
+  double m_maximumInput = 0;
+
+  double m_minimumInput = 0;
+
+  // Do the endpoints wrap around? eg. Absolute encoder
+  bool m_continuous = false;
+
+  // The error at the time of the most recent call to Calculate()
+  double m_error = 0;
+  double m_errorDerivative = 0;
+
+  // The error at the time of the second-most-recent call to Calculate() (used
+  // to compute velocity)
+  double m_prevError = 0;
+
+  // The sum of the errors for use in the integral calc
+  double m_totalError = 0;
+
+  // The error that is considered at setpoint.
+  double m_errorTolerance = 0.05;
+  double m_errorDerivativeTolerance = std::numeric_limits<double>::infinity();
+
+  double m_setpoint = 0;
+  double m_measurement = 0;
+
+  bool m_haveSetpoint = false;
+  bool m_haveMeasurement = false;
+
+  // Usage reporting instances
+  inline static int instances = 0;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/controller/ProfiledPIDController.hpp

@@ -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.
+
+#pragma once
+
+#include <cmath>
+#include <limits>
+#include <type_traits>
+
+#include <wpi/SymbolExports.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/MathUtil.h"
+#include "frc/controller/PIDController.h"
+#include "frc/trajectory/TrapezoidProfile.h"
+#include "units/time.h"
+
+namespace frc {
+namespace detail {
+WPILIB_DLLEXPORT
+int IncrementAndGetProfiledPIDControllerInstances();
+}  // namespace detail
+
+/**
+ * Implements a PID control loop whose setpoint is constrained by a trapezoid
+ * profile.
+ */
+template <class Distance>
+class ProfiledPIDController
+    : public wpi::Sendable,
+      public wpi::SendableHelper<ProfiledPIDController<Distance>> {
+ public:
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using Acceleration =
+      units::compound_unit<Velocity, units::inverse<units::seconds>>;
+  using Acceleration_t = units::unit_t<Acceleration>;
+  using State = typename TrapezoidProfile<Distance>::State;
+  using Constraints = typename TrapezoidProfile<Distance>::Constraints;
+
+  /**
+   * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
+   * Kd. Users should call reset() when they first start running the controller
+   * to avoid unwanted behavior.
+   *
+   * @param Kp          The proportional coefficient. Must be >= 0.
+   * @param Ki          The integral coefficient. Must be >= 0.
+   * @param Kd          The derivative coefficient. Must be >= 0.
+   * @param constraints Velocity and acceleration constraints for goal.
+   * @param period      The period between controller updates in seconds. The
+   *                    default is 20 milliseconds. Must be positive.
+   */
+  constexpr ProfiledPIDController(double Kp, double Ki, double Kd,
+                                  Constraints constraints,
+                                  units::second_t period = 20_ms)
+      : m_controller{Kp, Ki, Kd, period},
+        m_constraints{constraints},
+        m_profile{m_constraints} {
+    if (!std::is_constant_evaluated()) {
+      int instances = detail::IncrementAndGetProfiledPIDControllerInstances();
+      wpi::math::MathSharedStore::ReportUsage("ProfiledPIDController",
+                                              std::to_string(instances));
+      wpi::SendableRegistry::Add(this, "ProfiledPIDController", instances);
+    }
+  }
+
+  constexpr ~ProfiledPIDController() override = default;
+
+  constexpr ProfiledPIDController(const ProfiledPIDController&) = default;
+  constexpr ProfiledPIDController& operator=(const ProfiledPIDController&) =
+      default;
+  constexpr ProfiledPIDController(ProfiledPIDController&&) = default;
+  constexpr ProfiledPIDController& operator=(ProfiledPIDController&&) = default;
+
+  /**
+   * Sets the PID Controller gain parameters.
+   *
+   * Sets the proportional, integral, and differential coefficients.
+   *
+   * @param Kp The proportional coefficient. Must be >= 0.
+   * @param Ki The integral coefficient. Must be >= 0.
+   * @param Kd The differential coefficient. Must be >= 0.
+   */
+  constexpr void SetPID(double Kp, double Ki, double Kd) {
+    m_controller.SetPID(Kp, Ki, Kd);
+  }
+
+  /**
+   * Sets the proportional coefficient of the PID controller gain.
+   *
+   * @param Kp The proportional coefficient. Must be >= 0.
+   */
+  constexpr void SetP(double Kp) { m_controller.SetP(Kp); }
+
+  /**
+   * Sets the integral coefficient of the PID controller gain.
+   *
+   * @param Ki The integral coefficient. Must be >= 0.
+   */
+  constexpr void SetI(double Ki) { m_controller.SetI(Ki); }
+
+  /**
+   * Sets the differential coefficient of the PID controller gain.
+   *
+   * @param Kd The differential coefficient. Must be >= 0.
+   */
+  constexpr void SetD(double Kd) { m_controller.SetD(Kd); }
+
+  /**
+   * Sets the IZone range. When the absolute value of the position error is
+   * greater than IZone, the total accumulated error will reset to zero,
+   * disabling integral gain until the absolute value of the position error is
+   * less than IZone. This is used to prevent integral windup. Must be
+   * non-negative. Passing a value of zero will effectively disable integral
+   * gain. Passing a value of infinity disables IZone functionality.
+   *
+   * @param iZone Maximum magnitude of error to allow integral control. Must be
+   *   >= 0.
+   */
+  constexpr void SetIZone(double iZone) { m_controller.SetIZone(iZone); }
+
+  /**
+   * Gets the proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  constexpr double GetP() const { return m_controller.GetP(); }
+
+  /**
+   * Gets the integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  constexpr double GetI() const { return m_controller.GetI(); }
+
+  /**
+   * Gets the differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  constexpr double GetD() const { return m_controller.GetD(); }
+
+  /**
+   * Get the IZone range.
+   *
+   * @return Maximum magnitude of error to allow integral control.
+   */
+  constexpr double GetIZone() const { return m_controller.GetIZone(); }
+
+  /**
+   * Gets the period of this controller.
+   *
+   * @return The period of the controller.
+   */
+  constexpr units::second_t GetPeriod() const {
+    return m_controller.GetPeriod();
+  }
+
+  /**
+   * Gets the position tolerance of this controller.
+   *
+   * @return The position tolerance of the controller.
+   */
+  constexpr double GetPositionTolerance() const {
+    return m_controller.GetErrorTolerance();
+  }
+
+  /**
+   * Gets the velocity tolerance of this controller.
+   *
+   * @return The velocity tolerance of the controller.
+   */
+  constexpr double GetVelocityTolerance() const {
+    return m_controller.GetErrorDerivativeTolerance();
+  }
+
+  /**
+   * Gets the accumulated error used in the integral calculation of this
+   * controller.
+   *
+   * @return The accumulated error of this controller.
+   */
+  constexpr double GetAccumulatedError() const {
+    return m_controller.GetAccumulatedError();
+  }
+
+  /**
+   * Sets the goal for the ProfiledPIDController.
+   *
+   * @param goal The desired unprofiled setpoint.
+   */
+  constexpr void SetGoal(State goal) { m_goal = goal; }
+
+  /**
+   * Sets the goal for the ProfiledPIDController.
+   *
+   * @param goal The desired unprofiled setpoint.
+   */
+  constexpr void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t{0}}; }
+
+  /**
+   * Gets the goal for the ProfiledPIDController.
+   */
+  constexpr State GetGoal() const { return m_goal; }
+
+  /**
+   * Returns true if the error is within the tolerance of the error.
+   *
+   * This will return false until at least one input value has been computed.
+   */
+  constexpr bool AtGoal() const { return AtSetpoint() && m_goal == m_setpoint; }
+
+  /**
+   * Set velocity and acceleration constraints for goal.
+   *
+   * @param constraints Velocity and acceleration constraints for goal.
+   */
+  constexpr void SetConstraints(Constraints constraints) {
+    m_constraints = constraints;
+    m_profile = TrapezoidProfile<Distance>{m_constraints};
+  }
+
+  /**
+   * Get the velocity and acceleration constraints for this controller.
+   * @return Velocity and acceleration constraints.
+   */
+  constexpr Constraints GetConstraints() { return m_constraints; }
+
+  /**
+   * Returns the current setpoint of the ProfiledPIDController.
+   *
+   * @return The current setpoint.
+   */
+  constexpr State GetSetpoint() const { return m_setpoint; }
+
+  /**
+   * Returns true if the error is within the tolerance of the error.
+   *
+   * Currently this just reports on target as the actual value passes through
+   * the setpoint. Ideally it should be based on being within the tolerance for
+   * some period of time.
+   *
+   * This will return false until at least one input value has been computed.
+   */
+  constexpr bool AtSetpoint() const { return m_controller.AtSetpoint(); }
+
+  /**
+   * Enables continuous input.
+   *
+   * Rather then using the max and min input range as constraints, it considers
+   * them to be the same point and automatically calculates the shortest route
+   * to the setpoint.
+   *
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  constexpr void EnableContinuousInput(Distance_t minimumInput,
+                                       Distance_t maximumInput) {
+    m_controller.EnableContinuousInput(minimumInput.value(),
+                                       maximumInput.value());
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
+  }
+
+  /**
+   * Disables continuous input.
+   */
+  constexpr void DisableContinuousInput() {
+    m_controller.DisableContinuousInput();
+  }
+
+  /**
+   * Sets the minimum and maximum contributions of the integral term.
+   *
+   * The internal integrator is clamped so that the integral term's contribution
+   * to the output stays between minimumIntegral and maximumIntegral. This
+   * prevents integral windup.
+   *
+   * @param minimumIntegral The minimum contribution of the integral term.
+   * @param maximumIntegral The maximum contribution of the integral term.
+   */
+  constexpr void SetIntegratorRange(double minimumIntegral,
+                                    double maximumIntegral) {
+    m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
+  }
+
+  /**
+   * Sets the error which is considered tolerable for use with
+   * AtSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   * @param velocityTolerance Velocity error which is tolerable.
+   */
+  constexpr void SetTolerance(Distance_t positionTolerance,
+                              Velocity_t velocityTolerance = Velocity_t{
+                                  std::numeric_limits<double>::infinity()}) {
+    m_controller.SetTolerance(positionTolerance.value(),
+                              velocityTolerance.value());
+  }
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   *
+   * @return The error.
+   */
+  constexpr Distance_t GetPositionError() const {
+    return Distance_t{m_controller.GetError()};
+  }
+
+  /**
+   * Returns the change in error per second.
+   */
+  constexpr Velocity_t GetVelocityError() const {
+    return Velocity_t{m_controller.GetErrorDerivative()};
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   */
+  constexpr double Calculate(Distance_t measurement) {
+    if (m_controller.IsContinuousInputEnabled()) {
+      // Get error which is smallest distance between goal and measurement
+      auto errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+      auto goalMinDistance = frc::InputModulus<Distance_t>(
+          m_goal.position - measurement, -errorBound, errorBound);
+      auto setpointMinDistance = frc::InputModulus<Distance_t>(
+          m_setpoint.position - measurement, -errorBound, errorBound);
+
+      // Recompute the profile goal with the smallest error, thus giving the
+      // shortest path. The goal may be outside the input range after this
+      // operation, but that's OK because the controller will still go there and
+      // report an error of zero. In other words, the setpoint only needs to be
+      // offset from the measurement by the input range modulus; they don't need
+      // to be equal.
+      m_goal.position = goalMinDistance + measurement;
+      m_setpoint.position = setpointMinDistance + measurement;
+    }
+
+    m_setpoint = m_profile.Calculate(GetPeriod(), m_setpoint, m_goal);
+    return m_controller.Calculate(measurement.value(),
+                                  m_setpoint.position.value());
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   */
+  constexpr double Calculate(Distance_t measurement, State goal) {
+    SetGoal(goal);
+    return Calculate(measurement);
+  }
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   */
+  constexpr double Calculate(Distance_t measurement, Distance_t goal) {
+    SetGoal(goal);
+    return Calculate(measurement);
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal        The new goal of the controller.
+   * @param constraints Velocity and acceleration constraints for goal.
+   */
+  constexpr double Calculate(
+      Distance_t measurement, Distance_t goal,
+      typename frc::TrapezoidProfile<Distance>::Constraints constraints) {
+    SetConstraints(constraints);
+    return Calculate(measurement, goal);
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measurement The current measured State of the system.
+   */
+  constexpr void Reset(const State& measurement) {
+    m_controller.Reset();
+    m_setpoint = measurement;
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system.
+   * @param measuredVelocity The current measured velocity of the system.
+   */
+  constexpr void Reset(Distance_t measuredPosition,
+                       Velocity_t measuredVelocity) {
+    Reset(State{measuredPosition, measuredVelocity});
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system. The
+   * velocity is assumed to be zero.
+   */
+  constexpr void Reset(Distance_t measuredPosition) {
+    Reset(measuredPosition, Velocity_t{0});
+  }
+
+  void InitSendable(wpi::SendableBuilder& builder) override {
+    builder.SetSmartDashboardType("ProfiledPIDController");
+    builder.AddDoubleProperty(
+        "p", [this] { return GetP(); }, [this](double value) { SetP(value); });
+    builder.AddDoubleProperty(
+        "i", [this] { return GetI(); }, [this](double value) { SetI(value); });
+    builder.AddDoubleProperty(
+        "d", [this] { return GetD(); }, [this](double value) { SetD(value); });
+    builder.AddDoubleProperty(
+        "izone", [this] { return GetIZone(); },
+        [this](double value) { SetIZone(value); });
+    builder.AddDoubleProperty(
+        "maxVelocity", [this] { return GetConstraints().maxVelocity.value(); },
+        [this](double value) {
+          SetConstraints(
+              Constraints{Velocity_t{value}, GetConstraints().maxAcceleration});
+        });
+    builder.AddDoubleProperty(
+        "maxAcceleration",
+        [this] { return GetConstraints().maxAcceleration.value(); },
+        [this](double value) {
+          SetConstraints(
+              Constraints{GetConstraints().maxVelocity, Acceleration_t{value}});
+        });
+    builder.AddDoubleProperty(
+        "goal", [this] { return GetGoal().position.value(); },
+        [this](double value) { SetGoal(Distance_t{value}); });
+  }
+
+ private:
+  PIDController m_controller;
+  Distance_t m_minimumInput{0};
+  Distance_t m_maximumInput{0};
+
+  typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
+  TrapezoidProfile<Distance> m_profile;
+  typename frc::TrapezoidProfile<Distance>::State m_goal;
+  typename frc::TrapezoidProfile<Distance>::State m_setpoint;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/controller/SimpleMotorFeedforward.hpp

@@ -0,0 +1,244 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <gcem.hpp>
+#include <wpi/MathExtras.h>
+
+#include "units/angle.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "units/voltage.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+/**
+ * A helper class that computes feedforward voltages for a simple
+ * permanent-magnet DC motor.
+ */
+template <class Distance>
+  requires units::length_unit<Distance> || units::angle_unit<Distance>
+class SimpleMotorFeedforward {
+ public:
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Acceleration =
+      units::compound_unit<Velocity, units::inverse<units::seconds>>;
+  using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
+  using ka_unit =
+      units::compound_unit<units::volts, units::inverse<Acceleration>>;
+
+  /**
+   * Creates a new SimpleMotorFeedforward with the specified gains.
+   *
+   * @param kS The static gain, in volts.
+   * @param kV The velocity gain, in volt seconds per distance.
+   * @param kA The acceleration gain, in volt seconds² per distance.
+   * @param dt The period in seconds.
+   * @throws IllegalArgumentException for kv &lt; zero.
+   * @throws IllegalArgumentException for ka &lt; zero.
+   * @throws IllegalArgumentException for period &le; zero.
+   */
+  constexpr SimpleMotorFeedforward(
+      units::volt_t kS, units::unit_t<kv_unit> kV,
+      units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0),
+      units::second_t dt = 20_ms)
+      : kS(kS), kV(kV), kA(kA), m_dt(dt) {
+    if (kV.value() < 0) {
+      wpi::math::MathSharedStore::ReportError(
+          "kV must be a non-negative number, got {}!", kV.value());
+      this->kV = units::unit_t<kv_unit>{0};
+      wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
+    }
+    if (kA.value() < 0) {
+      wpi::math::MathSharedStore::ReportError(
+          "kA must be a non-negative number, got {}!", kA.value());
+      this->kA = units::unit_t<ka_unit>{0};
+      wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0.");
+    }
+    if (dt <= 0_ms) {
+      wpi::math::MathSharedStore::ReportError(
+          "period must be a positive number, got {}!", dt.value());
+      this->m_dt = 20_ms;
+      wpi::math::MathSharedStore::ReportWarning("period defaulted to 20 ms.");
+    }
+  }
+
+  /**
+   * Calculates the feedforward from the gains and velocity setpoint assuming
+   * discrete control. Use this method when the velocity setpoint does not
+   * change.
+   *
+   * @param velocity The velocity setpoint.
+   * @return The computed feedforward, in volts.
+   */
+  constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity) const {
+    return Calculate(velocity, velocity);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints assuming discrete
+   * control.
+   *
+   * <p>Note this method is inaccurate when the velocity crosses 0.
+   *
+   * @param currentVelocity The current velocity setpoint.
+   * @param nextVelocity    The next velocity setpoint.
+   * @return The computed feedforward, in volts.
+   */
+  constexpr units::volt_t Calculate(
+      units::unit_t<Velocity> currentVelocity,
+      units::unit_t<Velocity> nextVelocity) const {
+    // See wpimath/algorithms.md#Simple_motor_feedforward for derivation
+    if (kA < decltype(kA)(1e-9)) {
+      return kS * wpi::sgn(nextVelocity) + kV * nextVelocity;
+    } else {
+      double A = -kV.value() / kA.value();
+      double B = 1.0 / kA.value();
+      double A_d = gcem::exp(A * m_dt.value());
+      double B_d = A > -1e-9 ? B * m_dt.value() : 1.0 / A * (A_d - 1.0) * B;
+      return kS * wpi::sgn(currentVelocity) +
+             units::volt_t{
+                 1.0 / B_d *
+                 (nextVelocity.value() - A_d * currentVelocity.value())};
+    }
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param acceleration The acceleration of the motor.
+   * @return The maximum possible velocity at the given acceleration.
+   */
+  constexpr units::unit_t<Velocity> MaxAchievableVelocity(
+      units::volt_t maxVoltage,
+      units::unit_t<Acceleration> acceleration) const {
+    // Assume max velocity is positive
+    return (maxVoltage - kS - kA * acceleration) / kV;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param acceleration The acceleration of the motor.
+   * @return The minimum possible velocity at the given acceleration.
+   */
+  constexpr units::unit_t<Velocity> MinAchievableVelocity(
+      units::volt_t maxVoltage,
+      units::unit_t<Acceleration> acceleration) const {
+    // Assume min velocity is positive, ks flips sign
+    return (-maxVoltage + kS - kA * acceleration) / kV;
+  }
+
+  /**
+   * Calculates the maximum achievable acceleration given a maximum voltage
+   * supply and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param velocity The velocity of the motor.
+   * @return The maximum possible acceleration at the given velocity.
+   */
+  constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
+      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) const {
+    return (maxVoltage - kS * wpi::sgn(velocity) - kV * velocity) / kA;
+  }
+
+  /**
+   * Calculates the minimum achievable acceleration given a maximum voltage
+   * supply and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param velocity The velocity of the motor.
+   * @return The minimum possible acceleration at the given velocity.
+   */
+  constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
+      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) const {
+    return MaxAchievableAcceleration(-maxVoltage, velocity);
+  }
+
+  /**
+   * Sets the static gain.
+   *
+   * @param kS The static gain.
+   */
+  constexpr void SetKs(units::volt_t kS) { this->kS = kS; }
+
+  /**
+   * Sets the velocity gain.
+   *
+   * @param kV The velocity gain.
+   */
+  constexpr void SetKv(units::unit_t<kv_unit> kV) { this->kV = kV; }
+
+  /**
+   * Sets the acceleration gain.
+   *
+   * @param kA The acceleration gain.
+   */
+  constexpr void SetKa(units::unit_t<ka_unit> kA) { this->kA = kA; }
+
+  /**
+   * Returns the static gain.
+   *
+   * @return The static gain.
+   */
+  constexpr units::volt_t GetKs() const { return kS; }
+
+  /**
+   * Returns the velocity gain.
+   *
+   * @return The velocity gain.
+   */
+  constexpr units::unit_t<kv_unit> GetKv() const { return kV; }
+
+  /**
+   * Returns the acceleration gain.
+   *
+   * @return The acceleration gain.
+   */
+  constexpr units::unit_t<ka_unit> GetKa() const { return kA; }
+
+  /**
+   * Returns the period.
+   *
+   * @return The period.
+   */
+  constexpr units::second_t GetDt() const { return m_dt; }
+
+ private:
+  /** The static gain. */
+  units::volt_t kS;
+
+  /** The velocity gain. */
+  units::unit_t<kv_unit> kV;
+
+  /** The acceleration gain. */
+  units::unit_t<ka_unit> kA;
+
+  /** The period. */
+  units::second_t m_dt;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/controller/proto/ArmFeedforwardProto.hpp

@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/controller/ArmFeedforward.h"
+#include "pb.h"
+#include "wpimath/protobuf/controller.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::ArmFeedforward> {
+  using MessageStruct = wpi_proto_ProtobufArmFeedforward;
+  using InputStream = wpi::ProtoInputStream<frc::ArmFeedforward>;
+  using OutputStream = wpi::ProtoOutputStream<frc::ArmFeedforward>;
+  static std::optional<frc::ArmFeedforward> Unpack(InputStream& stream);
+  static bool Pack(OutputStream& stream, const frc::ArmFeedforward& value);
+};

wpimath/src/main/native/include/wpi/math/controller/proto/DifferentialDriveFeedforwardProto.hpp

@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/controller/DifferentialDriveFeedforward.h"
+#include "pb.h"
+#include "wpimath/protobuf/controller.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::DifferentialDriveFeedforward> {
+  using MessageStruct = wpi_proto_ProtobufDifferentialDriveFeedforward;
+  using InputStream = wpi::ProtoInputStream<frc::DifferentialDriveFeedforward>;
+  using OutputStream =
+      wpi::ProtoOutputStream<frc::DifferentialDriveFeedforward>;
+  static std::optional<frc::DifferentialDriveFeedforward> Unpack(
+      InputStream& stream);
+  static bool Pack(OutputStream& stream,
+                   const frc::DifferentialDriveFeedforward& value);
+};

wpimath/src/main/native/include/wpi/math/controller/proto/DifferentialDriveWheelVoltagesProto.hpp

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/controller/DifferentialDriveWheelVoltages.h"
+#include "pb.h"
+#include "wpimath/protobuf/controller.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::DifferentialDriveWheelVoltages> {
+  using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelVoltages;
+  using InputStream =
+      wpi::ProtoInputStream<frc::DifferentialDriveWheelVoltages>;
+  using OutputStream =
+      wpi::ProtoOutputStream<frc::DifferentialDriveWheelVoltages>;
+  static std::optional<frc::DifferentialDriveWheelVoltages> Unpack(
+      InputStream& stream);
+  static bool Pack(OutputStream& stream,
+                   const frc::DifferentialDriveWheelVoltages& value);
+};

wpimath/src/main/native/include/wpi/math/controller/proto/ElevatorFeedforwardProto.hpp

@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/controller/ElevatorFeedforward.h"
+#include "pb.h"
+#include "wpimath/protobuf/controller.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::ElevatorFeedforward> {
+  using MessageStruct = wpi_proto_ProtobufElevatorFeedforward;
+  using InputStream = wpi::ProtoInputStream<frc::ElevatorFeedforward>;
+  using OutputStream = wpi::ProtoOutputStream<frc::ElevatorFeedforward>;
+  static std::optional<frc::ElevatorFeedforward> Unpack(InputStream& stream);
+  static bool Pack(OutputStream& stream, const frc::ElevatorFeedforward& value);
+};

wpimath/src/main/native/include/wpi/math/controller/proto/SimpleMotorFeedforwardProto.hpp

@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/controller/SimpleMotorFeedforward.h"
+#include "pb.h"
+#include "units/length.h"
+#include "wpimath/protobuf/controller.npb.h"
+
+// Everything is converted into units for
+// frc::SimpleMotorFeedforward<units::meters> or
+// frc::SimpleMotorFeedforward<units::radians>
+
+template <class Distance>
+  requires units::length_unit<Distance> || units::angle_unit<Distance>
+struct wpi::Protobuf<frc::SimpleMotorFeedforward<Distance>> {
+  using MessageStruct = wpi_proto_ProtobufSimpleMotorFeedforward;
+  using InputStream =
+      wpi::ProtoInputStream<frc::SimpleMotorFeedforward<Distance>>;
+  using OutputStream =
+      wpi::ProtoOutputStream<frc::SimpleMotorFeedforward<Distance>>;
+
+  static std::optional<frc::SimpleMotorFeedforward<Distance>> Unpack(
+      InputStream& stream) {
+    using BaseUnit =
+        units::unit<std::ratio<1>, units::traits::base_unit_of<Distance>>;
+    using BaseFeedforward = frc::SimpleMotorFeedforward<BaseUnit>;
+    wpi_proto_ProtobufSimpleMotorFeedforward msg;
+    if (!stream.Decode(msg)) {
+      return {};
+    }
+
+    return frc::SimpleMotorFeedforward<Distance>{
+        units::volt_t{msg.ks},
+        units::unit_t<typename BaseFeedforward::kv_unit>{msg.kv},
+        units::unit_t<typename BaseFeedforward::ka_unit>{msg.ka},
+        units::second_t{msg.dt},
+    };
+  }
+
+  static bool Pack(OutputStream& stream,
+                   const frc::SimpleMotorFeedforward<Distance>& value) {
+    using BaseUnit =
+        units::unit<std::ratio<1>, units::traits::base_unit_of<Distance>>;
+    using BaseFeedforward = frc::SimpleMotorFeedforward<BaseUnit>;
+    wpi_proto_ProtobufSimpleMotorFeedforward msg{
+        .ks = value.GetKs().value(),
+        .kv = units::unit_t<typename BaseFeedforward::kv_unit>{value.GetKv()}
+                  .value(),
+        .ka = units::unit_t<typename BaseFeedforward::ka_unit>{value.GetKa()}
+                  .value(),
+        .dt = units::second_t{value.GetDt()}.value(),
+    };
+    return stream.Encode(msg);
+  }
+};

wpimath/src/main/native/include/wpi/math/controller/struct/ArmFeedforwardStruct.hpp

@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/controller/ArmFeedforward.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::ArmFeedforward> {
+  static constexpr std::string_view GetTypeName() { return "ArmFeedforward"; }
+  static constexpr size_t GetSize() { return 32; }
+  static constexpr std::string_view GetSchema() {
+    return "double ks;double kg;double kv;double ka";
+  }
+
+  static frc::ArmFeedforward Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::ArmFeedforward& value);
+};
+
+static_assert(wpi::StructSerializable<frc::ArmFeedforward>);

wpimath/src/main/native/include/wpi/math/controller/struct/DifferentialDriveFeedforwardStruct.hpp

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/controller/DifferentialDriveFeedforward.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveFeedforward> {
+  static constexpr std::string_view GetTypeName() {
+    return "DifferentialDriveFeedforward";
+  }
+  static constexpr size_t GetSize() { return 32; }
+  static constexpr std::string_view GetSchema() {
+    return "double kv_linear;double ka_linear;double kv_angular;double "
+           "ka_angular";
+  }
+
+  static frc::DifferentialDriveFeedforward Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::DifferentialDriveFeedforward& value);
+};
+
+static_assert(wpi::StructSerializable<frc::DifferentialDriveFeedforward>);

wpimath/src/main/native/include/wpi/math/controller/struct/DifferentialDriveWheelVoltagesStruct.hpp

@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/controller/DifferentialDriveWheelVoltages.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveWheelVoltages> {
+  static constexpr std::string_view GetTypeName() {
+    return "DifferentialDriveWheelVoltages";
+  }
+  static constexpr size_t GetSize() { return 16; }
+  static constexpr std::string_view GetSchema() {
+    return "double left;double right";
+  }
+
+  static frc::DifferentialDriveWheelVoltages Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::DifferentialDriveWheelVoltages& value);
+};
+
+static_assert(wpi::StructSerializable<frc::DifferentialDriveWheelVoltages>);

wpimath/src/main/native/include/wpi/math/controller/struct/ElevatorFeedforwardStruct.hpp

@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/controller/ElevatorFeedforward.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::ElevatorFeedforward> {
+  static constexpr std::string_view GetTypeName() {
+    return "ElevatorFeedforward";
+  }
+  static constexpr size_t GetSize() { return 32; }
+  static constexpr std::string_view GetSchema() {
+    return "double ks;double kg;double kv;double ka";
+  }
+
+  static frc::ElevatorFeedforward Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::ElevatorFeedforward& value);
+};
+
+static_assert(wpi::StructSerializable<frc::ElevatorFeedforward>);

wpimath/src/main/native/include/wpi/math/controller/struct/SimpleMotorFeedforwardStruct.hpp

@@ -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.
+
+#pragma once
+
+#include <wpi/struct/Struct.h>
+
+#include "frc/controller/SimpleMotorFeedforward.h"
+#include "units/length.h"
+
+// Everything is converted into units for
+// frc::SimpleMotorFeedforward<units::meters> or
+// frc::SimpleMotorFeedforward<units::radians>
+
+template <class Distance>
+  requires units::length_unit<Distance> || units::angle_unit<Distance>
+struct wpi::Struct<frc::SimpleMotorFeedforward<Distance>> {
+  static constexpr std::string_view GetTypeName() {
+    return "SimpleMotorFeedforward";
+  }
+  static constexpr size_t GetSize() { return 32; }
+  static constexpr std::string_view GetSchema() {
+    return "double ks;double kv;double ka;double dt";
+  }
+
+  static frc::SimpleMotorFeedforward<Distance> Unpack(
+      std::span<const uint8_t> data) {
+    using BaseUnit =
+        units::unit<std::ratio<1>, units::traits::base_unit_of<Distance>>;
+    using BaseFeedforward = frc::SimpleMotorFeedforward<BaseUnit>;
+    constexpr size_t kKsOff = 0;
+    constexpr size_t kKvOff = kKsOff + 8;
+    constexpr size_t kKaOff = kKvOff + 8;
+    constexpr size_t kDtOff = kKaOff + 8;
+    return {units::volt_t{wpi::UnpackStruct<double, kKsOff>(data)},
+            units::unit_t<typename BaseFeedforward::kv_unit>{
+                wpi::UnpackStruct<double, kKvOff>(data)},
+            units::unit_t<typename BaseFeedforward::ka_unit>{
+                wpi::UnpackStruct<double, kKaOff>(data)},
+            units::second_t{wpi::UnpackStruct<double, kDtOff>(data)}};
+  }
+
+  static void Pack(std::span<uint8_t> data,
+                   const frc::SimpleMotorFeedforward<Distance>& value) {
+    using BaseUnit =
+        units::unit<std::ratio<1>, units::traits::base_unit_of<Distance>>;
+    using BaseFeedforward = frc::SimpleMotorFeedforward<BaseUnit>;
+    constexpr size_t kKsOff = 0;
+    constexpr size_t kKvOff = kKsOff + 8;
+    constexpr size_t kKaOff = kKvOff + 8;
+    constexpr size_t kDtOff = kKaOff + 8;
+    wpi::PackStruct<kKsOff>(data, value.GetKs().value());
+    wpi::PackStruct<kKvOff>(
+        data, units::unit_t<typename BaseFeedforward::kv_unit>{value.GetKv()}
+                  .value());
+    wpi::PackStruct<kKaOff>(
+        data, units::unit_t<typename BaseFeedforward::ka_unit>{value.GetKa()}
+                  .value());
+    wpi::PackStruct<kDtOff>(data, units::second_t{value.GetDt()}.value());
+  }
+};
+
+static_assert(
+    wpi::StructSerializable<frc::SimpleMotorFeedforward<units::meters>>);
+static_assert(
+    wpi::StructSerializable<frc::SimpleMotorFeedforward<units::feet>>);
+static_assert(
+    wpi::StructSerializable<frc::SimpleMotorFeedforward<units::radians>>);

wpimath/src/main/native/include/wpi/math/estimator/AngleStatistics.hpp

@@ -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.
+
+#pragma once
+
+#include <functional>
+#include <numbers>
+
+#include "frc/EigenCore.h"
+#include "frc/MathUtil.h"
+
+namespace frc {
+
+/**
+ * Subtracts a and b while normalizing the resulting value in the selected row
+ * as if it were an angle.
+ *
+ * @tparam States Number of states.
+ * @param a A vector to subtract from.
+ * @param b A vector to subtract with.
+ * @param angleStateIdx The row containing angles to be normalized.
+ */
+template <int States>
+Vectord<States> AngleResidual(const Vectord<States>& a,
+                              const Vectord<States>& b, int angleStateIdx) {
+  Vectord<States> ret = a - b;
+  ret[angleStateIdx] =
+      AngleModulus(units::radian_t{ret[angleStateIdx]}).value();
+  return ret;
+}
+
+/**
+ * Returns a function that subtracts two vectors while normalizing the resulting
+ * value in the selected row as if it were an angle.
+ *
+ * @tparam States Number of states.
+ * @param angleStateIdx The row containing angles to be normalized.
+ */
+template <int States>
+std::function<Vectord<States>(const Vectord<States>&, const Vectord<States>&)>
+AngleResidual(int angleStateIdx) {
+  return [=](auto a, auto b) {
+    return AngleResidual<States>(a, b, angleStateIdx);
+  };
+}
+
+/**
+ * Adds a and b while normalizing the resulting value in the selected row as an
+ * angle.
+ *
+ * @tparam States Number of states.
+ * @param a A vector to add with.
+ * @param b A vector to add with.
+ * @param angleStateIdx The row containing angles to be normalized.
+ */
+template <int States>
+Vectord<States> AngleAdd(const Vectord<States>& a, const Vectord<States>& b,
+                         int angleStateIdx) {
+  Vectord<States> ret = a + b;
+  ret[angleStateIdx] =
+      InputModulus(ret[angleStateIdx], -std::numbers::pi, std::numbers::pi);
+  return ret;
+}
+
+/**
+ * Returns a function that adds two vectors while normalizing the resulting
+ * value in the selected row as an angle.
+ *
+ * @tparam States Number of states.
+ * @param angleStateIdx The row containing angles to be normalized.
+ */
+template <int States>
+std::function<Vectord<States>(const Vectord<States>&, const Vectord<States>&)>
+AngleAdd(int angleStateIdx) {
+  return [=](auto a, auto b) { return AngleAdd<States>(a, b, angleStateIdx); };
+}
+
+/**
+ * Computes the mean of sigmas with the weights Wm while computing a special
+ * angle mean for a select row.
+ *
+ * @tparam CovDim Dimension of covariance of sigma points after passing through
+ *                the transform.
+ * @tparam NumSigmas Number of sigma points.
+ * @param sigmas Sigma points.
+ * @param Wm Weights for the mean.
+ * @param angleStatesIdx The row containing the angles.
+ */
+template <int CovDim, int NumSigmas>
+Vectord<CovDim> AngleMean(const Matrixd<CovDim, NumSigmas>& sigmas,
+                          const Vectord<NumSigmas>& Wm, int angleStatesIdx) {
+  double sumSin = (sigmas.row(angleStatesIdx).unaryExpr([](auto it) {
+                    return std::sin(it);
+                  }) *
+                   Wm)
+                      .sum();
+  double sumCos = (sigmas.row(angleStatesIdx).unaryExpr([](auto it) {
+                    return std::cos(it);
+                  }) *
+                   Wm)
+                      .sum();
+
+  Vectord<CovDim> ret = sigmas * Wm;
+  ret[angleStatesIdx] = std::atan2(sumSin, sumCos);
+  return ret;
+}
+
+/**
+ * Returns a function that computes the mean of sigmas with the weights Wm while
+ * computing a special angle mean for a select row.
+ *
+ * @tparam CovDim Dimension of covariance of sigma points after passing through
+ *                the transform.
+ * @tparam NumSigmas Number of sigma points.
+ * @param angleStateIdx The row containing the angles.
+ */
+template <int CovDim, int NumSigmas>
+std::function<Vectord<CovDim>(const Matrixd<CovDim, NumSigmas>&,
+                              const Vectord<NumSigmas>&)>
+AngleMean(int angleStateIdx) {
+  return [=](auto sigmas, auto Wm) {
+    return AngleMean<CovDim, NumSigmas>(sigmas, Wm, angleStateIdx);
+  };
+}
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator.hpp

@@ -0,0 +1,134 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/estimator/PoseEstimator.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/DifferentialDriveOdometry.h"
+#include "units/time.h"
+
+namespace frc {
+/**
+ * This class wraps Differential Drive Odometry to fuse latency-compensated
+ * vision measurements with differential drive encoder measurements. It will
+ * correct for noisy vision measurements and encoder drift. It is intended to be
+ * an easy drop-in for DifferentialDriveOdometry. In fact, if you never call
+ * AddVisionMeasurement(), and only call Update(), this will behave exactly the
+ * same as DifferentialDriveOdometry.
+ *
+ * Update() should be called every robot loop (if your robot loops are faster or
+ * slower than the default of 20 ms, then you should change the nominal delta
+ * time via the constructor).
+ *
+ * AddVisionMeasurement() can be called as infrequently as you want; if you
+ * never call it, then this class will behave like regular encoder odometry.
+ */
+class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
+    : public PoseEstimator<DifferentialDriveWheelSpeeds,
+                           DifferentialDriveWheelPositions> {
+ public:
+  /**
+   * Constructs a DifferentialDrivePoseEstimator with default standard
+   * deviations for the model and vision measurements.
+   *
+   * The default standard deviations of the model states are
+   * 0.02 meters for x, 0.02 meters for y, and 0.01 radians for heading.
+   * The default standard deviations of the vision measurements are
+   * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The gyro angle of the robot.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @param initialPose The estimated initial pose.
+   */
+  DifferentialDrivePoseEstimator(DifferentialDriveKinematics& kinematics,
+                                 const Rotation2d& gyroAngle,
+                                 units::meter_t leftDistance,
+                                 units::meter_t rightDistance,
+                                 const Pose2d& initialPose);
+
+  /**
+   * Constructs a DifferentialDrivePoseEstimator.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The gyro angle of the robot.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @param initialPose The estimated initial pose.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in
+   *     meters, y position in meters, and heading in radians). Increase these
+   *     numbers to trust your state estimate less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, and heading in
+   *     radians). Increase these numbers to trust the vision pose measurement
+   *     less.
+   */
+  DifferentialDrivePoseEstimator(
+      DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+      units::meter_t leftDistance, units::meter_t rightDistance,
+      const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
+      const wpi::array<double, 3>& visionMeasurementStdDevs);
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @param pose The estimated pose of the robot on the field.
+   */
+  void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
+                     units::meter_t rightDistance, const Pose2d& pose) {
+    PoseEstimator::ResetPosition(gyroAngle, {leftDistance, rightDistance},
+                                 pose);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This
+   * should be called every loop.
+   *
+   * @param gyroAngle     The current gyro angle.
+   * @param leftDistance  The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   *
+   * @return The estimated pose of the robot.
+   */
+  Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
+                units::meter_t rightDistance) {
+    return PoseEstimator::Update(gyroAngle, {leftDistance, rightDistance});
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This
+   * should be called every loop.
+   *
+   * @param currentTime   The time at which this method was called.
+   * @param gyroAngle     The current gyro angle.
+   * @param leftDistance  The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   *
+   * @return The estimated pose of the robot.
+   */
+  Pose2d UpdateWithTime(units::second_t currentTime,
+                        const Rotation2d& gyroAngle,
+                        units::meter_t leftDistance,
+                        units::meter_t rightDistance) {
+    return PoseEstimator::UpdateWithTime(currentTime, gyroAngle,
+                                         {leftDistance, rightDistance});
+  }
+
+ private:
+  DifferentialDriveOdometry m_odometryImpl;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator3d.hpp

@@ -0,0 +1,139 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/estimator/PoseEstimator3d.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/DifferentialDriveOdometry3d.h"
+#include "units/time.h"
+
+namespace frc {
+/**
+ * This class wraps Differential Drive Odometry to fuse latency-compensated
+ * vision measurements with differential drive encoder measurements. It will
+ * correct for noisy vision measurements and encoder drift. It is intended to be
+ * an easy drop-in for DifferentialDriveOdometry3d. In fact, if you never call
+ * AddVisionMeasurement(), and only call Update(), this will behave exactly the
+ * same as DifferentialDriveOdometry3d. It is also intended to be an easy
+ * replacement for PoseEstimator, only requiring the addition of a standard
+ * deviation for Z and appropriate conversions between 2D and 3D versions of
+ * geometry classes. (See Pose3d(Pose2d), Rotation3d(Rotation2d),
+ * Translation3d(Translation2d), and Pose3d.ToPose2d().)
+ *
+ * Update() should be called every robot loop (if your robot loops are faster or
+ * slower than the default of 20 ms, then you should change the nominal delta
+ * time via the constructor).
+ *
+ * AddVisionMeasurement() can be called as infrequently as you want; if you
+ * never call it, then this class will behave like regular encoder odometry.
+ */
+class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d
+    : public PoseEstimator3d<DifferentialDriveWheelSpeeds,
+                             DifferentialDriveWheelPositions> {
+ public:
+  /**
+   * Constructs a DifferentialDrivePoseEstimator3d with default standard
+   * deviations for the model and vision measurements.
+   *
+   * The default standard deviations of the model states are
+   * 0.02 meters for x, 0.02 meters for y, 0.02 meters for z, and 0.01 radians
+   * for angle. The default standard deviations of the vision measurements are
+   * 0.1 meters for x, 0.1 meters for y, 0.1 meters for z, and 0.1 radians for
+   * angle.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The gyro angle of the robot.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @param initialPose The estimated initial pose.
+   */
+  DifferentialDrivePoseEstimator3d(DifferentialDriveKinematics& kinematics,
+                                   const Rotation3d& gyroAngle,
+                                   units::meter_t leftDistance,
+                                   units::meter_t rightDistance,
+                                   const Pose3d& initialPose);
+
+  /**
+   * Constructs a DifferentialDrivePoseEstimator3d.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The gyro angle of the robot.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @param initialPose The estimated initial pose.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in
+   *     meters, y position in meters, z position in meters, and angle in
+   * radians). Increase these numbers to trust your state estimate less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, z position in
+   * meters, and angle in radians). Increase these numbers to trust the vision
+   * pose measurement less.
+   */
+  DifferentialDrivePoseEstimator3d(
+      DifferentialDriveKinematics& kinematics, const Rotation3d& gyroAngle,
+      units::meter_t leftDistance, units::meter_t rightDistance,
+      const Pose3d& initialPose, const wpi::array<double, 4>& stateStdDevs,
+      const wpi::array<double, 4>& visionMeasurementStdDevs);
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @param pose The estimated pose of the robot on the field.
+   */
+  void ResetPosition(const Rotation3d& gyroAngle, units::meter_t leftDistance,
+                     units::meter_t rightDistance, const Pose3d& pose) {
+    PoseEstimator3d::ResetPosition(gyroAngle, {leftDistance, rightDistance},
+                                   pose);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This
+   * should be called every loop.
+   *
+   * @param gyroAngle     The current gyro angle.
+   * @param leftDistance  The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   *
+   * @return The estimated pose of the robot.
+   */
+  Pose3d Update(const Rotation3d& gyroAngle, units::meter_t leftDistance,
+                units::meter_t rightDistance) {
+    return PoseEstimator3d::Update(gyroAngle, {leftDistance, rightDistance});
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This
+   * should be called every loop.
+   *
+   * @param currentTime   The time at which this method was called.
+   * @param gyroAngle     The current gyro angle.
+   * @param leftDistance  The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   *
+   * @return The estimated pose of the robot.
+   */
+  Pose3d UpdateWithTime(units::second_t currentTime,
+                        const Rotation3d& gyroAngle,
+                        units::meter_t leftDistance,
+                        units::meter_t rightDistance) {
+    return PoseEstimator3d::UpdateWithTime(currentTime, gyroAngle,
+                                           {leftDistance, rightDistance});
+  }
+
+ private:
+  DifferentialDriveOdometry3d m_odometryImpl;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/ExtendedKalmanFilter.hpp

@@ -0,0 +1,431 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <string>
+#include <utility>
+
+#include <Eigen/Cholesky>
+#include <wpi/array.h>
+
+#include "frc/DARE.h"
+#include "frc/EigenCore.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/fmt/Eigen.h"
+#include "frc/system/Discretization.h"
+#include "frc/system/NumericalIntegration.h"
+#include "frc/system/NumericalJacobian.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * A Kalman filter combines predictions from a model and measurements to give an
+ * estimate of the true system state. This is useful because many states cannot
+ * be measured directly as a result of sensor noise, or because the state is
+ * "hidden".
+ *
+ * Kalman filters use a K gain matrix to determine whether to trust the model or
+ * measurements more. Kalman filter theory uses statistics to compute an optimal
+ * K gain which minimizes the sum of squares error in the state estimate. This K
+ * gain is used to correct the state estimate by some amount of the difference
+ * between the actual measurements and the measurements predicted by the model.
+ *
+ * An extended Kalman filter supports nonlinear state and measurement models. It
+ * propagates the error covariance by linearizing the models around the state
+ * estimate, then applying the linear Kalman filter equations.
+ *
+ * For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
+ * "Stochastic control theory".
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @tparam Outputs Number of outputs.
+ */
+template <int States, int Inputs, int Outputs>
+class ExtendedKalmanFilter {
+ public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+  using OutputVector = Vectord<Outputs>;
+
+  using StateArray = wpi::array<double, States>;
+  using OutputArray = wpi::array<double, Outputs>;
+
+  using StateMatrix = Matrixd<States, States>;
+
+  /**
+   * Constructs an extended Kalman filter.
+   *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
+   * @param f                  A vector-valued function of x and u that returns
+   *                           the derivative of the state vector.
+   * @param h                  A vector-valued function of x and u that returns
+   *                           the measurement vector.
+   * @param stateStdDevs       Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dt                 Nominal discretization timestep.
+   */
+  ExtendedKalmanFilter(
+      std::function<StateVector(const StateVector&, const InputVector&)> f,
+      std::function<OutputVector(const StateVector&, const InputVector&)> h,
+      const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+      units::second_t dt)
+      : m_f(std::move(f)), m_h(std::move(h)) {
+    m_contQ = MakeCovMatrix(stateStdDevs);
+    m_contR = MakeCovMatrix(measurementStdDevs);
+    m_residualFuncY = [](const OutputVector& a,
+                         const OutputVector& b) -> OutputVector {
+      return a - b;
+    };
+    m_addFuncX = [](const StateVector& a, const StateVector& b) -> StateVector {
+      return a + b;
+    };
+    m_dt = dt;
+
+    StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
+        m_f, m_xHat, InputVector::Zero());
+    Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
+        m_h, m_xHat, InputVector::Zero());
+
+    StateMatrix discA;
+    StateMatrix discQ;
+    DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
+
+    Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
+
+    if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
+      if (auto P = DARE<States, Outputs>(discA.transpose(), C.transpose(),
+                                         discQ, discR)) {
+        m_initP = P.value();
+      } else if (P.error() == DAREError::QNotSymmetric ||
+                 P.error() == DAREError::QNotPositiveSemidefinite) {
+        std::string msg =
+            fmt::format("{}\n\nQ =\n{}\n", to_string(P.error()), discQ);
+
+        wpi::math::MathSharedStore::ReportError(msg);
+        throw std::invalid_argument(msg);
+      } else if (P.error() == DAREError::RNotSymmetric ||
+                 P.error() == DAREError::RNotPositiveDefinite) {
+        std::string msg =
+            fmt::format("{}\n\nR =\n{}\n", to_string(P.error()), discR);
+
+        wpi::math::MathSharedStore::ReportError(msg);
+        throw std::invalid_argument(msg);
+      } else if (P.error() == DAREError::ABNotStabilizable) {
+        std::string msg = fmt::format(
+            "The (A, C) pair is not detectable.\n\nA =\n{}\nC =\n{}\n",
+            to_string(P.error()), discA, C);
+
+        wpi::math::MathSharedStore::ReportError(msg);
+        throw std::invalid_argument(msg);
+      } else if (P.error() == DAREError::ACNotDetectable) {
+        std::string msg = fmt::format("{}\n\nA =\n{}\nQ =\n{}\n",
+                                      to_string(P.error()), discA, discQ);
+
+        wpi::math::MathSharedStore::ReportError(msg);
+        throw std::invalid_argument(msg);
+      }
+    } else {
+      m_initP = StateMatrix::Zero();
+    }
+    m_P = m_initP;
+  }
+
+  /**
+   * Constructs an extended Kalman filter.
+   *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
+   * @param f                  A vector-valued function of x and u that returns
+   *                           the derivative of the state vector.
+   * @param h                  A vector-valued function of x and u that returns
+   *                           the measurement vector.
+   * @param stateStdDevs       Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param residualFuncY      A function that computes the residual of two
+   *                           measurement vectors (i.e. it subtracts them.)
+   * @param addFuncX           A function that adds two state vectors.
+   * @param dt                 Nominal discretization timestep.
+   */
+  ExtendedKalmanFilter(
+      std::function<StateVector(const StateVector&, const InputVector&)> f,
+      std::function<OutputVector(const StateVector&, const InputVector&)> h,
+      const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+      std::function<OutputVector(const OutputVector&, const OutputVector&)>
+          residualFuncY,
+      std::function<StateVector(const StateVector&, const StateVector&)>
+          addFuncX,
+      units::second_t dt)
+      : m_f(std::move(f)),
+        m_h(std::move(h)),
+        m_residualFuncY(std::move(residualFuncY)),
+        m_addFuncX(std::move(addFuncX)) {
+    m_contQ = MakeCovMatrix(stateStdDevs);
+    m_contR = MakeCovMatrix(measurementStdDevs);
+    m_dt = dt;
+
+    StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
+        m_f, m_xHat, InputVector::Zero());
+    Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
+        m_h, m_xHat, InputVector::Zero());
+
+    StateMatrix discA;
+    StateMatrix discQ;
+    DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
+
+    Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
+
+    if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
+      if (auto P = DARE<States, Outputs>(discA.transpose(), C.transpose(),
+                                         discQ, discR)) {
+        m_initP = P.value();
+      } else if (P.error() == DAREError::QNotSymmetric ||
+                 P.error() == DAREError::QNotPositiveSemidefinite) {
+        std::string msg =
+            fmt::format("{}\n\nQ =\n{}\n", to_string(P.error()), discQ);
+
+        wpi::math::MathSharedStore::ReportError(msg);
+        throw std::invalid_argument(msg);
+      } else if (P.error() == DAREError::RNotSymmetric ||
+                 P.error() == DAREError::RNotPositiveDefinite) {
+        std::string msg =
+            fmt::format("{}\n\nR =\n{}\n", to_string(P.error()), discR);
+
+        wpi::math::MathSharedStore::ReportError(msg);
+        throw std::invalid_argument(msg);
+      } else if (P.error() == DAREError::ABNotStabilizable) {
+        std::string msg = fmt::format(
+            "The (A, C) pair is not detectable.\n\nA =\n{}\nC =\n{}\n",
+            to_string(P.error()), discA, C);
+
+        wpi::math::MathSharedStore::ReportError(msg);
+        throw std::invalid_argument(msg);
+      } else if (P.error() == DAREError::ACNotDetectable) {
+        std::string msg = fmt::format("{}\n\nA =\n{}\nQ =\n{}\n",
+                                      to_string(P.error()), discA, discQ);
+
+        wpi::math::MathSharedStore::ReportError(msg);
+        throw std::invalid_argument(msg);
+      }
+    } else {
+      m_initP = StateMatrix::Zero();
+    }
+    m_P = m_initP;
+  }
+
+  /**
+   * Returns the error covariance matrix P.
+   */
+  const StateMatrix& P() const { return m_P; }
+
+  /**
+   * Returns an element of the error covariance matrix P.
+   *
+   * @param i Row of P.
+   * @param j Column of P.
+   */
+  double P(int i, int j) const { return m_P(i, j); }
+
+  /**
+   * Set the current error covariance matrix P.
+   *
+   * @param P The error covariance matrix P.
+   */
+  void SetP(const StateMatrix& P) { m_P = P; }
+
+  /**
+   * Returns the state estimate x-hat.
+   */
+  const StateVector& Xhat() const { return m_xHat; }
+
+  /**
+   * Returns an element of the state estimate x-hat.
+   *
+   * @param i Row of x-hat.
+   */
+  double Xhat(int i) const { return m_xHat(i); }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xHat The state estimate x-hat.
+   */
+  void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param i     Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  void SetXhat(int i, double value) { m_xHat(i) = value; }
+
+  /**
+   * Resets the observer.
+   */
+  void Reset() {
+    m_xHat.setZero();
+    m_P = m_initP;
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u  New control input from controller.
+   * @param dt Timestep for prediction.
+   */
+  void Predict(const InputVector& u, units::second_t dt) {
+    // Find continuous A
+    StateMatrix contA =
+        NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
+
+    // Find discrete A and Q
+    StateMatrix discA;
+    StateMatrix discQ;
+    DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
+
+    m_xHat = RK4(m_f, m_xHat, u, dt);
+
+    // Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
+    m_P = discA * m_P * discA.transpose() + discQ;
+
+    m_dt = dt;
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   */
+  void Correct(const InputVector& u, const OutputVector& y) {
+    Correct<Outputs>(u, y, m_h, m_contR, m_residualFuncY, m_addFuncX);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * This is useful for when the measurement noise covariances vary.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  void Correct(const InputVector& u, const OutputVector& y,
+               const Matrixd<Outputs, Outputs>& R) {
+    Correct<Outputs>(u, y, m_h, R, m_residualFuncY, m_addFuncX);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * This is useful for when the measurements available during a timestep's
+   * Correct() call vary. The h(x, u) passed to the constructor is used if one
+   * is not provided (the two-argument version of this function).
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement
+   *          vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  template <int Rows>
+  void Correct(
+      const InputVector& u, const Vectord<Rows>& y,
+      std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+      const Matrixd<Rows, Rows>& R) {
+    auto residualFuncY = [](const Vectord<Rows>& a,
+                            const Vectord<Rows>& b) -> Vectord<Rows> {
+      return a - b;
+    };
+    auto addFuncX = [](const StateVector& a,
+                       const StateVector& b) -> StateVector { return a + b; };
+    Correct<Rows>(u, y, std::move(h), R, std::move(residualFuncY),
+                  std::move(addFuncX));
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * This is useful for when the measurements available during a timestep's
+   * Correct() call vary. The h(x, u) passed to the constructor is used if one
+   * is not provided (the two-argument version of this function).
+   *
+   * @param u             Same control input used in the predict step.
+   * @param y             Measurement vector.
+   * @param h             A vector-valued function of x and u that returns
+   *                      the measurement vector.
+   * @param R             Continuous measurement noise covariance matrix.
+   * @param residualFuncY A function that computes the residual of two
+   *                      measurement vectors (i.e. it subtracts them.)
+   * @param addFuncX      A function that adds two state vectors.
+   */
+  template <int Rows>
+  void Correct(
+      const InputVector& u, const Vectord<Rows>& y,
+      std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+      const Matrixd<Rows, Rows>& R,
+      std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
+          residualFuncY,
+      std::function<StateVector(const StateVector&, const StateVector&)>
+          addFuncX) {
+    const Matrixd<Rows, States> C =
+        NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
+    const Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
+
+    Matrixd<Rows, Rows> S = C * m_P * C.transpose() + discR;
+
+    // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
+    // efficiently.
+    //
+    // K = PCᵀS⁻¹
+    // KS = PCᵀ
+    // (KS)ᵀ = (PCᵀ)ᵀ
+    // SᵀKᵀ = CPᵀ
+    //
+    // The solution of Ax = b can be found via x = A.solve(b).
+    //
+    // Kᵀ = Sᵀ.solve(CPᵀ)
+    // K = (Sᵀ.solve(CPᵀ))ᵀ
+    //
+    // Drop the transposes on symmetric matrices S and P.
+    //
+    // K = (S.solve(CP))ᵀ
+    Matrixd<States, Rows> K = S.ldlt().solve(C * m_P).transpose();
+
+    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + Kₖ₊₁(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
+    m_xHat = addFuncX(m_xHat, K * residualFuncY(y, h(m_xHat, u)));
+
+    // Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
+    // Use Joseph form for numerical stability
+    m_P = (StateMatrix::Identity() - K * C) * m_P *
+              (StateMatrix::Identity() - K * C).transpose() +
+          K * discR * K.transpose();
+  }
+
+ private:
+  std::function<StateVector(const StateVector&, const InputVector&)> m_f;
+  std::function<OutputVector(const StateVector&, const InputVector&)> m_h;
+  std::function<OutputVector(const OutputVector&, const OutputVector&)>
+      m_residualFuncY;
+  std::function<StateVector(const StateVector&, const StateVector&)> m_addFuncX;
+  StateVector m_xHat = StateVector::Zero();
+  StateMatrix m_P;
+  StateMatrix m_contQ;
+  Matrixd<Outputs, Outputs> m_contR;
+  units::second_t m_dt;
+
+  StateMatrix m_initP;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/KalmanFilter.hpp

@@ -0,0 +1,270 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <cmath>
+#include <stdexcept>
+#include <string>
+
+#include <Eigen/Cholesky>
+#include <wpi/array.h>
+
+#include "frc/DARE.h"
+#include "frc/EigenCore.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/fmt/Eigen.h"
+#include "frc/system/Discretization.h"
+#include "frc/system/LinearSystem.h"
+#include "units/time.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+/**
+ * A Kalman filter combines predictions from a model and measurements to give an
+ * estimate of the true system state. This is useful because many states cannot
+ * be measured directly as a result of sensor noise, or because the state is
+ * "hidden".
+ *
+ * Kalman filters use a K gain matrix to determine whether to trust the model or
+ * measurements more. Kalman filter theory uses statistics to compute an optimal
+ * K gain which minimizes the sum of squares error in the state estimate. This K
+ * gain is used to correct the state estimate by some amount of the difference
+ * between the actual measurements and the measurements predicted by the model.
+ *
+ * For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
+ * "Stochastic control theory".
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @tparam Outputs Number of outputs.
+ */
+template <int States, int Inputs, int Outputs>
+class KalmanFilter {
+ public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+  using OutputVector = Vectord<Outputs>;
+
+  using StateArray = wpi::array<double, States>;
+  using OutputArray = wpi::array<double, Outputs>;
+
+  using StateMatrix = Matrixd<States, States>;
+
+  /**
+   * Constructs a Kalman filter with the given plant.
+   *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
+   * @param plant              The plant used for the prediction step.
+   * @param stateStdDevs       Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dt                 Nominal discretization timestep.
+   * @throws std::invalid_argument If the system is undetectable.
+   */
+  KalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
+               const StateArray& stateStdDevs,
+               const OutputArray& measurementStdDevs, units::second_t dt) {
+    m_plant = &plant;
+
+    m_contQ = MakeCovMatrix(stateStdDevs);
+    m_contR = MakeCovMatrix(measurementStdDevs);
+    m_dt = dt;
+
+    // Find discrete A and Q
+    Matrixd<States, States> discA;
+    Matrixd<States, States> discQ;
+    DiscretizeAQ<States>(plant.A(), m_contQ, dt, &discA, &discQ);
+
+    Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
+
+    const auto& C = plant.C();
+
+    if (auto P = DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ,
+                                       discR)) {
+      m_initP = P.value();
+    } else if (P.error() == DAREError::QNotSymmetric ||
+               P.error() == DAREError::QNotPositiveSemidefinite) {
+      std::string msg =
+          fmt::format("{}\n\nQ =\n{}\n", to_string(P.error()), discQ);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    } else if (P.error() == DAREError::RNotSymmetric ||
+               P.error() == DAREError::RNotPositiveDefinite) {
+      std::string msg =
+          fmt::format("{}\n\nR =\n{}\n", to_string(P.error()), discR);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    } else if (P.error() == DAREError::ABNotStabilizable) {
+      std::string msg = fmt::format(
+          "The (A, C) pair is not detectable.\n\nA =\n{}\nC =\n{}\n",
+          to_string(P.error()), discA, C);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    } else if (P.error() == DAREError::ACNotDetectable) {
+      std::string msg = fmt::format("{}\n\nA =\n{}\nQ =\n{}\n",
+                                    to_string(P.error()), discA, discQ);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    }
+
+    Reset();
+  }
+
+  /**
+   * Returns the error covariance matrix P.
+   */
+  const StateMatrix& P() const { return m_P; }
+
+  /**
+   * Returns an element of the error covariance matrix P.
+   *
+   * @param i Row of P.
+   * @param j Column of P.
+   */
+  double P(int i, int j) const { return m_P(i, j); }
+
+  /**
+   * Set the current error covariance matrix P.
+   *
+   * @param P The error covariance matrix P.
+   */
+  void SetP(const StateMatrix& P) { m_P = P; }
+
+  /**
+   * Returns the state estimate x-hat.
+   */
+  const StateVector& Xhat() const { return m_xHat; }
+
+  /**
+   * Returns an element of the state estimate x-hat.
+   *
+   * @param i Row of x-hat.
+   */
+  double Xhat(int i) const { return m_xHat(i); }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xHat The state estimate x-hat.
+   */
+  void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param i     Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  void SetXhat(int i, double value) { m_xHat(i) = value; }
+
+  /**
+   * Resets the observer.
+   */
+  void Reset() {
+    m_xHat.setZero();
+    m_P = m_initP;
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u  New control input from controller.
+   * @param dt Timestep for prediction.
+   */
+  void Predict(const InputVector& u, units::second_t dt) {
+    // Find discrete A and Q
+    StateMatrix discA;
+    StateMatrix discQ;
+    DiscretizeAQ<States>(m_plant->A(), m_contQ, dt, &discA, &discQ);
+
+    m_xHat = m_plant->CalculateX(m_xHat, u, dt);
+
+    // Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
+    m_P = discA * m_P * discA.transpose() + discQ;
+
+    m_dt = dt;
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   */
+  void Correct(const InputVector& u, const OutputVector& y) {
+    Correct(u, y, m_contR);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * This is useful for when the measurement noise covariances vary.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  void Correct(const InputVector& u, const OutputVector& y,
+               const Matrixd<Outputs, Outputs>& R) {
+    const auto& C = m_plant->C();
+    const auto& D = m_plant->D();
+
+    const Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(R, m_dt);
+
+    Matrixd<Outputs, Outputs> S = C * m_P * C.transpose() + discR;
+
+    // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
+    // efficiently.
+    //
+    // K = PCᵀS⁻¹
+    // KS = PCᵀ
+    // (KS)ᵀ = (PCᵀ)ᵀ
+    // SᵀKᵀ = CPᵀ
+    //
+    // The solution of Ax = b can be found via x = A.solve(b).
+    //
+    // Kᵀ = Sᵀ.solve(CPᵀ)
+    // K = (Sᵀ.solve(CPᵀ))ᵀ
+    //
+    // Drop the transposes on symmetric matrices S and P.
+    //
+    // K = (S.solve(CP))ᵀ
+    Matrixd<States, Outputs> K = S.ldlt().solve(C * m_P).transpose();
+
+    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
+    m_xHat += K * (y - (C * m_xHat + D * u));
+
+    // Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
+    // Use Joseph form for numerical stability
+    m_P = (StateMatrix::Identity() - K * C) * m_P *
+              (StateMatrix::Identity() - K * C).transpose() +
+          K * discR * K.transpose();
+  }
+
+ private:
+  LinearSystem<States, Inputs, Outputs>* m_plant;
+  StateVector m_xHat;
+  StateMatrix m_P;
+  StateMatrix m_contQ;
+  Matrixd<Outputs, Outputs> m_contR;
+  units::second_t m_dt;
+
+  StateMatrix m_initP;
+};
+
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    KalmanFilter<1, 1, 1>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    KalmanFilter<2, 1, 1>;
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/KalmanFilterLatencyCompensator.hpp

@@ -0,0 +1,180 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <array>
+#include <functional>
+#include <utility>
+#include <vector>
+
+#include "frc/EigenCore.h"
+#include "units/math.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * This class incorporates time-delayed measurements into a Kalman filter's
+ * state estimate.
+ *
+ * @tparam States The number of states.
+ * @tparam Inputs The number of inputs.
+ * @tparam Outputs The number of outputs.
+ */
+template <int States, int Inputs, int Outputs, typename KalmanFilterType>
+class KalmanFilterLatencyCompensator {
+ public:
+  /**
+   * This class contains all the information about our observer at a given time.
+   */
+  struct ObserverSnapshot {
+    /// The state estimate.
+    Vectord<States> xHat;
+    /// The square root error covariance.
+    Matrixd<States, States> squareRootErrorCovariances;
+    /// The inputs.
+    Vectord<Inputs> inputs;
+    /// The local measurements.
+    Vectord<Outputs> localMeasurements;
+
+    ObserverSnapshot(const KalmanFilterType& observer, const Vectord<Inputs>& u,
+                     const Vectord<Outputs>& localY)
+        : xHat(observer.Xhat()),
+          squareRootErrorCovariances(observer.S()),
+          inputs(u),
+          localMeasurements(localY) {}
+  };
+
+  /**
+   * Clears the observer snapshot buffer.
+   */
+  void Reset() { m_pastObserverSnapshots.clear(); }
+
+  /**
+   * Add past observer states to the observer snapshots list.
+   *
+   * @param observer  The observer.
+   * @param u         The input at the timestamp.
+   * @param localY    The local output at the timestamp
+   * @param timestamp The timestamp of the state.
+   */
+  void AddObserverState(const KalmanFilterType& observer, Vectord<Inputs> u,
+                        Vectord<Outputs> localY, units::second_t timestamp) {
+    // Add the new state into the vector.
+    m_pastObserverSnapshots.emplace_back(timestamp,
+                                         ObserverSnapshot{observer, u, localY});
+
+    // Remove the oldest snapshot if the vector exceeds our maximum size.
+    if (m_pastObserverSnapshots.size() > kMaxPastObserverStates) {
+      m_pastObserverSnapshots.erase(m_pastObserverSnapshots.begin());
+    }
+  }
+
+  /**
+   * Add past global measurements (such as from vision)to the estimator.
+   *
+   * @param observer                 The observer to apply the past global
+   *                                 measurement.
+   * @param nominalDt                The nominal timestep.
+   * @param y                        The measurement.
+   * @param globalMeasurementCorrect The function take calls correct() on the
+   *                                 observer.
+   * @param timestamp                The timestamp of the measurement.
+   */
+  template <int Rows>
+  void ApplyPastGlobalMeasurement(
+      KalmanFilterType* observer, units::second_t nominalDt, Vectord<Rows> y,
+      std::function<void(const Vectord<Inputs>& u, const Vectord<Rows>& y)>
+          globalMeasurementCorrect,
+      units::second_t timestamp) {
+    if (m_pastObserverSnapshots.size() == 0) {
+      // State map was empty, which means that we got a measurement right at
+      // startup. The only thing we can do is ignore the measurement.
+      return;
+    }
+
+    // Perform a binary search to find the index of first snapshot whose
+    // timestamp is greater than or equal to the global measurement timestamp
+    auto it = std::lower_bound(
+        m_pastObserverSnapshots.cbegin(), m_pastObserverSnapshots.cend(),
+        timestamp,
+        [](const auto& entry, const auto& ts) { return entry.first < ts; });
+
+    size_t indexOfClosestEntry;
+
+    if (it == m_pastObserverSnapshots.cbegin()) {
+      // If the global measurement is older than any snapshot, throw out the
+      // measurement because there's no state estimate into which to incorporate
+      // the measurement
+      if (timestamp < it->first) {
+        return;
+      }
+
+      // If the first snapshot has same timestamp as the global measurement, use
+      // that snapshot
+      indexOfClosestEntry = 0;
+    } else if (it == m_pastObserverSnapshots.cend()) {
+      // If all snapshots are older than the global measurement, use the newest
+      // snapshot
+      indexOfClosestEntry = m_pastObserverSnapshots.size() - 1;
+    } else {
+      // Index of snapshot taken after the global measurement
+      int nextIdx = std::distance(m_pastObserverSnapshots.cbegin(), it);
+
+      // Index of snapshot taken before the global measurement. Since we already
+      // handled the case where the index points to the first snapshot, this
+      // computation is guaranteed to be nonnegative.
+      int prevIdx = nextIdx - 1;
+
+      // Find the snapshot closest in time to global measurement
+      units::second_t prevTimeDiff =
+          units::math::abs(timestamp - m_pastObserverSnapshots[prevIdx].first);
+      units::second_t nextTimeDiff =
+          units::math::abs(timestamp - m_pastObserverSnapshots[nextIdx].first);
+      indexOfClosestEntry = prevTimeDiff < nextTimeDiff ? prevIdx : nextIdx;
+    }
+
+    units::second_t lastTimestamp =
+        m_pastObserverSnapshots[indexOfClosestEntry].first - nominalDt;
+
+    // We will now go back in time to the state of the system at the time when
+    // the measurement was captured. We will reset the observer to that state,
+    // and apply correction based on the measurement. Then, we will go back
+    // through all observer states until the present and apply past inputs to
+    // get the present estimated state.
+    for (size_t i = indexOfClosestEntry; i < m_pastObserverSnapshots.size();
+         ++i) {
+      auto& [key, snapshot] = m_pastObserverSnapshots[i];
+
+      if (i == indexOfClosestEntry) {
+        observer->SetS(snapshot.squareRootErrorCovariances);
+        observer->SetXhat(snapshot.xHat);
+      }
+
+      observer->Predict(snapshot.inputs, key - lastTimestamp);
+      observer->Correct(snapshot.inputs, snapshot.localMeasurements);
+
+      if (i == indexOfClosestEntry) {
+        // Note that the measurement is at a timestep close but probably not
+        // exactly equal to the timestep for which we called predict. This makes
+        // the assumption that the dt is small enough that the difference
+        // between the measurement time and the time that the inputs were
+        // captured at is very small.
+        globalMeasurementCorrect(snapshot.inputs, y);
+      }
+
+      lastTimestamp = key;
+      snapshot = ObserverSnapshot{*observer, snapshot.inputs,
+                                  snapshot.localMeasurements};
+    }
+  }
+
+ private:
+  static constexpr size_t kMaxPastObserverStates = 300;
+  std::vector<std::pair<units::second_t, ObserverSnapshot>>
+      m_pastObserverSnapshots;
+};
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator.hpp

@@ -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.
+
+#pragma once
+
+#include <functional>
+
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/estimator/PoseEstimator.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveOdometry.h"
+#include "units/time.h"
+
+namespace frc {
+/**
+ * This class wraps Mecanum Drive Odometry to fuse latency-compensated
+ * vision measurements with mecanum drive encoder velocity measurements. It will
+ * correct for noisy measurements and encoder drift. It is intended to be an
+ * easy drop-in for MecanumDriveOdometry.
+ *
+ * Update() should be called every robot loop. If your loops are faster or
+ * slower than the default of 20 ms, then you should change the nominal delta
+ * time by specifying it in the constructor.
+ *
+ * AddVisionMeasurement() can be called as infrequently as you want; if you
+ * never call it, then this class will behave mostly like regular encoder
+ * odometry.
+ */
+class WPILIB_DLLEXPORT MecanumDrivePoseEstimator
+    : public PoseEstimator<MecanumDriveWheelSpeeds,
+                           MecanumDriveWheelPositions> {
+ public:
+  /**
+   * Constructs a MecanumDrivePoseEstimator with default standard deviations
+   * for the model and vision measurements.
+   *
+   * The default standard deviations of the model states are
+   * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading.
+   * The default standard deviations of the vision measurements are
+   * 0.45 meters for x, 0.45 meters for y, and 0.45 radians for heading.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The distance measured by each wheel.
+   * @param initialPose The starting pose estimate.
+   */
+  MecanumDrivePoseEstimator(MecanumDriveKinematics& kinematics,
+                            const Rotation2d& gyroAngle,
+                            const MecanumDriveWheelPositions& wheelPositions,
+                            const Pose2d& initialPose);
+
+  /**
+   * Constructs a MecanumDrivePoseEstimator.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The distance measured by each wheel.
+   * @param initialPose The starting pose estimate.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in
+   *     meters, y position in meters, and heading in radians). Increase these
+   *     numbers to trust your state estimate less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, and heading in
+   *     radians). Increase these numbers to trust the vision pose measurement
+   *     less.
+   */
+  MecanumDrivePoseEstimator(
+      MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+      const MecanumDriveWheelPositions& wheelPositions,
+      const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
+      const wpi::array<double, 3>& visionMeasurementStdDevs);
+
+ private:
+  MecanumDriveOdometry m_odometryImpl;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator3d.hpp

@@ -0,0 +1,90 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/estimator/PoseEstimator3d.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveOdometry3d.h"
+#include "units/time.h"
+
+namespace frc {
+/**
+ * This class wraps Mecanum Drive Odometry to fuse latency-compensated
+ * vision measurements with mecanum drive encoder velocity measurements. It will
+ * correct for noisy measurements and encoder drift. It is intended to be an
+ * easy drop-in for MecanumDriveOdometry3d. It is also intended to be an easy
+ * replacement for PoseEstimator, only requiring the addition of a standard
+ * deviation for Z and appropriate conversions between 2D and 3D versions of
+ * geometry classes. (See Pose3d(Pose2d), Rotation3d(Rotation2d),
+ * Translation3d(Translation2d), and Pose3d.ToPose2d().)
+ *
+ * Update() should be called every robot loop. If your loops are faster or
+ * slower than the default of 20 ms, then you should change the nominal delta
+ * time by specifying it in the constructor.
+ *
+ * AddVisionMeasurement() can be called as infrequently as you want; if you
+ * never call it, then this class will behave mostly like regular encoder
+ * odometry.
+ */
+class WPILIB_DLLEXPORT MecanumDrivePoseEstimator3d
+    : public PoseEstimator3d<MecanumDriveWheelSpeeds,
+                             MecanumDriveWheelPositions> {
+ public:
+  /**
+   * Constructs a MecanumDrivePoseEstimator3d with default standard deviations
+   * for the model and vision measurements.
+   *
+   * The default standard deviations of the model states are
+   * 0.1 meters for x, 0.1 meters for y, 0.1 meters for z, and 0.1 radians for
+   * angle. The default standard deviations of the vision measurements are 0.45
+   * meters for x, 0.45 meters for y, 0.45 meters for z, and 0.45 radians for
+   * angle.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The distance measured by each wheel.
+   * @param initialPose The starting pose estimate.
+   */
+  MecanumDrivePoseEstimator3d(MecanumDriveKinematics& kinematics,
+                              const Rotation3d& gyroAngle,
+                              const MecanumDriveWheelPositions& wheelPositions,
+                              const Pose3d& initialPose);
+
+  /**
+   * Constructs a MecanumDrivePoseEstimator3d.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The distance measured by each wheel.
+   * @param initialPose The starting pose estimate.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in
+   *     meters, y position in meters, z position in meters, and angle in
+   * radians). Increase these numbers to trust your state estimate less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, z position in
+   * meters, and angle in radians). Increase these numbers to trust the vision
+   * pose measurement less.
+   */
+  MecanumDrivePoseEstimator3d(
+      MecanumDriveKinematics& kinematics, const Rotation3d& gyroAngle,
+      const MecanumDriveWheelPositions& wheelPositions,
+      const Pose3d& initialPose, const wpi::array<double, 4>& stateStdDevs,
+      const wpi::array<double, 4>& visionMeasurementStdDevs);
+
+ private:
+  MecanumDriveOdometry3d m_odometryImpl;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/MerweScaledSigmaPoints.hpp

@@ -0,0 +1,128 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <cmath>
+
+#include "frc/EigenCore.h"
+
+namespace frc {
+
+/**
+ * Generates sigma points and weights according to Van der Merwe's 2004
+ * dissertation[1] for the UnscentedKalmanFilter class.
+ *
+ * It parametrizes the sigma points using alpha, beta, kappa terms, and is the
+ * version seen in most publications. S3SigmaPoints is generally preferred due
+ * to its greater performance with nearly identical accuracy.
+ *
+ * [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilistic
+ *     Inference in Dynamic State-Space Models" (Doctoral dissertation)
+ *
+ * @tparam States The dimensionality of the state. 2 * States + 1 weights will
+ *                be generated.
+ */
+template <int States>
+class MerweScaledSigmaPoints {
+ public:
+  static constexpr int NumSigmas = 2 * States + 1;
+
+  /**
+   * Constructs a generator for Van der Merwe scaled sigma points.
+   *
+   * @param alpha Determines the spread of the sigma points around the mean.
+   *              Usually a small positive value (1e-3).
+   * @param beta Incorporates prior knowledge of the distribution of the mean.
+   *             For Gaussian distributions, beta = 2 is optimal.
+   * @param kappa Secondary scaling parameter usually set to 0 or 3 - States.
+   */
+  explicit MerweScaledSigmaPoints(double alpha = 1e-3, double beta = 2,
+                                  int kappa = 3 - States) {
+    m_alpha = alpha;
+    m_kappa = kappa;
+
+    ComputeWeights(beta);
+  }
+
+  /**
+   * Computes the sigma points for an unscented Kalman filter given the mean
+   * (x) and square-root covariance (S) of the filter.
+   *
+   * @param x An array of the means.
+   * @param S Square-root covariance of the filter.
+   *
+   * @return Two dimensional array of sigma points. Each column contains all of
+   *         the sigmas for one dimension in the problem space. Ordered by
+   *         Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
+   *
+   */
+  Matrixd<States, NumSigmas> SquareRootSigmaPoints(
+      const Vectord<States>& x, const Matrixd<States, States>& S) {
+    double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
+    double eta = std::sqrt(lambda + States);
+    Matrixd<States, States> U = eta * S;
+
+    Matrixd<States, NumSigmas> sigmas;
+
+    // equation (17)
+    sigmas.template block<States, 1>(0, 0) = x;
+    for (int k = 0; k < States; ++k) {
+      sigmas.template block<States, 1>(0, k + 1) =
+          x + U.template block<States, 1>(0, k);
+      sigmas.template block<States, 1>(0, States + k + 1) =
+          x - U.template block<States, 1>(0, k);
+    }
+
+    return sigmas;
+  }
+
+  /**
+   * Returns the weight for each sigma point for the mean.
+   */
+  const Vectord<NumSigmas>& Wm() const { return m_Wm; }
+
+  /**
+   * Returns an element of the weight for each sigma point for the mean.
+   *
+   * @param i Element of vector to return.
+   */
+  double Wm(int i) const { return m_Wm(i, 0); }
+
+  /**
+   * Returns the weight for each sigma point for the covariance.
+   */
+  const Vectord<NumSigmas>& Wc() const { return m_Wc; }
+
+  /**
+   * Returns an element of the weight for each sigma point for the covariance.
+   *
+   * @param i Element of vector to return.
+   */
+  double Wc(int i) const { return m_Wc(i, 0); }
+
+ private:
+  Vectord<NumSigmas> m_Wm;
+  Vectord<NumSigmas> m_Wc;
+  double m_alpha;
+  int m_kappa;
+
+  /**
+   * Computes the weights for the scaled unscented Kalman filter.
+   *
+   * @param beta Incorporates prior knowledge of the distribution of the mean.
+   */
+  void ComputeWeights(double beta) {
+    double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
+
+    double c = 0.5 / (States + lambda);
+    m_Wm = Vectord<NumSigmas>::Constant(c);
+    m_Wc = Vectord<NumSigmas>::Constant(c);
+
+    m_Wm(0) = lambda / (States + lambda);
+    m_Wc(0) = lambda / (States + lambda) + (1 - std::pow(m_alpha, 2) + beta);
+  }
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/MerweUKF.hpp

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/estimator/MerweScaledSigmaPoints.h"
+#include "frc/estimator/UnscentedKalmanFilter.h"
+
+namespace frc {
+
+template <int States, int Inputs, int Outputs>
+using MerweUKF = UnscentedKalmanFilter<States, Inputs, Outputs,
+                                       MerweScaledSigmaPoints<States>>;
+
+// Because MerweUKF is a type alias and not a class, we have to use
+// UnscentedKalmanFilter instead
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    UnscentedKalmanFilter<3, 3, 1, MerweScaledSigmaPoints<3>>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    UnscentedKalmanFilter<5, 3, 3, MerweScaledSigmaPoints<5>>;
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp

@@ -0,0 +1,441 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <map>
+#include <optional>
+#include <vector>
+
+#include <Eigen/Core>
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
+#include "frc/kinematics/Kinematics.h"
+#include "frc/kinematics/Odometry.h"
+#include "units/time.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+/**
+ * This class wraps odometry to fuse latency-compensated
+ * vision measurements with encoder measurements. Robot code should not use this
+ * directly- Instead, use the particular type for your drivetrain (e.g.,
+ * DifferentialDrivePoseEstimator). It will correct for noisy vision
+ * measurements and encoder drift. It is intended to be an easy drop-in for
+ * Odometry.
+ *
+ * Update() should be called every robot loop.
+ *
+ * AddVisionMeasurement() can be called as infrequently as you want; if you
+ * never call it, then this class will behave like regular encoder odometry.
+ *
+ * @tparam WheelSpeeds Wheel speeds type.
+ * @tparam WheelPositions Wheel positions type.
+ */
+template <typename WheelSpeeds, typename WheelPositions>
+class WPILIB_DLLEXPORT PoseEstimator {
+ public:
+  /**
+   * Constructs a PoseEstimator.
+   *
+   * @warning The initial pose estimate will always be the default pose,
+   * regardless of the odometry's current pose.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param odometry A correctly-configured odometry object for your drivetrain.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in
+   *     meters, y position in meters, and heading in radians). Increase these
+   *     numbers to trust your state estimate less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, and heading in
+   *     radians). Increase these numbers to trust the vision pose measurement
+   *     less.
+   */
+  PoseEstimator(Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+                Odometry<WheelSpeeds, WheelPositions>& odometry,
+                const wpi::array<double, 3>& stateStdDevs,
+                const wpi::array<double, 3>& visionMeasurementStdDevs)
+      : m_odometry(odometry) {
+    for (size_t i = 0; i < 3; ++i) {
+      m_q[i] = stateStdDevs[i] * stateStdDevs[i];
+    }
+
+    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+  }
+
+  /**
+   * Sets the pose estimator's trust in vision measurements. This might be used
+   * to change trust in vision measurements after the autonomous period, or to
+   * change trust as distance to a vision target increases.
+   *
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, and heading in
+   *     radians). Increase these numbers to trust the vision pose measurement
+   *     less.
+   */
+  void SetVisionMeasurementStdDevs(
+      const wpi::array<double, 3>& visionMeasurementStdDevs) {
+    wpi::array<double, 3> r{wpi::empty_array};
+    for (size_t i = 0; i < 3; ++i) {
+      r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
+    }
+
+    // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+    // and C = I. See wpimath/algorithms.md.
+    for (size_t row = 0; row < 3; ++row) {
+      if (m_q[row] == 0.0) {
+        m_visionK(row, row) = 0.0;
+      } else {
+        m_visionK(row, row) =
+            m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
+      }
+    }
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * The gyroscope angle does not need to be reset in the user's robot code.
+   * The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The distances traveled by the encoders.
+   * @param pose The estimated pose of the robot on the field.
+   */
+  void ResetPosition(const Rotation2d& gyroAngle,
+                     const WheelPositions& wheelPositions, const Pose2d& pose) {
+    // Reset state estimate and error covariance
+    m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
+    m_odometryPoseBuffer.Clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.GetPose();
+  }
+
+  /**
+   * Resets the robot's pose.
+   *
+   * @param pose The pose to reset to.
+   */
+  void ResetPose(const Pose2d& pose) {
+    m_odometry.ResetPose(pose);
+    m_odometryPoseBuffer.Clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.GetPose();
+  }
+
+  /**
+   * Resets the robot's translation.
+   *
+   * @param translation The pose to translation to.
+   */
+  void ResetTranslation(const Translation2d& translation) {
+    m_odometry.ResetTranslation(translation);
+    m_odometryPoseBuffer.Clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.GetPose();
+  }
+
+  /**
+   * Resets the robot's rotation.
+   *
+   * @param rotation The rotation to reset to.
+   */
+  void ResetRotation(const Rotation2d& rotation) {
+    m_odometry.ResetRotation(rotation);
+    m_odometryPoseBuffer.Clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.GetPose();
+  }
+
+  /**
+   * Gets the estimated robot pose.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  Pose2d GetEstimatedPosition() const { return m_poseEstimate; }
+
+  /**
+   * Return the pose at a given timestamp, if the buffer is not empty.
+   *
+   * @param timestamp The pose's timestamp.
+   * @return The pose at the given timestamp (or std::nullopt if the buffer is
+   * empty).
+   */
+  std::optional<Pose2d> SampleAt(units::second_t timestamp) const {
+    // Step 0: If there are no odometry updates to sample, skip.
+    if (m_odometryPoseBuffer.GetInternalBuffer().empty()) {
+      return std::nullopt;
+    }
+
+    // Step 1: Make sure timestamp matches the sample from the odometry pose
+    // buffer. (When sampling, the buffer will always use a timestamp
+    // between the first and last timestamps)
+    units::second_t oldestOdometryTimestamp =
+        m_odometryPoseBuffer.GetInternalBuffer().front().first;
+    units::second_t newestOdometryTimestamp =
+        m_odometryPoseBuffer.GetInternalBuffer().back().first;
+    timestamp =
+        std::clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp);
+
+    // Step 2: If there are no applicable vision updates, use the odometry-only
+    // information.
+    if (m_visionUpdates.empty() || timestamp < m_visionUpdates.begin()->first) {
+      return m_odometryPoseBuffer.Sample(timestamp);
+    }
+
+    // Step 3: Get the latest vision update from before or at the timestamp to
+    // sample at.
+    // First, find the iterator past the sample timestamp, then go back one.
+    // Note that upper_bound() won't return begin() because we check begin()
+    // earlier.
+    auto floorIter = m_visionUpdates.upper_bound(timestamp);
+    --floorIter;
+    auto visionUpdate = floorIter->second;
+
+    // Step 4: Get the pose measured by odometry at the time of the sample.
+    auto odometryEstimate = m_odometryPoseBuffer.Sample(timestamp);
+
+    // Step 5: Apply the vision compensation to the odometry pose.
+    // TODO Replace with std::optional::transform() in C++23
+    if (odometryEstimate) {
+      return visionUpdate.Compensate(*odometryEstimate);
+    }
+    return std::nullopt;
+  }
+
+  /**
+   * Adds a vision measurement to the Kalman Filter. This will correct
+   * the odometry pose estimate while still accounting for measurement noise.
+   *
+   * This method can be called as infrequently as you want, as long as you are
+   * calling Update() every loop.
+   *
+   * To promote stability of the pose estimate and make it robust to bad vision
+   * data, we recommend only adding vision measurements that are already within
+   * one meter or so of the current pose estimate.
+   *
+   * @param visionRobotPose The pose of the robot as measured by the vision
+   *     camera.
+   * @param timestamp The timestamp of the vision measurement in seconds. Note
+   *     that if you don't use your own time source by calling UpdateWithTime(),
+   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
+   *     the epoch of this timestamp is the same epoch as
+   *     frc::Timer::GetTimestamp(). This means that you should use
+   *     frc::Timer::GetTimestamp() as your time source in this case.
+   */
+  void AddVisionMeasurement(const Pose2d& visionRobotPose,
+                            units::second_t timestamp) {
+    // Step 0: If this measurement is old enough to be outside the pose buffer's
+    // timespan, skip.
+    if (m_odometryPoseBuffer.GetInternalBuffer().empty() ||
+        m_odometryPoseBuffer.GetInternalBuffer().front().first -
+                kBufferDuration >
+            timestamp) {
+      return;
+    }
+
+    // Step 1: Clean up any old entries
+    CleanUpVisionUpdates();
+
+    // Step 2: Get the pose measured by odometry at the moment the vision
+    // measurement was made.
+    auto odometrySample = m_odometryPoseBuffer.Sample(timestamp);
+
+    if (!odometrySample) {
+      return;
+    }
+
+    // Step 3: Get the vision-compensated pose estimate at the moment the vision
+    // measurement was made.
+    auto visionSample = SampleAt(timestamp);
+
+    if (!visionSample) {
+      return;
+    }
+
+    // Step 4: Measure the twist between the old pose estimate and the vision
+    // pose.
+    auto twist = (visionRobotPose - visionSample.value()).Log();
+
+    // Step 5: We should not trust the twist entirely, so instead we scale this
+    // twist by a Kalman gain matrix representing how much we trust vision
+    // measurements compared to our current pose.
+    Eigen::Vector3d k_times_twist =
+        m_visionK * Eigen::Vector3d{twist.dx.value(), twist.dy.value(),
+                                    twist.dtheta.value()};
+
+    // Step 6: Convert back to Twist2d.
+    Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
+                        units::meter_t{k_times_twist(1)},
+                        units::radian_t{k_times_twist(2)}};
+
+    // Step 7: Calculate and record the vision update.
+    VisionUpdate visionUpdate{visionSample.value() + scaledTwist.Exp(),
+                              *odometrySample};
+    m_visionUpdates[timestamp] = visionUpdate;
+
+    // Step 8: Remove later vision measurements. (Matches previous behavior)
+    auto firstAfter = m_visionUpdates.upper_bound(timestamp);
+    m_visionUpdates.erase(firstAfter, m_visionUpdates.end());
+
+    // Step 9: Update latest pose estimate. Since we cleared all updates after
+    // this vision update, it's guaranteed to be the latest vision update.
+    m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
+  }
+
+  /**
+   * Adds a vision measurement to the Kalman Filter. This will correct
+   * the odometry pose estimate while still accounting for measurement noise.
+   *
+   * This method can be called as infrequently as you want, as long as you are
+   * calling Update() every loop.
+   *
+   * To promote stability of the pose estimate and make it robust to bad vision
+   * data, we recommend only adding vision measurements that are already within
+   * one meter or so of the current pose estimate.
+   *
+   * Note that the vision measurement standard deviations passed into this
+   * method will continue to apply to future measurements until a subsequent
+   * call to SetVisionMeasurementStdDevs() or this method.
+   *
+   * @param visionRobotPose The pose of the robot as measured by the vision
+   *     camera.
+   * @param timestamp The timestamp of the vision measurement in seconds. Note
+   *     that if you don't use your own time source by calling UpdateWithTime(),
+   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
+   *     the epoch of this timestamp is the same epoch as
+   *     frc::Timer::GetTimestamp(). This means that you should use
+   *     frc::Timer::GetTimestamp() as your time source in this case.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, and heading in
+   *     radians). Increase these numbers to trust the vision pose measurement
+   *     less.
+   */
+  void AddVisionMeasurement(
+      const Pose2d& visionRobotPose, units::second_t timestamp,
+      const wpi::array<double, 3>& visionMeasurementStdDevs) {
+    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    AddVisionMeasurement(visionRobotPose, timestamp);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This
+   * should be called every loop.
+   *
+   * @param gyroAngle      The current gyro angle.
+   * @param wheelPositions The distances traveled by the encoders.
+   *
+   * @return The estimated pose of the robot in meters.
+   */
+  Pose2d Update(const Rotation2d& gyroAngle,
+                const WheelPositions& wheelPositions) {
+    return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
+                          wheelPositions);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This
+   * should be called every loop.
+   *
+   * @param currentTime   The time at which this method was called.
+   * @param gyroAngle     The current gyro angle.
+   * @param wheelPositions The distances traveled by the encoders.
+   *
+   * @return The estimated pose of the robot in meters.
+   */
+  Pose2d UpdateWithTime(units::second_t currentTime,
+                        const Rotation2d& gyroAngle,
+                        const WheelPositions& wheelPositions) {
+    auto odometryEstimate = m_odometry.Update(gyroAngle, wheelPositions);
+
+    m_odometryPoseBuffer.AddSample(currentTime, odometryEstimate);
+
+    if (m_visionUpdates.empty()) {
+      m_poseEstimate = odometryEstimate;
+    } else {
+      auto visionUpdate = m_visionUpdates.rbegin()->second;
+      m_poseEstimate = visionUpdate.Compensate(odometryEstimate);
+    }
+
+    return GetEstimatedPosition();
+  }
+
+ private:
+  /**
+   * Removes stale vision updates that won't affect sampling.
+   */
+  void CleanUpVisionUpdates() {
+    // Step 0: If there are no odometry samples, skip.
+    if (m_odometryPoseBuffer.GetInternalBuffer().empty()) {
+      return;
+    }
+
+    // Step 1: Find the oldest timestamp that needs a vision update.
+    units::second_t oldestOdometryTimestamp =
+        m_odometryPoseBuffer.GetInternalBuffer().front().first;
+
+    // Step 2: If there are no vision updates before that timestamp, skip.
+    if (m_visionUpdates.empty() ||
+        oldestOdometryTimestamp < m_visionUpdates.begin()->first) {
+      return;
+    }
+
+    // Step 3: Find the newest vision update timestamp before or at the oldest
+    // timestamp.
+    // First, find the iterator past the oldest odometry timestamp, then go
+    // back one. Note that upper_bound() won't return begin() because we check
+    // begin() earlier.
+    auto newestNeededVisionUpdate =
+        m_visionUpdates.upper_bound(oldestOdometryTimestamp);
+    --newestNeededVisionUpdate;
+
+    // Step 4: Remove all entries strictly before the newest timestamp we need.
+    m_visionUpdates.erase(m_visionUpdates.begin(), newestNeededVisionUpdate);
+  }
+
+  struct VisionUpdate {
+    // The vision-compensated pose estimate
+    Pose2d visionPose;
+
+    // The pose estimated based solely on odometry
+    Pose2d odometryPose;
+
+    /**
+     * Returns the vision-compensated version of the pose. Specifically, changes
+     * the pose from being relative to this record's odometry pose to being
+     * relative to this record's vision pose.
+     *
+     * @param pose The pose to compensate.
+     * @return The compensated pose.
+     */
+    Pose2d Compensate(const Pose2d& pose) const {
+      auto delta = pose - odometryPose;
+      return visionPose + delta;
+    }
+  };
+
+  static constexpr units::second_t kBufferDuration = 1.5_s;
+
+  Odometry<WheelSpeeds, WheelPositions>& m_odometry;
+  wpi::array<double, 3> m_q{wpi::empty_array};
+  Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
+
+  // Maps timestamps to odometry-only pose estimates
+  TimeInterpolatableBuffer<Pose2d> m_odometryPoseBuffer{kBufferDuration};
+  // Maps timestamps to vision updates
+  // Always contains one entry before the oldest entry in m_odometryPoseBuffer,
+  // unless there have been no vision measurements after the last reset
+  std::map<units::second_t, VisionUpdate> m_visionUpdates;
+
+  Pose2d m_poseEstimate;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp

@@ -0,0 +1,452 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <map>
+#include <optional>
+#include <utility>
+#include <vector>
+
+#include <Eigen/Core>
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/EigenCore.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
+#include "frc/kinematics/Kinematics.h"
+#include "frc/kinematics/Odometry3d.h"
+#include "units/time.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+/**
+ * This class wraps odometry to fuse latency-compensated
+ * vision measurements with encoder measurements. Robot code should not use this
+ * directly- Instead, use the particular type for your drivetrain (e.g.,
+ * DifferentialDrivePoseEstimator3d). It will correct for noisy vision
+ * measurements and encoder drift. It is intended to be an easy drop-in for
+ * Odometry3d. It is also intended to be an easy replacement for PoseEstimator,
+ * only requiring the addition of a standard deviation for Z and appropriate
+ * conversions between 2D and 3D versions of geometry classes. (See
+ * Pose3d(Pose2d), Rotation3d(Rotation2d), Translation3d(Translation2d), and
+ * Pose3d.ToPose2d().)
+ *
+ * Update() should be called every robot loop.
+ *
+ * AddVisionMeasurement() can be called as infrequently as you want; if you
+ * never call it, then this class will behave like regular encoder odometry.
+ *
+ * @tparam WheelSpeeds Wheel speeds type.
+ * @tparam WheelPositions Wheel positions type.
+ */
+template <typename WheelSpeeds, typename WheelPositions>
+class WPILIB_DLLEXPORT PoseEstimator3d {
+ public:
+  /**
+   * Constructs a PoseEstimator3d.
+   *
+   * @warning The initial pose estimate will always be the default pose,
+   * regardless of the odometry's current pose.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param odometry A correctly-configured odometry object for your drivetrain.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in
+   *     meters, y position in meters, and heading in radians). Increase these
+   *     numbers to trust your state estimate less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, and z position
+   * in meters, and angle in radians). Increase these numbers to trust the
+   * vision pose measurement less.
+   */
+  PoseEstimator3d(Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+                  Odometry3d<WheelSpeeds, WheelPositions>& odometry,
+                  const wpi::array<double, 4>& stateStdDevs,
+                  const wpi::array<double, 4>& visionMeasurementStdDevs)
+      : m_odometry(odometry) {
+    for (size_t i = 0; i < 4; ++i) {
+      m_q[i] = stateStdDevs[i] * stateStdDevs[i];
+    }
+
+    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+  }
+
+  /**
+   * Sets the pose estimator's trust in vision measurements. This might be used
+   * to change trust in vision measurements after the autonomous period, or to
+   * change trust as distance to a vision target increases.
+   *
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, and heading in
+   *     radians). Increase these numbers to trust the vision pose measurement
+   *     less.
+   */
+  void SetVisionMeasurementStdDevs(
+      const wpi::array<double, 4>& visionMeasurementStdDevs) {
+    wpi::array<double, 4> r{wpi::empty_array};
+    for (size_t i = 0; i < 4; ++i) {
+      r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
+    }
+
+    // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+    // and C = I. See wpimath/algorithms.md.
+    for (size_t row = 0; row < 4; ++row) {
+      if (m_q[row] == 0.0) {
+        m_visionK(row, row) = 0.0;
+      } else {
+        m_visionK(row, row) =
+            m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
+      }
+    }
+    double angle_gain = m_visionK(3, 3);
+    m_visionK(4, 4) = angle_gain;
+    m_visionK(5, 5) = angle_gain;
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * The gyroscope angle does not need to be reset in the user's robot code.
+   * The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The distances traveled by the encoders.
+   * @param pose The estimated pose of the robot on the field.
+   */
+  void ResetPosition(const Rotation3d& gyroAngle,
+                     const WheelPositions& wheelPositions, const Pose3d& pose) {
+    // Reset state estimate and error covariance
+    m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
+    m_odometryPoseBuffer.Clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.GetPose();
+  }
+
+  /**
+   * Resets the robot's pose.
+   *
+   * @param pose The pose to reset to.
+   */
+  void ResetPose(const Pose3d& pose) {
+    m_odometry.ResetPose(pose);
+    m_odometryPoseBuffer.Clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.GetPose();
+  }
+
+  /**
+   * Resets the robot's translation.
+   *
+   * @param translation The pose to translation to.
+   */
+  void ResetTranslation(const Translation3d& translation) {
+    m_odometry.ResetTranslation(translation);
+    m_odometryPoseBuffer.Clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.GetPose();
+  }
+
+  /**
+   * Resets the robot's rotation.
+   *
+   * @param rotation The rotation to reset to.
+   */
+  void ResetRotation(const Rotation3d& rotation) {
+    m_odometry.ResetRotation(rotation);
+    m_odometryPoseBuffer.Clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.GetPose();
+  }
+
+  /**
+   * Gets the estimated robot pose.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  Pose3d GetEstimatedPosition() const { return m_poseEstimate; }
+
+  /**
+   * Return the pose at a given timestamp, if the buffer is not empty.
+   *
+   * @param timestamp The pose's timestamp.
+   * @return The pose at the given timestamp (or std::nullopt if the buffer is
+   * empty).
+   */
+  std::optional<Pose3d> SampleAt(units::second_t timestamp) const {
+    // Step 0: If there are no odometry updates to sample, skip.
+    if (m_odometryPoseBuffer.GetInternalBuffer().empty()) {
+      return std::nullopt;
+    }
+
+    // Step 1: Make sure timestamp matches the sample from the odometry pose
+    // buffer. (When sampling, the buffer will always use a timestamp
+    // between the first and last timestamps)
+    units::second_t oldestOdometryTimestamp =
+        m_odometryPoseBuffer.GetInternalBuffer().front().first;
+    units::second_t newestOdometryTimestamp =
+        m_odometryPoseBuffer.GetInternalBuffer().back().first;
+    timestamp =
+        std::clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp);
+
+    // Step 2: If there are no applicable vision updates, use the odometry-only
+    // information.
+    if (m_visionUpdates.empty() || timestamp < m_visionUpdates.begin()->first) {
+      return m_odometryPoseBuffer.Sample(timestamp);
+    }
+
+    // Step 3: Get the latest vision update from before or at the timestamp to
+    // sample at.
+    // First, find the iterator past the sample timestamp, then go back one.
+    // Note that upper_bound() won't return begin() because we check begin()
+    // earlier.
+    auto floorIter = m_visionUpdates.upper_bound(timestamp);
+    --floorIter;
+    auto visionUpdate = floorIter->second;
+
+    // Step 4: Get the pose measured by odometry at the time of the sample.
+    auto odometryEstimate = m_odometryPoseBuffer.Sample(timestamp);
+
+    // Step 5: Apply the vision compensation to the odometry pose.
+    // TODO Replace with std::optional::transform() in C++23
+    if (odometryEstimate) {
+      return visionUpdate.Compensate(*odometryEstimate);
+    }
+    return std::nullopt;
+  }
+
+  /**
+   * Adds a vision measurement to the Kalman Filter. This will correct
+   * the odometry pose estimate while still accounting for measurement noise.
+   *
+   * This method can be called as infrequently as you want, as long as you are
+   * calling Update() every loop.
+   *
+   * To promote stability of the pose estimate and make it robust to bad vision
+   * data, we recommend only adding vision measurements that are already within
+   * one meter or so of the current pose estimate.
+   *
+   * @param visionRobotPose The pose of the robot as measured by the vision
+   *     camera.
+   * @param timestamp The timestamp of the vision measurement in seconds. Note
+   *     that if you don't use your own time source by calling UpdateWithTime(),
+   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
+   *     the epoch of this timestamp is the same epoch as
+   *     frc::Timer::GetFPGATimestamp(). This means that you should use
+   *     frc::Timer::GetFPGATimestamp() as your time source in this case.
+   */
+  void AddVisionMeasurement(const Pose3d& visionRobotPose,
+                            units::second_t timestamp) {
+    // Step 0: If this measurement is old enough to be outside the pose buffer's
+    // timespan, skip.
+    if (m_odometryPoseBuffer.GetInternalBuffer().empty() ||
+        m_odometryPoseBuffer.GetInternalBuffer().front().first -
+                kBufferDuration >
+            timestamp) {
+      return;
+    }
+
+    // Step 1: Clean up any old entries
+    CleanUpVisionUpdates();
+
+    // Step 2: Get the pose measured by odometry at the moment the vision
+    // measurement was made.
+    auto odometrySample = m_odometryPoseBuffer.Sample(timestamp);
+
+    if (!odometrySample) {
+      return;
+    }
+
+    // Step 3: Get the vision-compensated pose estimate at the moment the vision
+    // measurement was made.
+    auto visionSample = SampleAt(timestamp);
+
+    if (!visionSample) {
+      return;
+    }
+
+    // Step 4: Measure the twist between the old pose estimate and the vision
+    // pose.
+    auto twist = (visionRobotPose - visionSample.value()).Log();
+
+    // Step 5: We should not trust the twist entirely, so instead we scale this
+    // twist by a Kalman gain matrix representing how much we trust vision
+    // measurements compared to our current pose.
+    frc::Vectord<6> k_times_twist =
+        m_visionK * frc::Vectord<6>{twist.dx.value(), twist.dy.value(),
+                                    twist.dz.value(), twist.rx.value(),
+                                    twist.ry.value(), twist.rz.value()};
+
+    // Step 6: Convert back to Twist3d.
+    Twist3d scaledTwist{
+        units::meter_t{k_times_twist(0)},  units::meter_t{k_times_twist(1)},
+        units::meter_t{k_times_twist(2)},  units::radian_t{k_times_twist(3)},
+        units::radian_t{k_times_twist(4)}, units::radian_t{k_times_twist(5)}};
+
+    // Step 7: Calculate and record the vision update.
+    VisionUpdate visionUpdate{visionSample.value() + scaledTwist.Exp(),
+                              *odometrySample};
+    m_visionUpdates[timestamp] = visionUpdate;
+
+    // Step 8: Remove later vision measurements. (Matches previous behavior)
+    auto firstAfter = m_visionUpdates.upper_bound(timestamp);
+    m_visionUpdates.erase(firstAfter, m_visionUpdates.end());
+
+    // Step 9: Update latest pose estimate. Since we cleared all updates after
+    // this vision update, it's guaranteed to be the latest vision update.
+    m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
+  }
+
+  /**
+   * Adds a vision measurement to the Kalman Filter. This will correct
+   * the odometry pose estimate while still accounting for measurement noise.
+   *
+   * This method can be called as infrequently as you want, as long as you are
+   * calling Update() every loop.
+   *
+   * To promote stability of the pose estimate and make it robust to bad vision
+   * data, we recommend only adding vision measurements that are already within
+   * one meter or so of the current pose estimate.
+   *
+   * Note that the vision measurement standard deviations passed into this
+   * method will continue to apply to future measurements until a subsequent
+   * call to SetVisionMeasurementStdDevs() or this method.
+   *
+   * @param visionRobotPose The pose of the robot as measured by the vision
+   *     camera.
+   * @param timestamp The timestamp of the vision measurement in seconds. Note
+   *     that if you don't use your own time source by calling UpdateWithTime(),
+   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
+   *     the epoch of this timestamp is the same epoch as
+   *     frc::Timer::GetFPGATimestamp(). This means that you should use
+   *     frc::Timer::GetFPGATimestamp() as your time source in this case.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, and heading in
+   *     radians). Increase these numbers to trust the vision pose measurement
+   *     less.
+   */
+  void AddVisionMeasurement(
+      const Pose3d& visionRobotPose, units::second_t timestamp,
+      const wpi::array<double, 4>& visionMeasurementStdDevs) {
+    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    AddVisionMeasurement(visionRobotPose, timestamp);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This
+   * should be called every loop.
+   *
+   * @param gyroAngle      The current gyro angle.
+   * @param wheelPositions The distances traveled by the encoders.
+   *
+   * @return The estimated pose of the robot in meters.
+   */
+  Pose3d Update(const Rotation3d& gyroAngle,
+                const WheelPositions& wheelPositions) {
+    return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
+                          wheelPositions);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This
+   * should be called every loop.
+   *
+   * @param currentTime   The time at which this method was called.
+   * @param gyroAngle     The current gyro angle.
+   * @param wheelPositions The distances traveled by the encoders.
+   *
+   * @return The estimated pose of the robot in meters.
+   */
+  Pose3d UpdateWithTime(units::second_t currentTime,
+                        const Rotation3d& gyroAngle,
+                        const WheelPositions& wheelPositions) {
+    auto odometryEstimate = m_odometry.Update(gyroAngle, wheelPositions);
+
+    m_odometryPoseBuffer.AddSample(currentTime, odometryEstimate);
+
+    if (m_visionUpdates.empty()) {
+      m_poseEstimate = odometryEstimate;
+    } else {
+      auto visionUpdate = m_visionUpdates.rbegin()->second;
+      m_poseEstimate = visionUpdate.Compensate(odometryEstimate);
+    }
+
+    return GetEstimatedPosition();
+  }
+
+ private:
+  /**
+   * Removes stale vision updates that won't affect sampling.
+   */
+  void CleanUpVisionUpdates() {
+    // Step 0: If there are no odometry samples, skip.
+    if (m_odometryPoseBuffer.GetInternalBuffer().empty()) {
+      return;
+    }
+
+    // Step 1: Find the oldest timestamp that needs a vision update.
+    units::second_t oldestOdometryTimestamp =
+        m_odometryPoseBuffer.GetInternalBuffer().front().first;
+
+    // Step 2: If there are no vision updates before that timestamp, skip.
+    if (m_visionUpdates.empty() ||
+        oldestOdometryTimestamp < m_visionUpdates.begin()->first) {
+      return;
+    }
+
+    // Step 3: Find the newest vision update timestamp before or at the oldest
+    // timestamp.
+    // First, find the iterator past the oldest odometry timestamp, then go
+    // back one. Note that upper_bound() won't return begin() because we check
+    // begin() earlier.
+    auto newestNeededVisionUpdate =
+        m_visionUpdates.upper_bound(oldestOdometryTimestamp);
+    --newestNeededVisionUpdate;
+
+    // Step 4: Remove all entries strictly before the newest timestamp we need.
+    m_visionUpdates.erase(m_visionUpdates.begin(), newestNeededVisionUpdate);
+  }
+
+  struct VisionUpdate {
+    // The vision-compensated pose estimate
+    Pose3d visionPose;
+
+    // The pose estimated based solely on odometry
+    Pose3d odometryPose;
+
+    /**
+     * Returns the vision-compensated version of the pose. Specifically, changes
+     * the pose from being relative to this record's odometry pose to being
+     * relative to this record's vision pose.
+     *
+     * @param pose The pose to compensate.
+     * @return The compensated pose.
+     */
+    Pose3d Compensate(const Pose3d& pose) const {
+      auto delta = pose - odometryPose;
+      return visionPose + delta;
+    }
+  };
+
+  static constexpr units::second_t kBufferDuration = 1.5_s;
+
+  Odometry3d<WheelSpeeds, WheelPositions>& m_odometry;
+  wpi::array<double, 4> m_q{wpi::empty_array};
+  frc::Matrixd<6, 6> m_visionK = frc::Matrixd<6, 6>::Zero();
+
+  // Maps timestamps to odometry-only pose estimates
+  TimeInterpolatableBuffer<Pose3d> m_odometryPoseBuffer{kBufferDuration};
+  // Maps timestamps to vision updates
+  // Always contains one entry before the oldest entry in m_odometryPoseBuffer,
+  // unless there have been no vision measurements after the last reset
+  std::map<units::second_t, VisionUpdate> m_visionUpdates;
+
+  Pose3d m_poseEstimate;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/S3SigmaPoints.hpp

@@ -0,0 +1,135 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/array.h>
+
+#include "frc/EigenCore.h"
+#include "frc/estimator/SigmaPoints.h"
+
+namespace frc {
+
+/**
+ * Generates sigma points and weights according to Papakonstantinou's paper[1]
+ * for the UnscentedKalmanFilter class.
+ *
+ * It parameterizes the sigma points using alpha and beta terms. Unless you know
+ * better, this should be your default choice due to its high accuracy and
+ * performance.
+ *
+ * [1] K. Papakonstantinou "A Scaled Spherical Simplex Filter (S3F) with a
+ * decreased n + 2 sigma points set size and equivalent 2n + 1 Unscented Kalman
+ * Filter (UKF) accuracy"
+ *
+ * @tparam States The dimenstionality of the state. States + 2 weights will be
+ * generated.
+ */
+template <int States>
+class S3SigmaPoints {
+ public:
+  static constexpr int NumSigmas = States + 2;
+
+  /**
+   * Constructs a generator for Papakonstantinou sigma points.
+   *
+   * @param alpha Determines the spread of the sigma points around the mean.
+   * Usually a small positive value (1e-3).
+   * @param beta Incorporates prior knowledge of the distribution of the mean.
+   * For Gaussian distributions, beta = 2 is optimal.
+   */
+  explicit S3SigmaPoints(double alpha = 1e-3, double beta = 2)
+      : m_alpha{alpha} {
+    ComputeWeights(beta);
+  }
+
+  /**
+   * Computes the sigma points for an unscented Kalman filter given the mean (x)
+   * and square-root covariance (S) of the filter.
+   *
+   * @param x An array of the means.
+   * @param S Square-root covariance of the filter.
+   *
+   * @return Two dimensional array of sigma points. Each column contains all of
+   * the sigmas for one dimension in the problem space. Ordered by Xi_0,
+   * Xi_{1..n+1}.
+   */
+  Matrixd<States, NumSigmas> SquareRootSigmaPoints(
+      const Vectord<States>& x, const Matrixd<States, States>& S) const {
+    // table (1), equation (12)
+    wpi::array<double, States> q{wpi::empty_array};
+    for (size_t t = 1; t <= States; ++t) {
+      q[t - 1] = m_alpha * std::sqrt(static_cast<double>(t * (States + 1)) /
+                                     static_cast<double>(t + 1));
+    }
+
+    Matrixd<States, NumSigmas> C;
+    C.template block<States, 1>(0, 0) = Vectord<States>::Constant(0.0);
+    for (int col = 1; col < NumSigmas; ++col) {
+      for (int row = 0; row < States; ++row) {
+        if (row < col - 2) {
+          C(row, col) = 0.0;
+        } else if (row == col - 2) {
+          C(row, col) = q[row];
+        } else {
+          C(row, col) = -q[row] / (row + 1);
+        }
+      }
+    }
+
+    Matrixd<States, NumSigmas> sigmas;
+    for (int col = 0; col < NumSigmas; ++col) {
+      sigmas.col(col) = x + S * C.col(col);
+    }
+
+    return sigmas;
+  }
+
+  /**
+   * Returns the weight for each sigma point for the mean.
+   */
+  const Vectord<NumSigmas>& Wm() const { return m_Wm; }
+
+  /**
+   * Returns an element of the weight for each sigma point for the mean.
+   *
+   * @param i Element of vector to return.
+   */
+  double Wm(int i) const { return m_Wm(i, 0); }
+
+  /**
+   * Returns the weight for each sigma point for the covariance.
+   */
+  const Vectord<NumSigmas>& Wc() const { return m_Wc; }
+
+  /**
+   * Returns an element of the weight for each sigma point for the covariance.
+   *
+   * @param i Element of vector to return.
+   */
+  double Wc(int i) const { return m_Wc(i, 0); }
+
+ private:
+  Vectord<NumSigmas> m_Wm;
+  Vectord<NumSigmas> m_Wc;
+  double m_alpha;
+
+  /**
+   * Computes the weights for the scaled unscented Kalman filter.
+   *
+   * @param beta Incorporates prior knowledge of the distribution of the mean.
+   */
+  void ComputeWeights(double beta) {
+    double alpha_sq = m_alpha * m_alpha;
+
+    double c = 1.0 / (alpha_sq * (States + 1));
+    m_Wm = Vectord<NumSigmas>::Constant(c);
+    m_Wc = Vectord<NumSigmas>::Constant(c);
+
+    m_Wm(0) = 1.0 - 1.0 / alpha_sq;
+    m_Wc(0) = 1.0 - 1.0 / alpha_sq + (1 - alpha_sq + beta);
+  }
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/S3UKF.hpp

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/estimator/S3SigmaPoints.h"
+#include "frc/estimator/UnscentedKalmanFilter.h"
+
+namespace frc {
+
+template <int States, int Inputs, int Outputs>
+using S3UKF =
+    UnscentedKalmanFilter<States, Inputs, Outputs, S3SigmaPoints<States>>;
+
+// Because S3UKF is a type alias and not a class, we have to use
+// UnscentedKalmanFilter instead
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    UnscentedKalmanFilter<3, 3, 1, S3SigmaPoints<3>>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    UnscentedKalmanFilter<5, 3, 3, S3SigmaPoints<5>>;
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/SigmaPoints.hpp

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <concepts>
+
+#include "frc/EigenCore.h"
+
+namespace frc {
+
+template <typename T, int States>
+concept SigmaPoints =
+    requires(T t, Vectord<States> x, Matrixd<States, States> S, int i) {
+      { T::NumSigmas } -> std::convertible_to<const int>;
+      {
+        t.SquareRootSigmaPoints(x, S)
+      } -> std::same_as<Matrixd<States, T::NumSigmas>>;
+      { t.Wm() } -> std::convertible_to<Vectord<T::NumSigmas>>;
+      { t.Wm(i) } -> std::same_as<double>;
+      { t.Wc() } -> std::convertible_to<Vectord<T::NumSigmas>>;
+      { t.Wc(i) } -> std::same_as<double>;
+    } && std::default_initializable<T>;
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/SteadyStateKalmanFilter.hpp

@@ -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.
+
+#pragma once
+
+#include <cmath>
+#include <stdexcept>
+#include <string>
+
+#include <Eigen/Cholesky>
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/DARE.h"
+#include "frc/EigenCore.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/fmt/Eigen.h"
+#include "frc/system/Discretization.h"
+#include "frc/system/LinearSystem.h"
+#include "units/time.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+/**
+ * A Kalman filter combines predictions from a model and measurements to give an
+ * estimate of the true system state. This is useful because many states cannot
+ * be measured directly as a result of sensor noise, or because the state is
+ * "hidden".
+ *
+ * Kalman filters use a K gain matrix to determine whether to trust the model or
+ * measurements more. Kalman filter theory uses statistics to compute an optimal
+ * K gain which minimizes the sum of squares error in the state estimate. This K
+ * gain is used to correct the state estimate by some amount of the difference
+ * between the actual measurements and the measurements predicted by the model.
+ *
+ * This class assumes predict() and correct() are called in pairs, so the Kalman
+ * gain converges to a steady-state value. If they aren't, use KalmanFilter
+ * instead.
+ *
+ * For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
+ * "Stochastic control theory".
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @tparam Outputs Number of outputs.
+ */
+template <int States, int Inputs, int Outputs>
+class SteadyStateKalmanFilter {
+ public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+  using OutputVector = Vectord<Outputs>;
+
+  using StateArray = wpi::array<double, States>;
+  using OutputArray = wpi::array<double, Outputs>;
+
+  /**
+   * Constructs a steady-state Kalman filter with the given plant.
+   *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
+   * @param plant              The plant used for the prediction step.
+   * @param stateStdDevs       Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dt                 Nominal discretization timestep.
+   * @throws std::invalid_argument If the system is undetectable.
+   */
+  SteadyStateKalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
+                          const StateArray& stateStdDevs,
+                          const OutputArray& measurementStdDevs,
+                          units::second_t dt) {
+    m_plant = &plant;
+
+    auto contQ = MakeCovMatrix(stateStdDevs);
+    auto contR = MakeCovMatrix(measurementStdDevs);
+
+    Matrixd<States, States> discA;
+    Matrixd<States, States> discQ;
+    DiscretizeAQ<States>(plant.A(), contQ, dt, &discA, &discQ);
+
+    auto discR = DiscretizeR<Outputs>(contR, dt);
+
+    const auto& C = plant.C();
+
+    if (!IsDetectable<States, Outputs>(discA, C)) {
+      std::string msg = fmt::format(
+          "The system passed to the Kalman filter is undetectable!\n\n"
+          "A =\n{}\nC =\n{}\n",
+          discA, C);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    }
+
+    if (auto P = DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ,
+                                       discR)) {
+      // S = CPCᵀ + R
+      Matrixd<Outputs, Outputs> S = C * P.value() * C.transpose() + discR;
+
+      // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
+      // efficiently.
+      //
+      // K = PCᵀS⁻¹
+      // KS = PCᵀ
+      // (KS)ᵀ = (PCᵀ)ᵀ
+      // SᵀKᵀ = CPᵀ
+      //
+      // The solution of Ax = b can be found via x = A.solve(b).
+      //
+      // Kᵀ = Sᵀ.solve(CPᵀ)
+      // K = (Sᵀ.solve(CPᵀ))ᵀ
+      //
+      // Drop the transposes on symmetric matrices S and P.
+      //
+      // K = (S.solve(CP))ᵀ
+      m_K = S.ldlt().solve(C * P.value()).transpose();
+    } else if (P.error() == DAREError::QNotSymmetric ||
+               P.error() == DAREError::QNotPositiveSemidefinite) {
+      std::string msg =
+          fmt::format("{}\n\nQ =\n{}\n", to_string(P.error()), discQ);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    } else if (P.error() == DAREError::RNotSymmetric ||
+               P.error() == DAREError::RNotPositiveDefinite) {
+      std::string msg =
+          fmt::format("{}\n\nR =\n{}\n", to_string(P.error()), discR);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    } else if (P.error() == DAREError::ABNotStabilizable) {
+      std::string msg = fmt::format(
+          "The (A, C) pair is not detectable.\n\nA =\n{}\nC =\n{}\n",
+          to_string(P.error()), discA, C);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    } else if (P.error() == DAREError::ACNotDetectable) {
+      std::string msg = fmt::format("{}\n\nA =\n{}\nQ =\n{}\n",
+                                    to_string(P.error()), discA, discQ);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    }
+
+    Reset();
+  }
+
+  SteadyStateKalmanFilter(SteadyStateKalmanFilter&&) = default;
+  SteadyStateKalmanFilter& operator=(SteadyStateKalmanFilter&&) = default;
+
+  /**
+   * Returns the steady-state Kalman gain matrix K.
+   */
+  const Matrixd<States, Outputs>& K() const { return m_K; }
+
+  /**
+   * Returns an element of the steady-state Kalman gain matrix K.
+   *
+   * @param i Row of K.
+   * @param j Column of K.
+   */
+  double K(int i, int j) const { return m_K(i, j); }
+
+  /**
+   * Returns the state estimate x-hat.
+   */
+  const StateVector& Xhat() const { return m_xHat; }
+
+  /**
+   * Returns an element of the state estimate x-hat.
+   *
+   * @param i Row of x-hat.
+   */
+  double Xhat(int i) const { return m_xHat(i); }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xHat The state estimate x-hat.
+   */
+  void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param i     Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  void SetXhat(int i, double value) { m_xHat(i) = value; }
+
+  /**
+   * Resets the observer.
+   */
+  void Reset() { m_xHat.setZero(); }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u  New control input from controller.
+   * @param dt Timestep for prediction.
+   */
+  void Predict(const InputVector& u, units::second_t dt) {
+    m_xHat = m_plant->CalculateX(m_xHat, u, dt);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param u Same control input used in the last predict step.
+   * @param y Measurement vector.
+   */
+  void Correct(const InputVector& u, const OutputVector& y) {
+    const auto& C = m_plant->C();
+    const auto& D = m_plant->D();
+
+    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
+    m_xHat += m_K * (y - (C * m_xHat + D * u));
+  }
+
+ private:
+  LinearSystem<States, Inputs, Outputs>* m_plant;
+
+  /**
+   * The steady-state Kalman gain matrix.
+   */
+  Matrixd<States, Outputs> m_K;
+
+  /**
+   * The state estimate.
+   */
+  StateVector m_xHat;
+};
+
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    SteadyStateKalmanFilter<1, 1, 1>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    SteadyStateKalmanFilter<2, 1, 1>;
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp

@@ -0,0 +1,98 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <cmath>
+
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/estimator/PoseEstimator.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "frc/kinematics/SwerveDriveOdometry.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * This class wraps Swerve Drive Odometry to fuse latency-compensated
+ * vision measurements with swerve drive encoder distance measurements. It is
+ * intended to be a drop-in for SwerveDriveOdometry.
+ *
+ * Update() should be called every robot loop.
+ *
+ * AddVisionMeasurement() can be called as infrequently as you want; if you
+ * never call it, then this class will behave as regular encoder
+ * odometry.
+ */
+template <size_t NumModules>
+class SwerveDrivePoseEstimator
+    : public PoseEstimator<wpi::array<SwerveModuleState, NumModules>,
+                           wpi::array<SwerveModulePosition, NumModules>> {
+ public:
+  /**
+   * Constructs a SwerveDrivePoseEstimator with default standard deviations
+   * for the model and vision measurements.
+   *
+   * The default standard deviations of the model states are
+   * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading.
+   * The default standard deviations of the vision measurements are
+   * 0.9 meters for x, 0.9 meters for y, and 0.9 radians for heading.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param modulePositions The current distance and rotation measurements of
+   *     the swerve modules.
+   * @param initialPose The starting pose estimate.
+   */
+  SwerveDrivePoseEstimator(
+      SwerveDriveKinematics<NumModules>& kinematics,
+      const Rotation2d& gyroAngle,
+      const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
+      const Pose2d& initialPose)
+      : SwerveDrivePoseEstimator{kinematics,      gyroAngle,
+                                 modulePositions, initialPose,
+                                 {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}} {}
+
+  /**
+   * Constructs a SwerveDrivePoseEstimator.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param modulePositions The current distance and rotation measurements of
+   *     the swerve modules.
+   * @param initialPose The starting pose estimate.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in
+   *     meters, y position in meters, and heading in radians). Increase these
+   *     numbers to trust your state estimate less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, and heading in
+   *     radians). Increase these numbers to trust the vision pose measurement
+   *     less.
+   */
+  SwerveDrivePoseEstimator(
+      SwerveDriveKinematics<NumModules>& kinematics,
+      const Rotation2d& gyroAngle,
+      const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
+      const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
+      const wpi::array<double, 3>& visionMeasurementStdDevs)
+      : SwerveDrivePoseEstimator::PoseEstimator(
+            kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
+        m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {
+    this->ResetPose(initialPose);
+  }
+
+ private:
+  SwerveDriveOdometry<NumModules> m_odometryImpl;
+};
+
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    SwerveDrivePoseEstimator<4>;
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp

@@ -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.
+
+#pragma once
+
+#include <cmath>
+
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/estimator/PoseEstimator3d.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "frc/kinematics/SwerveDriveOdometry3d.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * This class wraps Swerve Drive Odometry to fuse latency-compensated
+ * vision measurements with swerve drive encoder distance measurements. It is
+ * intended to be a drop-in for SwerveDriveOdometry3d. It is also intended to be
+ * an easy replacement for PoseEstimator, only requiring the addition of a
+ * standard deviation for Z and appropriate conversions between 2D and 3D
+ * versions of geometry classes. (See Pose3d(Pose2d), Rotation3d(Rotation2d),
+ * Translation3d(Translation2d), and Pose3d.ToPose2d().)
+ *
+ * Update() should be called every robot loop.
+ *
+ * AddVisionMeasurement() can be called as infrequently as you want; if you
+ * never call it, then this class will behave as regular encoder
+ * odometry.
+ */
+template <size_t NumModules>
+class SwerveDrivePoseEstimator3d
+    : public PoseEstimator3d<wpi::array<SwerveModuleState, NumModules>,
+                             wpi::array<SwerveModulePosition, NumModules>> {
+ public:
+  /**
+   * Constructs a SwerveDrivePoseEstimator3d with default standard deviations
+   * for the model and vision measurements.
+   *
+   * The default standard deviations of the model states are
+   * 0.1 meters for x, 0.1 meters for y, 0.1 meters for z, and 0.1 radians for
+   * angle. The default standard deviations of the vision measurements are 0.9
+   * meters for x, 0.9 meters for y, 0.9 meters for z, and 0.9 radians for
+   * angle.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param modulePositions The current distance and rotation measurements of
+   *     the swerve modules.
+   * @param initialPose The starting pose estimate.
+   */
+  SwerveDrivePoseEstimator3d(
+      SwerveDriveKinematics<NumModules>& kinematics,
+      const Rotation3d& gyroAngle,
+      const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
+      const Pose3d& initialPose)
+      : SwerveDrivePoseEstimator3d{kinematics,           gyroAngle,
+                                   modulePositions,      initialPose,
+                                   {0.1, 0.1, 0.1, 0.1}, {0.9, 0.9, 0.9, 0.9}} {
+  }
+
+  /**
+   * Constructs a SwerveDrivePoseEstimator3d.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param modulePositions The current distance and rotation measurements of
+   *     the swerve modules.
+   * @param initialPose The starting pose estimate.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in
+   *     meters, y position in meters, and heading in radians). Increase these
+   *     numbers to trust your state estimate less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, and heading in
+   *     radians). Increase these numbers to trust the vision pose measurement
+   *     less.
+   */
+  SwerveDrivePoseEstimator3d(
+      SwerveDriveKinematics<NumModules>& kinematics,
+      const Rotation3d& gyroAngle,
+      const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
+      const Pose3d& initialPose, const wpi::array<double, 4>& stateStdDevs,
+      const wpi::array<double, 4>& visionMeasurementStdDevs)
+      : SwerveDrivePoseEstimator3d::PoseEstimator3d(
+            kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
+        m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {
+    this->ResetPose(initialPose);
+  }
+
+ private:
+  SwerveDriveOdometry3d<NumModules> m_odometryImpl;
+};
+
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    SwerveDrivePoseEstimator3d<4>;
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp

@@ -0,0 +1,499 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <utility>
+
+#include <Eigen/Cholesky>
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/EigenCore.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/SigmaPoints.h"
+#include "frc/estimator/UnscentedTransform.h"
+#include "frc/system/Discretization.h"
+#include "frc/system/NumericalIntegration.h"
+#include "frc/system/NumericalJacobian.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * A Kalman filter combines predictions from a model and measurements to give an
+ * estimate of the true system state. This is useful because many states cannot
+ * be measured directly as a result of sensor noise, or because the state is
+ * "hidden".
+ *
+ * This class requires a SigmaPoints template parameter. For convenience, S3UKF
+ * and MerweUKF type aliases are provided to specify a suitable generator for
+ * you. S3UKF is generally preferred over MerweUKF because of its greater
+ * performance while maintaining nearly identical accuracy.
+ *
+ * Kalman filters use a K gain matrix to determine whether to trust the model or
+ * measurements more. Kalman filter theory uses statistics to compute an optimal
+ * K gain which minimizes the sum of squares error in the state estimate. This K
+ * gain is used to correct the state estimate by some amount of the difference
+ * between the actual measurements and the measurements predicted by the model.
+ *
+ * An unscented Kalman filter uses nonlinear state and measurement models. It
+ * propagates the error covariance using sigma points chosen to approximate the
+ * true probability distribution.
+ *
+ * For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
+ * "Stochastic control theory".
+ *
+ * <p> This class implements a square-root-form unscented Kalman filter
+ * (SR-UKF). The main reason for this is to guarantee that the covariance
+ * matrix remains positive definite.
+ * For more information about the SR-UKF, see
+ * https://www.researchgate.net/publication/3908304.
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @tparam Outputs Number of outputs.
+ * @tparam SigmaPoints Type used to generate sigma points and weights.
+ */
+template <int States, int Inputs, int Outputs, SigmaPoints<States> SigmaPoints>
+class UnscentedKalmanFilter {
+ public:
+  static constexpr int NumSigmas = SigmaPoints::NumSigmas;
+
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+  using OutputVector = Vectord<Outputs>;
+
+  using StateArray = wpi::array<double, States>;
+  using OutputArray = wpi::array<double, Outputs>;
+
+  using StateMatrix = Matrixd<States, States>;
+
+  /**
+   * Constructs an unscented Kalman filter.
+   *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
+   * @param f                  A vector-valued function of x and u that returns
+   *                           the derivative of the state vector.
+   * @param h                  A vector-valued function of x and u that returns
+   *                           the measurement vector.
+   * @param stateStdDevs       Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dt                 Nominal discretization timestep.
+   */
+  UnscentedKalmanFilter(
+      std::function<StateVector(const StateVector&, const InputVector&)> f,
+      std::function<OutputVector(const StateVector&, const InputVector&)> h,
+      const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+      units::second_t dt)
+      : m_f(std::move(f)), m_h(std::move(h)) {
+    m_contQ = MakeCovMatrix(stateStdDevs);
+    m_contR = MakeCovMatrix(measurementStdDevs);
+    m_meanFuncX = [](const Matrixd<States, NumSigmas>& sigmas,
+                     const Vectord<NumSigmas>& Wm) -> StateVector {
+      return sigmas * Wm;
+    };
+    m_meanFuncY = [](const Matrixd<Outputs, NumSigmas>& sigmas,
+                     const Vectord<NumSigmas>& Wc) -> OutputVector {
+      return sigmas * Wc;
+    };
+    m_residualFuncX = [](const StateVector& a,
+                         const StateVector& b) -> StateVector { return a - b; };
+    m_residualFuncY = [](const OutputVector& a,
+                         const OutputVector& b) -> OutputVector {
+      return a - b;
+    };
+    m_addFuncX = [](const StateVector& a, const StateVector& b) -> StateVector {
+      return a + b;
+    };
+    m_dt = dt;
+
+    Reset();
+  }
+
+  /**
+   * Constructs an Unscented Kalman filter with custom mean, residual, and
+   * addition functions. Using custom functions for arithmetic can be useful if
+   * you have angles in the state or measurements, because they allow you to
+   * correctly account for the modular nature of angle arithmetic.
+   *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
+   * @param f                  A vector-valued function of x and u that returns
+   *                           the derivative of the state vector.
+   * @param h                  A vector-valued function of x and u that returns
+   *                           the measurement vector.
+   * @param stateStdDevs       Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param meanFuncX          A function that computes the mean of 2 * States +
+   *                           1 state vectors using a given set of weights.
+   * @param meanFuncY          A function that computes the mean of 2 * States +
+   *                           1 measurement vectors using a given set of
+   *                           weights.
+   * @param residualFuncX      A function that computes the residual of two
+   *                           state vectors (i.e. it subtracts them.)
+   * @param residualFuncY      A function that computes the residual of two
+   *                           measurement vectors (i.e. it subtracts them.)
+   * @param addFuncX           A function that adds two state vectors.
+   * @param dt                 Nominal discretization timestep.
+   */
+  UnscentedKalmanFilter(
+      std::function<StateVector(const StateVector&, const InputVector&)> f,
+      std::function<OutputVector(const StateVector&, const InputVector&)> h,
+      const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+      std::function<StateVector(const Matrixd<States, NumSigmas>&,
+                                const Vectord<NumSigmas>&)>
+          meanFuncX,
+      std::function<OutputVector(const Matrixd<Outputs, NumSigmas>&,
+                                 const Vectord<NumSigmas>&)>
+          meanFuncY,
+      std::function<StateVector(const StateVector&, const StateVector&)>
+          residualFuncX,
+      std::function<OutputVector(const OutputVector&, const OutputVector&)>
+          residualFuncY,
+      std::function<StateVector(const StateVector&, const StateVector&)>
+          addFuncX,
+      units::second_t dt)
+      : m_f(std::move(f)),
+        m_h(std::move(h)),
+        m_meanFuncX(std::move(meanFuncX)),
+        m_meanFuncY(std::move(meanFuncY)),
+        m_residualFuncX(std::move(residualFuncX)),
+        m_residualFuncY(std::move(residualFuncY)),
+        m_addFuncX(std::move(addFuncX)) {
+    m_contQ = MakeCovMatrix(stateStdDevs);
+    m_contR = MakeCovMatrix(measurementStdDevs);
+    m_dt = dt;
+
+    Reset();
+  }
+
+  /**
+   * Returns the square-root error covariance matrix S.
+   */
+  const StateMatrix& S() const { return m_S; }
+
+  /**
+   * Returns an element of the square-root error covariance matrix S.
+   *
+   * @param i Row of S.
+   * @param j Column of S.
+   */
+  double S(int i, int j) const { return m_S(i, j); }
+
+  /**
+   * Set the current square-root error covariance matrix S.
+   *
+   * @param S The square-root error covariance matrix S.
+   */
+  void SetS(const StateMatrix& S) { m_S = S; }
+
+  /**
+   * Returns the reconstructed error covariance matrix P.
+   */
+  StateMatrix P() const { return m_S * m_S.transpose(); }
+
+  /**
+   * Set the current square-root error covariance matrix S by taking the square
+   * root of P.
+   *
+   * @param P The error covariance matrix P.
+   */
+  void SetP(const StateMatrix& P) { m_S = P.llt().matrixL(); }
+
+  /**
+   * Returns the state estimate x-hat.
+   */
+  const StateVector& Xhat() const { return m_xHat; }
+
+  /**
+   * Returns an element of the state estimate x-hat.
+   *
+   * @param i Row of x-hat.
+   */
+  double Xhat(int i) const { return m_xHat(i); }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xHat The state estimate x-hat.
+   */
+  void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param i     Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  void SetXhat(int i, double value) { m_xHat(i) = value; }
+
+  /**
+   * Resets the observer.
+   */
+  void Reset() {
+    m_xHat.setZero();
+    m_S.setZero();
+    m_sigmasF.setZero();
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u  New control input from controller.
+   * @param dt Timestep for prediction.
+   */
+  void Predict(const InputVector& u, units::second_t dt) {
+    m_dt = dt;
+
+    // Discretize Q before projecting mean and covariance forward
+    StateMatrix contA =
+        NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
+    StateMatrix discA;
+    StateMatrix discQ;
+    DiscretizeAQ<States>(contA, m_contQ, m_dt, &discA, &discQ);
+    Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);
+
+    // Generate sigma points around the state mean
+    //
+    // equation (17)
+    Matrixd<States, NumSigmas> sigmas =
+        m_pts.SquareRootSigmaPoints(m_xHat, m_S);
+
+    // Project each sigma point forward in time according to the
+    // dynamics f(x, u)
+    //
+    //   sigmas  = 𝒳ₖ₋₁
+    //   sigmasF = 𝒳ₖ,ₖ₋₁ or just 𝒳 for readability
+    //
+    // equation (18)
+    for (int i = 0; i < NumSigmas; ++i) {
+      StateVector x = sigmas.template block<States, 1>(0, i);
+      m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
+    }
+
+    // Pass the predicted sigmas (𝒳) through the Unscented Transform
+    // to compute the prior state mean and covariance
+    //
+    // equations (18) (19) and (20)
+    auto [xHat, S] = SquareRootUnscentedTransform<States, NumSigmas>(
+        m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX,
+        discQ.template triangularView<Eigen::Lower>());
+    m_xHat = xHat;
+    m_S = S;
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   */
+  void Correct(const InputVector& u, const OutputVector& y) {
+    Correct<Outputs>(u, y, m_h, m_contR, m_meanFuncY, m_residualFuncY,
+                     m_residualFuncX, m_addFuncX);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * This is useful for when the measurement noise covariances vary.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  void Correct(const InputVector& u, const OutputVector& y,
+               const Matrixd<Outputs, Outputs>& R) {
+    Correct<Outputs>(u, y, m_h, R, m_meanFuncY, m_residualFuncY,
+                     m_residualFuncX, m_addFuncX);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * This is useful for when the measurements available during a timestep's
+   * Correct() call vary. The h(x, u) passed to the constructor is used if one
+   * is not provided (the two-argument version of this function).
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement
+   *          vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  template <int Rows>
+  void Correct(
+      const InputVector& u, const Vectord<Rows>& y,
+      std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+      const Matrixd<Rows, Rows>& R) {
+    auto meanFuncY = [](const Matrixd<Outputs, NumSigmas>& sigmas,
+                        const Vectord<NumSigmas>& Wc) -> Vectord<Rows> {
+      return sigmas * Wc;
+    };
+    auto residualFuncX = [](const StateVector& a,
+                            const StateVector& b) -> StateVector {
+      return a - b;
+    };
+    auto residualFuncY = [](const Vectord<Rows>& a,
+                            const Vectord<Rows>& b) -> Vectord<Rows> {
+      return a - b;
+    };
+    auto addFuncX = [](const StateVector& a,
+                       const StateVector& b) -> StateVector { return a + b; };
+    Correct<Rows>(u, y, std::move(h), R, std::move(meanFuncY),
+                  std::move(residualFuncY), std::move(residualFuncX),
+                  std::move(addFuncX));
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * This is useful for when the measurements available during a timestep's
+   * Correct() call vary. The h(x, u) passed to the constructor is used if one
+   * is not provided (the two-argument version of this function).
+   *
+   * @param u             Same control input used in the predict step.
+   * @param y             Measurement vector.
+   * @param h             A vector-valued function of x and u that returns the
+   *                      measurement vector.
+   * @param R             Continuous measurement noise covariance matrix.
+   * @param meanFuncY     A function that computes the mean of NumSigmas
+   *                      measurement vectors using a given set of weights.
+   * @param residualFuncY A function that computes the residual of two
+   *                      measurement vectors (i.e. it subtracts them.)
+   * @param residualFuncX A function that computes the residual of two state
+   *                      vectors (i.e. it subtracts them.)
+   * @param addFuncX      A function that adds two state vectors.
+   */
+  template <int Rows>
+  void Correct(
+      const InputVector& u, const Vectord<Rows>& y,
+      std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+      const Matrixd<Rows, Rows>& R,
+      std::function<Vectord<Rows>(const Matrixd<Rows, NumSigmas>&,
+                                  const Vectord<NumSigmas>&)>
+          meanFuncY,
+      std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
+          residualFuncY,
+      std::function<StateVector(const StateVector&, const StateVector&)>
+          residualFuncX,
+      std::function<StateVector(const StateVector&, const StateVector&)>
+          addFuncX) {
+    Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
+    Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discR);
+
+    // Generate new sigma points from the prior mean and covariance
+    // and transform them into measurement space using h(x, u)
+    //
+    //   sigmas  = 𝒳
+    //   sigmasH = 𝒴
+    //
+    // This differs from equation (22) which uses
+    // the prior sigma points, regenerating them allows
+    // multiple measurement updates per time update
+    Matrixd<Rows, NumSigmas> sigmasH;
+    Matrixd<States, NumSigmas> sigmas =
+        m_pts.SquareRootSigmaPoints(m_xHat, m_S);
+    for (int i = 0; i < NumSigmas; ++i) {
+      sigmasH.template block<Rows, 1>(0, i) =
+          h(sigmas.template block<States, 1>(0, i), u);
+    }
+
+    // Pass the predicted measurement sigmas through the Unscented Transform
+    // to compute the mean predicted measurement and square-root innovation
+    // covariance.
+    //
+    // equations (23) (24) and (25)
+    auto [yHat, Sy] = SquareRootUnscentedTransform<Rows, NumSigmas>(
+        sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY,
+        discR.template triangularView<Eigen::Lower>());
+
+    // Compute cross covariance of the predicted state and measurement sigma
+    // points given as:
+    //
+    //           2n
+    //   P_{xy} = Σ Wᵢ⁽ᶜ⁾[𝒳ᵢ - x̂][𝒴ᵢ - ŷ⁻]ᵀ
+    //           i=0
+    //
+    // equation (26)
+    Matrixd<States, Rows> Pxy;
+    Pxy.setZero();
+    for (int i = 0; i < NumSigmas; ++i) {
+      Pxy +=
+          m_pts.Wc(i) *
+          (residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
+          (residualFuncY(sigmasH.template block<Rows, 1>(0, i), yHat))
+              .transpose();
+    }
+
+    // Compute the Kalman gain. We use Eigen's QR decomposition to solve. This
+    // is equivalent to MATLAB's \ operator, so we need to rearrange to use
+    // that.
+    //
+    //   K = (P_{xy} / S_{y}ᵀ) / S_{y}
+    //   K = (S_{y} \ P_{xy})ᵀ / S_{y}
+    //   K = (S_{y}ᵀ \ (S_{y} \ P_{xy}ᵀ))ᵀ
+    //
+    // equation (27)
+    Matrixd<States, Rows> K =
+        Sy.transpose()
+            .fullPivHouseholderQr()
+            .solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose()))
+            .transpose();
+
+    // Compute the posterior state mean
+    //
+    //   x̂ = x̂⁻ + K(y − ŷ⁻)
+    //
+    // second part of equation (27)
+    m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
+
+    // Compute the intermediate matrix U for downdating
+    // the square-root covariance
+    //
+    // equation (28)
+    Matrixd<States, Rows> U = K * Sy;
+
+    // Downdate the posterior square-root state covariance
+    //
+    // equation (29)
+    for (int i = 0; i < Rows; i++) {
+      Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(
+          m_S, U.template block<States, 1>(0, i), -1);
+    }
+  }
+
+ private:
+  std::function<StateVector(const StateVector&, const InputVector&)> m_f;
+  std::function<OutputVector(const StateVector&, const InputVector&)> m_h;
+  std::function<StateVector(const Matrixd<States, NumSigmas>&,
+                            const Vectord<NumSigmas>&)>
+      m_meanFuncX;
+  std::function<OutputVector(const Matrixd<Outputs, NumSigmas>&,
+                             const Vectord<NumSigmas>&)>
+      m_meanFuncY;
+  std::function<StateVector(const StateVector&, const StateVector&)>
+      m_residualFuncX;
+  std::function<OutputVector(const OutputVector&, const OutputVector&)>
+      m_residualFuncY;
+  std::function<StateVector(const StateVector&, const StateVector&)> m_addFuncX;
+  StateVector m_xHat;
+  StateMatrix m_S;
+  StateMatrix m_contQ;
+  Matrixd<Outputs, Outputs> m_contR;
+  Matrixd<States, NumSigmas> m_sigmasF;
+  units::second_t m_dt;
+
+  SigmaPoints m_pts;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/estimator/UnscentedTransform.hpp

@@ -0,0 +1,106 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <tuple>
+
+#include <Eigen/QR>
+
+#include "frc/EigenCore.h"
+
+namespace frc {
+
+/**
+ * Computes unscented transform of a set of sigma points and weights. CovDim
+ * returns the mean and square-root covariance of the sigma points in a tuple.
+ *
+ * This works in conjunction with the UnscentedKalmanFilter class. For use with
+ * square-root form UKFs.
+ *
+ * @tparam CovDim      Dimension of covariance of sigma points after passing
+ *                     through the transform.
+ * @tparam NumSigmas   Number of sigma points.
+ * @param sigmas       List of sigma points.
+ * @param Wm           Weights for the mean.
+ * @param Wc           Weights for the covariance.
+ * @param meanFunc     A function that computes the mean of NumSigmas state
+ *                     vectors using a given set of weights.
+ * @param residualFunc A function that computes the residual of two state
+ *                     vectors (i.e. it subtracts them.)
+ * @param squareRootR  Square-root of the noise covariance of the sigma points.
+ *
+ * @return Tuple of x, mean of sigma points; S, square-root covariance of
+ * sigmas.
+ */
+template <int CovDim, int NumSigmas>
+std::tuple<Vectord<CovDim>, Matrixd<CovDim, CovDim>>
+SquareRootUnscentedTransform(
+    const Matrixd<CovDim, NumSigmas>& sigmas, const Vectord<NumSigmas>& Wm,
+    const Vectord<NumSigmas>& Wc,
+    std::function<Vectord<CovDim>(const Matrixd<CovDim, NumSigmas>&,
+                                  const Vectord<NumSigmas>&)>
+        meanFunc,
+    std::function<Vectord<CovDim>(const Vectord<CovDim>&,
+                                  const Vectord<CovDim>&)>
+        residualFunc,
+    const Matrixd<CovDim, CovDim>& squareRootR) {
+  // New mean is usually just the sum of the sigmas * weights:
+  //
+  //      2n
+  //   x̂ = Σ Wᵢ⁽ᵐ⁾𝒳ᵢ
+  //      i=0
+  //
+  // equations (19) and (23) in the paper show this,
+  // but we allow a custom function, usually for angle wrapping
+  Vectord<CovDim> x = meanFunc(sigmas, Wm);
+
+  // Form an intermediate matrix S⁻ as:
+  //
+  //   [√{W₁⁽ᶜ⁾}(𝒳_{1:2L} - x̂) √{Rᵛ}]
+  //
+  // the part of equations (20) and (24) within the "qr{}"
+  //
+  // Note that we allow a custom function instead of the difference to allow
+  // angle wrapping. Furthermore, we allow an arbitrary number of sigma points
+  // to support similar methods such as the Scaled Spherical Simplex Filter
+  // (S3F).
+  Matrixd<CovDim, NumSigmas - 1 + CovDim> Sbar;
+  for (int i = 0; i < NumSigmas - 1; i++) {
+    Sbar.template block<CovDim, 1>(0, i) =
+        std::sqrt(Wc[1]) *
+        residualFunc(sigmas.template block<CovDim, 1>(0, 1 + i), x);
+  }
+  Sbar.template block<CovDim, CovDim>(0, NumSigmas - 1) = squareRootR;
+
+  // Compute the square-root covariance of the sigma points.
+  //
+  // We transpose S⁻ first because we formed it by horizontally
+  // concatenating each part; it should be vertical so we can take
+  // the QR decomposition as defined in the "QR Decomposition" passage
+  // of section 3. "EFFICIENT SQUARE-ROOT IMPLEMENTATION"
+  //
+  // The resulting matrix R is the square-root covariance S, but it
+  // is upper triangular, so we need to transpose it.
+  //
+  // equations (20) and (24)
+  Matrixd<CovDim, CovDim> S = Sbar.transpose()
+                                  .householderQr()
+                                  .matrixQR()
+                                  .template block<CovDim, CovDim>(0, 0)
+                                  .template triangularView<Eigen::Upper>()
+                                  .transpose();
+
+  // Update or downdate the square-root covariance with (𝒳₀-x̂)
+  // depending on whether its weight (W₀⁽ᶜ⁾) is positive or negative.
+  //
+  // equations (21) and (25)
+  Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(
+      S, residualFunc(sigmas.template block<CovDim, 1>(0, 0), x), Wc[0]);
+
+  return std::make_tuple(x, S);
+}
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/filter/Debouncer.hpp

@@ -0,0 +1,98 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/timestamp.h>
+
+#include "units/time.h"
+
+namespace frc {
+/**
+ * A simple debounce filter for boolean streams.  Requires that the boolean
+ * change value from baseline for a specified period of time before the filtered
+ * value changes.
+ */
+class WPILIB_DLLEXPORT Debouncer {
+ public:
+  /**
+   * Type of debouncing to perform.
+   */
+  enum DebounceType {
+    /// Rising edge.
+    kRising,
+    /// Falling edge.
+    kFalling,
+    /// Both rising and falling edges.
+    kBoth
+  };
+
+  /**
+   * Creates a new Debouncer.
+   *
+   * @param debounceTime The number of seconds the value must change from
+   *                     baseline for the filtered value to change.
+   * @param type         Which type of state change the debouncing will be
+   *                     performed on.
+   */
+  explicit Debouncer(units::second_t debounceTime,
+                     DebounceType type = DebounceType::kRising);
+
+  /**
+   * Applies the debouncer to the input stream.
+   *
+   * @param input The current value of the input stream.
+   * @return The debounced value of the input stream.
+   */
+  bool Calculate(bool input);
+
+  /**
+   * Sets the time to debounce.
+   *
+   * @param time The number of seconds the value must change from baseline
+   *             for the filtered value to change.
+   */
+  constexpr void SetDebounceTime(units::second_t time) {
+    m_debounceTime = time;
+  }
+
+  /**
+   * Gets the time to debounce.
+   *
+   * @return The number of seconds the value must change from baseline
+   *             for the filtered value to change.
+   */
+  constexpr units::second_t GetDebounceTime() const { return m_debounceTime; }
+
+  /**
+   * Set the debounce type.
+   *
+   * @param type Which type of state change the debouncing will be performed on.
+   */
+  constexpr void SetDebounceType(DebounceType type) {
+    m_debounceType = type;
+
+    m_baseline = m_debounceType == DebounceType::kFalling;
+  }
+
+  /**
+   * Get the debounce type.
+   *
+   * @return Which type of state change the debouncing will be performed on.
+   */
+  constexpr DebounceType GetDebounceType() const { return m_debounceType; }
+
+ private:
+  units::second_t m_debounceTime;
+  bool m_baseline;
+  DebounceType m_debounceType;
+
+  units::second_t m_prevTime;
+
+  void ResetTimer();
+
+  bool HasElapsed() const;
+};
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/filter/LinearFilter.hpp

@@ -0,0 +1,410 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <initializer_list>
+#include <span>
+#include <stdexcept>
+#include <type_traits>
+#include <vector>
+
+#include <Eigen/QR>
+#include <gcem.hpp>
+#include <wpi/array.h>
+#include <wpi/circular_buffer.h>
+
+#include "frc/EigenCore.h"
+#include "units/time.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+/**
+ * This class implements a linear, digital filter. All types of FIR and IIR
+ * filters are supported. Static factory methods are provided to create commonly
+ * used types of filters.
+ *
+ * Filters are of the form:<br>
+ *  y[n] = (b0 x[n] + b1 x[n-1] + … + bP x[n-P]) -
+ *         (a0 y[n-1] + a2 y[n-2] + … + aQ y[n-Q])
+ *
+ * Where:<br>
+ *  y[n] is the output at time "n"<br>
+ *  x[n] is the input at time "n"<br>
+ *  y[n-1] is the output from the LAST time step ("n-1")<br>
+ *  x[n-1] is the input from the LAST time step ("n-1")<br>
+ *  b0 … bP are the "feedforward" (FIR) gains<br>
+ *  a0 … aQ are the "feedback" (IIR) gains<br>
+ * IMPORTANT! Note the "-" sign in front of the feedback term! This is a common
+ *            convention in signal processing.
+ *
+ * What can linear filters do? Basically, they can filter, or diminish, the
+ * effects of undesirable input frequencies. High frequencies, or rapid changes,
+ * can be indicative of sensor noise or be otherwise undesirable. A "low pass"
+ * filter smooths out the signal, reducing the impact of these high frequency
+ * components.  Likewise, a "high pass" filter gets rid of slow-moving signal
+ * components, letting you detect large changes more easily.
+ *
+ * Example FRC applications of filters:
+ *  - Getting rid of noise from an analog sensor input (note: the roboRIO's FPGA
+ *    can do this faster in hardware)
+ *  - Smoothing out joystick input to prevent the wheels from slipping or the
+ *    robot from tipping
+ *  - Smoothing motor commands so that unnecessary strain isn't put on
+ *    electrical or mechanical components
+ *  - If you use clever gains, you can make a PID controller out of this class!
+ *
+ * For more on filters, we highly recommend the following articles:<br>
+ * https://en.wikipedia.org/wiki/Linear_filter<br>
+ * https://en.wikipedia.org/wiki/Iir_filter<br>
+ * https://en.wikipedia.org/wiki/Fir_filter<br>
+ *
+ * Note 1: Calculate() should be called by the user on a known, regular period.
+ * You can use a Notifier for this or do it "inline" with code in a
+ * periodic function.
+ *
+ * Note 2: For ALL filters, gains are necessarily a function of frequency. If
+ * you make a filter that works well for you at, say, 100Hz, you will most
+ * definitely need to adjust the gains if you then want to run it at 200Hz!
+ * Combining this with Note 1 - the impetus is on YOU as a developer to make
+ * sure Calculate() gets called at the desired, constant frequency!
+ */
+template <class T>
+class LinearFilter {
+ public:
+  /**
+   * Create a linear FIR or IIR filter.
+   *
+   * @param ffGains The "feedforward" or FIR gains.
+   * @param fbGains The "feedback" or IIR gains.
+   */
+  constexpr LinearFilter(std::span<const double> ffGains,
+                         std::span<const double> fbGains)
+      : m_inputs(ffGains.size()),
+        m_outputs(fbGains.size()),
+        m_inputGains(ffGains.begin(), ffGains.end()),
+        m_outputGains(fbGains.begin(), fbGains.end()) {
+    for (size_t i = 0; i < ffGains.size(); ++i) {
+      m_inputs.emplace_front(0.0);
+    }
+    for (size_t i = 0; i < fbGains.size(); ++i) {
+      m_outputs.emplace_front(0.0);
+    }
+
+    if (!std::is_constant_evaluated()) {
+      ++instances;
+      wpi::math::MathSharedStore::ReportUsage("LinearFilter",
+                                              std::to_string(instances));
+    }
+  }
+
+  /**
+   * Create a linear FIR or IIR filter.
+   *
+   * @param ffGains The "feedforward" or FIR gains.
+   * @param fbGains The "feedback" or IIR gains.
+   */
+  constexpr LinearFilter(std::initializer_list<double> ffGains,
+                         std::initializer_list<double> fbGains)
+      : LinearFilter({ffGains.begin(), ffGains.end()},
+                     {fbGains.begin(), fbGains.end()}) {}
+
+  // Static methods to create commonly used filters
+  /**
+   * Creates a one-pole IIR low-pass filter of the form:<br>
+   *   y[n] = (1 - gain) x[n] + gain y[n-1]<br>
+   * where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
+   *
+   * Note: T = 1 / (2πf) where f is the cutoff frequency in Hz, the frequency
+   * above which the input starts to attenuate.
+   *
+   * This filter is stable for time constants greater than zero.
+   *
+   * @param timeConstant The discrete-time time constant in seconds.
+   * @param period       The period in seconds between samples taken by the
+   *                     user.
+   */
+  static constexpr LinearFilter<T> SinglePoleIIR(double timeConstant,
+                                                 units::second_t period) {
+    double gain = gcem::exp(-period.value() / timeConstant);
+    return LinearFilter({1.0 - gain}, {-gain});
+  }
+
+  /**
+   * Creates a first-order high-pass filter of the form:<br>
+   *   y[n] = gain x[n] + (-gain) x[n-1] + gain y[n-1]<br>
+   * where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
+   *
+   * Note: T = 1 / (2πf) where f is the cutoff frequency in Hz, the frequency
+   * below which the input starts to attenuate.
+   *
+   * This filter is stable for time constants greater than zero.
+   *
+   * @param timeConstant The discrete-time time constant in seconds.
+   * @param period       The period in seconds between samples taken by the
+   *                     user.
+   */
+  static constexpr LinearFilter<T> HighPass(double timeConstant,
+                                            units::second_t period) {
+    double gain = gcem::exp(-period.value() / timeConstant);
+    return LinearFilter({gain, -gain}, {-gain});
+  }
+
+  /**
+   * Creates a K-tap FIR moving average filter of the form:<br>
+   *   y[n] = 1/k (x[k] + x[k-1] + … + x[0])
+   *
+   * This filter is always stable.
+   *
+   * @param taps The number of samples to average over. Higher = smoother but
+   *             slower
+   * @throws std::runtime_error if number of taps is less than 1.
+   */
+  static LinearFilter<T> MovingAverage(int taps) {
+    if (taps <= 0) {
+      throw std::runtime_error("Number of taps must be greater than zero.");
+    }
+
+    std::vector<double> gains(taps, 1.0 / taps);
+    return LinearFilter(gains, {});
+  }
+
+  /**
+   * Creates a finite difference filter that computes the nth derivative of the
+   * input given the specified stencil points.
+   *
+   * Stencil points are the indices of the samples to use in the finite
+   * difference. 0 is the current sample, -1 is the previous sample, -2 is the
+   * sample before that, etc. Don't use positive stencil points (samples from
+   * the future) if the LinearFilter will be used for stream-based online
+   * filtering (e.g., taking derivative of encoder samples in real-time).
+   *
+   * @tparam Derivative The order of the derivative to compute.
+   * @tparam Samples    The number of samples to use to compute the given
+   *                    derivative. This must be one more than the order of
+   *                    the derivative or higher.
+   * @param stencil     List of stencil points.
+   * @param period      The period in seconds between samples taken by the user.
+   */
+  template <int Derivative, int Samples>
+  static LinearFilter<T> FiniteDifference(
+      const wpi::array<int, Samples>& stencil, units::second_t period) {
+    // See
+    // https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points
+    //
+    // For a given list of stencil points s of length n and the order of
+    // derivative d < n, the finite difference coefficients can be obtained by
+    // solving the following linear system for the vector a.
+    //
+    // [s₁⁰   ⋯  sₙ⁰ ][a₁]      [ δ₀,d ]
+    // [ ⋮    ⋱  ⋮   ][⋮ ] = d! [  ⋮   ]
+    // [s₁ⁿ⁻¹ ⋯ sₙⁿ⁻¹][aₙ]      [δₙ₋₁,d]
+    //
+    // where δᵢ,ⱼ are the Kronecker delta. The FIR gains are the elements of the
+    // vector a in reverse order divided by hᵈ.
+    //
+    // The order of accuracy of the approximation is of the form O(hⁿ⁻ᵈ).
+
+    static_assert(Derivative >= 1,
+                  "Order of derivative must be greater than or equal to one.");
+    static_assert(Samples > 0, "Number of samples must be greater than zero.");
+    static_assert(Derivative < Samples,
+                  "Order of derivative must be less than number of samples.");
+
+    Matrixd<Samples, Samples> S;
+    for (int row = 0; row < Samples; ++row) {
+      for (int col = 0; col < Samples; ++col) {
+        S(row, col) = gcem::pow(stencil[col], row);
+      }
+    }
+
+    // Fill in Kronecker deltas: https://en.wikipedia.org/wiki/Kronecker_delta
+    Vectord<Samples> d;
+    for (int i = 0; i < Samples; ++i) {
+      d(i) = (i == Derivative) ? Factorial(Derivative) : 0.0;
+    }
+
+    Vectord<Samples> a =
+        S.householderQr().solve(d) / gcem::pow(period.value(), Derivative);
+
+    // Reverse gains list
+    std::vector<double> ffGains;
+    for (int i = Samples - 1; i >= 0; --i) {
+      ffGains.push_back(a(i));
+    }
+
+    return LinearFilter(ffGains, {});
+  }
+
+  /**
+   * Creates a backward finite difference filter that computes the nth
+   * derivative of the input given the specified number of samples.
+   *
+   * For example, a first derivative filter that uses two samples and a sample
+   * period of 20 ms would be
+   *
+   * <pre><code>
+   * LinearFilter<double>::BackwardFiniteDifference<1, 2>(20_ms);
+   * </code></pre>
+   *
+   * @tparam Derivative The order of the derivative to compute.
+   * @tparam Samples    The number of samples to use to compute the given
+   *                    derivative. This must be one more than the order of
+   *                    derivative or higher.
+   * @param period      The period in seconds between samples taken by the user.
+   */
+  template <int Derivative, int Samples>
+  static LinearFilter<T> BackwardFiniteDifference(units::second_t period) {
+    // Generate stencil points from -(samples - 1) to 0
+    wpi::array<int, Samples> stencil{wpi::empty_array};
+    for (int i = 0; i < Samples; ++i) {
+      stencil[i] = -(Samples - 1) + i;
+    }
+
+    return FiniteDifference<Derivative, Samples>(stencil, period);
+  }
+
+  /**
+   * Reset the filter state.
+   */
+  constexpr void Reset() {
+    std::fill(m_inputs.begin(), m_inputs.end(), T{0.0});
+    std::fill(m_outputs.begin(), m_outputs.end(), T{0.0});
+  }
+
+  /**
+   * Resets the filter state, initializing internal buffers to the provided
+   * values.
+   *
+   * These are the expected lengths of the buffers, depending on what type of
+   * linear filter used:
+   *
+   * <table>
+   *   <tr>
+   *     <th>Type</th>
+   *     <th>Input Buffer Size</th>
+   *     <th>Output Buffer Size</th>
+   *   </tr>
+   *   <tr>
+   *     <td>Unspecified</td>
+   *     <td>size of {@code ffGains}</td>
+   *     <td>size of {@code fbGains}</td>
+   *   </tr>
+   *   <tr>
+   *     <td>Single Pole IIR</td>
+   *     <td>1</td>
+   *     <td>1</td>
+   *   </tr>
+   *   <tr>
+   *     <td>High-Pass</td>
+   *     <td>2</td>
+   *     <td>1</td>
+   *   </tr>
+   *   <tr>
+   *     <td>Moving Average</td>
+   *     <td>{@code taps}</td>
+   *     <td>0</td>
+   *   </tr>
+   *   <tr>
+   *     <td>Finite Difference</td>
+   *     <td>size of {@code stencil}</td>
+   *     <td>0</td>
+   *   </tr>
+   *   <tr>
+   *     <td>Backward Finite Difference</td>
+   *     <td>{@code Samples}</td>
+   *     <td>0</td>
+   *   </tr>
+   * </table>
+   *
+   * @param inputBuffer Values to initialize input buffer.
+   * @param outputBuffer Values to initialize output buffer.
+   * @throws std::runtime_error if size of inputBuffer or outputBuffer does not
+   *   match the size of ffGains and fbGains provided in the constructor.
+   */
+  constexpr void Reset(std::span<const T> inputBuffer,
+                       std::span<const T> outputBuffer) {
+    // Clear buffers
+    Reset();
+
+    if (inputBuffer.size() != m_inputGains.size() ||
+        outputBuffer.size() != m_outputGains.size()) {
+      throw std::runtime_error(
+          "Incorrect length of inputBuffer or outputBuffer");
+    }
+
+    for (size_t i = 0; i < inputBuffer.size(); ++i) {
+      m_inputs.push_front(inputBuffer[i]);
+    }
+    for (size_t i = 0; i < outputBuffer.size(); ++i) {
+      m_outputs.push_front(outputBuffer[i]);
+    }
+  }
+
+  /**
+   * Calculates the next value of the filter.
+   *
+   * @param input Current input value.
+   *
+   * @return The filtered value at this step
+   */
+  constexpr T Calculate(T input) {
+    T retVal{0.0};
+
+    // Rotate the inputs
+    if (m_inputGains.size() > 0) {
+      m_inputs.push_front(input);
+    }
+
+    // Calculate the new value
+    for (size_t i = 0; i < m_inputGains.size(); ++i) {
+      retVal += m_inputs[i] * m_inputGains[i];
+    }
+    for (size_t i = 0; i < m_outputGains.size(); ++i) {
+      retVal -= m_outputs[i] * m_outputGains[i];
+    }
+
+    // Rotate the outputs
+    if (m_outputGains.size() > 0) {
+      m_outputs.push_front(retVal);
+    }
+
+    m_lastOutput = retVal;
+    return retVal;
+  }
+
+  /**
+   * Returns the last value calculated by the LinearFilter.
+   *
+   * @return The last value.
+   */
+  constexpr T LastValue() const { return m_lastOutput; }
+
+ private:
+  wpi::circular_buffer<T> m_inputs;
+  wpi::circular_buffer<T> m_outputs;
+  std::vector<double> m_inputGains;
+  std::vector<double> m_outputGains;
+  T m_lastOutput{0.0};
+
+  // Usage reporting instances
+  inline static int instances = 0;
+
+  /**
+   * Factorial of n.
+   *
+   * @param n Argument of which to take factorial.
+   */
+  static constexpr int Factorial(int n) {
+    if (n < 2) {
+      return 1;
+    } else {
+      return n * Factorial(n - 1);
+    }
+  }
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/filter/MedianFilter.hpp

@@ -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.
+
+#pragma once
+
+#include <algorithm>
+#include <vector>
+
+#include <wpi/Algorithm.h>
+#include <wpi/circular_buffer.h>
+
+namespace frc {
+/**
+ * A class that implements a moving-window median filter.  Useful for reducing
+ * measurement noise, especially with processes that generate occasional,
+ * extreme outliers (such as values from vision processing, LIDAR, or ultrasonic
+ * sensors).
+ */
+template <class T>
+class MedianFilter {
+ public:
+  /**
+   * Creates a new MedianFilter.
+   *
+   * @param size The number of samples in the moving window.
+   */
+  constexpr explicit MedianFilter(size_t size)
+      : m_valueBuffer(size), m_size{size} {}
+
+  /**
+   * Calculates the moving-window median for the next value of the input stream.
+   *
+   * @param next The next input value.
+   * @return The median of the moving window, updated to include the next value.
+   */
+  constexpr T Calculate(T next) {
+    // Insert next value at proper point in sorted array
+    wpi::insert_sorted(m_orderedValues, next);
+
+    size_t curSize = m_orderedValues.size();
+
+    // If buffer is at max size, pop element off of end of circular buffer
+    // and remove from ordered list
+    if (curSize > m_size) {
+      m_orderedValues.erase(std::find(m_orderedValues.begin(),
+                                      m_orderedValues.end(),
+                                      m_valueBuffer.pop_back()));
+      --curSize;
+    }
+
+    // Add next value to circular buffer
+    m_valueBuffer.push_front(next);
+
+    if (curSize % 2 != 0) {
+      // If size is odd, return middle element of sorted list
+      return m_orderedValues[curSize / 2];
+    } else {
+      // If size is even, return average of middle elements
+      return (m_orderedValues[curSize / 2 - 1] + m_orderedValues[curSize / 2]) /
+             2.0;
+    }
+  }
+
+  /**
+   * Returns the last value calculated by the MedianFilter.
+   *
+   * @return The last value.
+   */
+  constexpr T LastValue() const { return m_valueBuffer.front(); }
+
+  /**
+   * Resets the filter, clearing the window of all elements.
+   */
+  constexpr void Reset() {
+    m_orderedValues.clear();
+    m_valueBuffer.reset();
+  }
+
+ private:
+  wpi::circular_buffer<T> m_valueBuffer;
+  std::vector<T> m_orderedValues;
+  size_t m_size;
+};
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/filter/SlewRateLimiter.hpp

@@ -0,0 +1,101 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+
+#include <wpi/timestamp.h>
+
+#include "units/time.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+/**
+ * A class that limits the rate of change of an input value.  Useful for
+ * implementing voltage, setpoint, and/or output ramps.  A slew-rate limit
+ * is most appropriate when the quantity being controlled is a velocity or
+ * a voltage; when controlling a position, consider using a TrapezoidProfile
+ * instead.
+ *
+ * @see TrapezoidProfile
+ */
+template <class Unit>
+class SlewRateLimiter {
+ public:
+  using Unit_t = units::unit_t<Unit>;
+  using Rate = units::compound_unit<Unit, units::inverse<units::seconds>>;
+  using Rate_t = units::unit_t<Rate>;
+
+  /**
+   * Creates a new SlewRateLimiter with the given positive and negative rate
+   * limits and initial value.
+   *
+   * @param positiveRateLimit The rate-of-change limit in the positive
+   *                          direction, in units per second. This is expected
+   *                          to be positive.
+   * @param negativeRateLimit The rate-of-change limit in the negative
+   *                          direction, in units per second. This is expected
+   *                          to be negative.
+   * @param initialValue The initial value of the input.
+   */
+  SlewRateLimiter(Rate_t positiveRateLimit, Rate_t negativeRateLimit,
+                  Unit_t initialValue = Unit_t{0})
+      : m_positiveRateLimit{positiveRateLimit},
+        m_negativeRateLimit{negativeRateLimit},
+        m_prevVal{initialValue},
+        m_prevTime{
+            units::microsecond_t{wpi::math::MathSharedStore::GetTimestamp()}} {}
+
+  /**
+   * Creates a new SlewRateLimiter with the given positive rate limit and
+   * negative rate limit of -rateLimit.
+   *
+   * @param rateLimit The rate-of-change limit.
+   */
+  explicit SlewRateLimiter(Rate_t rateLimit)
+      : SlewRateLimiter(rateLimit, -rateLimit) {}
+
+  /**
+   * Filters the input to limit its slew rate.
+   *
+   * @param input The input value whose slew rate is to be limited.
+   * @return The filtered value, which will not change faster than the slew
+   * rate.
+   */
+  Unit_t Calculate(Unit_t input) {
+    units::second_t currentTime = wpi::math::MathSharedStore::GetTimestamp();
+    units::second_t elapsedTime = currentTime - m_prevTime;
+    m_prevVal +=
+        std::clamp(input - m_prevVal, m_negativeRateLimit * elapsedTime,
+                   m_positiveRateLimit * elapsedTime);
+    m_prevTime = currentTime;
+    return m_prevVal;
+  }
+
+  /**
+   * Returns the value last calculated by the SlewRateLimiter.
+   *
+   * @return The last value.
+   */
+  Unit_t LastValue() const { return m_prevVal; }
+
+  /**
+   * Resets the slew rate limiter to the specified value; ignores the rate limit
+   * when doing so.
+   *
+   * @param value The value to reset to.
+   */
+  void Reset(Unit_t value) {
+    m_prevVal = value;
+    m_prevTime = wpi::math::MathSharedStore::GetTimestamp();
+  }
+
+ private:
+  Rate_t m_positiveRateLimit;
+  Rate_t m_negativeRateLimit;
+  Unit_t m_prevVal;
+  units::second_t m_prevTime;
+};
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/fmt/Eigen.hpp

@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <concepts>
+
+#include <Eigen/Core>
+#include <Eigen/SparseCore>
+#include <fmt/format.h>
+
+// FIXME: Doxygen gives internal inconsistency errors:
+
+//! @cond Doxygen_Suppress
+
+/**
+ * Formatter for classes derived from Eigen::DenseBase<Derived> or
+ * Eigen::SparseCompressedBase<Derived>.
+ */
+template <typename Derived, typename CharT>
+  requires std::derived_from<Derived, Eigen::DenseBase<Derived>> ||
+           std::derived_from<Derived, Eigen::SparseCompressedBase<Derived>>
+struct fmt::formatter<Derived, CharT> {
+  template <typename ParseContext>
+  constexpr auto parse(ParseContext& ctx) {
+    return m_underlying.parse(ctx);
+  }
+
+  template <typename FmtContext>
+  auto format(const Derived& mat, FmtContext& ctx) const {
+    auto out = ctx.out();
+
+    for (int row = 0; row < mat.rows(); ++row) {
+      for (int col = 0; col < mat.cols(); ++col) {
+        out = fmt::format_to(out, "  ");
+        out = m_underlying.format(mat.coeff(row, col), ctx);
+      }
+
+      if (row < mat.rows() - 1) {
+        out = fmt::format_to(out, "\n");
+      }
+    }
+
+    return out;
+  }
+
+ private:
+  fmt::formatter<typename Derived::Scalar, CharT> m_underlying;
+};
+//! @endcond

wpimath/src/main/native/include/wpi/math/geometry/CoordinateAxis.hpp

@@ -0,0 +1,77 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <Eigen/Core>
+#include <gcem.hpp>
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Rotation3d.h"
+
+namespace frc {
+
+/**
+ * A class representing a coordinate system axis within the NWU coordinate
+ * system.
+ */
+class WPILIB_DLLEXPORT CoordinateAxis {
+ public:
+  /**
+   * Constructs a coordinate system axis within the NWU coordinate system and
+   * normalizes it.
+   *
+   * @param x The x component.
+   * @param y The y component.
+   * @param z The z component.
+   */
+  constexpr CoordinateAxis(double x, double y, double z) {
+    double norm = gcem::hypot(x, y, z);
+    m_axis = Eigen::Vector3d{{x / norm, y / norm, z / norm}};
+  }
+
+  CoordinateAxis(const CoordinateAxis&) = default;
+  CoordinateAxis& operator=(const CoordinateAxis&) = default;
+
+  CoordinateAxis(CoordinateAxis&&) = default;
+  CoordinateAxis& operator=(CoordinateAxis&&) = default;
+
+  /**
+   * Returns a coordinate axis corresponding to +X in the NWU coordinate system.
+   */
+  static constexpr CoordinateAxis N() { return CoordinateAxis{1.0, 0.0, 0.0}; }
+
+  /**
+   * Returns a coordinate axis corresponding to -X in the NWU coordinate system.
+   */
+  static constexpr CoordinateAxis S() { return CoordinateAxis{-1.0, 0.0, 0.0}; }
+
+  /**
+   * Returns a coordinate axis corresponding to -Y in the NWU coordinate system.
+   */
+  static constexpr CoordinateAxis E() { return CoordinateAxis{0.0, -1.0, 0.0}; }
+
+  /**
+   * Returns a coordinate axis corresponding to +Y in the NWU coordinate system.
+   */
+  static constexpr CoordinateAxis W() { return CoordinateAxis{0.0, 1.0, 0.0}; }
+
+  /**
+   * Returns a coordinate axis corresponding to +Z in the NWU coordinate system.
+   */
+  static constexpr CoordinateAxis U() { return CoordinateAxis{0.0, 0.0, 1.0}; }
+
+  /**
+   * Returns a coordinate axis corresponding to -Z in the NWU coordinate system.
+   */
+  static constexpr CoordinateAxis D() { return CoordinateAxis{0.0, 0.0, -1.0}; }
+
+ private:
+  friend class CoordinateSystem;
+
+  Eigen::Vector3d m_axis;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp

@@ -0,0 +1,143 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/CoordinateAxis.h"
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Rotation3d.h"
+#include "frc/geometry/Translation3d.h"
+
+namespace frc {
+
+/**
+ * A helper class that converts Pose3d objects between different standard
+ * coordinate frames.
+ */
+class WPILIB_DLLEXPORT CoordinateSystem {
+ public:
+  /**
+   * Constructs a coordinate system with the given cardinal directions for each
+   * axis.
+   *
+   * @param positiveX The cardinal direction of the positive x-axis.
+   * @param positiveY The cardinal direction of the positive y-axis.
+   * @param positiveZ The cardinal direction of the positive z-axis.
+   * @throws std::domain_error if the coordinate system isn't special orthogonal
+   */
+  constexpr CoordinateSystem(const CoordinateAxis& positiveX,
+                             const CoordinateAxis& positiveY,
+                             const CoordinateAxis& positiveZ) {
+    // Construct a change of basis matrix from the source coordinate system to
+    // the NWU coordinate system. Each column vector in the change of basis
+    // matrix is one of the old basis vectors mapped to its representation in
+    // the new basis.
+    Eigen::Matrix3d R{
+        {positiveX.m_axis(0), positiveY.m_axis(0), positiveZ.m_axis(0)},
+        {positiveX.m_axis(1), positiveY.m_axis(1), positiveZ.m_axis(1)},
+        {positiveX.m_axis(2), positiveY.m_axis(2), positiveZ.m_axis(2)}};
+
+    // The change of basis matrix should be a pure rotation. The Rotation3d
+    // constructor will verify this by checking for special orthogonality.
+    m_rotation = Rotation3d{R};
+  }
+
+  /**
+   * Returns an instance of the North-West-Up (NWU) coordinate system.
+   *
+   * The +X axis is north, the +Y axis is west, and the +Z axis is up.
+   */
+  constexpr static CoordinateSystem NWU() {
+    return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::W(),
+                            CoordinateAxis::U()};
+  }
+
+  /**
+   * Returns an instance of the East-Down-North (EDN) coordinate system.
+   *
+   * The +X axis is east, the +Y axis is down, and the +Z axis is north.
+   */
+  constexpr static CoordinateSystem EDN() {
+    return CoordinateSystem{CoordinateAxis::E(), CoordinateAxis::D(),
+                            CoordinateAxis::N()};
+  }
+
+  /**
+   * Returns an instance of the NED coordinate system.
+   *
+   * The +X axis is north, the +Y axis is east, and the +Z axis is down.
+   */
+  constexpr static CoordinateSystem NED() {
+    return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::E(),
+                            CoordinateAxis::D()};
+  }
+
+  /**
+   * Converts the given translation from one coordinate system to another.
+   *
+   * @param translation The translation to convert.
+   * @param from The coordinate system the translation starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given translation in the desired coordinate system.
+   */
+  constexpr static Translation3d Convert(const Translation3d& translation,
+                                         const CoordinateSystem& from,
+                                         const CoordinateSystem& to) {
+    return translation.RotateBy(from.m_rotation - to.m_rotation);
+  }
+
+  /**
+   * Converts the given rotation from one coordinate system to another.
+   *
+   * @param rotation The rotation to convert.
+   * @param from The coordinate system the rotation starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given rotation in the desired coordinate system.
+   */
+  constexpr static Rotation3d Convert(const Rotation3d& rotation,
+                                      const CoordinateSystem& from,
+                                      const CoordinateSystem& to) {
+    return rotation.RotateBy(from.m_rotation - to.m_rotation);
+  }
+
+  /**
+   * Converts the given pose from one coordinate system to another.
+   *
+   * @param pose The pose to convert.
+   * @param from The coordinate system the pose starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given pose in the desired coordinate system.
+   */
+  constexpr static Pose3d Convert(const Pose3d& pose,
+                                  const CoordinateSystem& from,
+                                  const CoordinateSystem& to) {
+    return Pose3d{Convert(pose.Translation(), from, to),
+                  Convert(pose.Rotation(), from, to)};
+  }
+
+  /**
+   * Converts the given transform from one coordinate system to another.
+   *
+   * @param transform The transform to convert.
+   * @param from The coordinate system the transform starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given transform in the desired coordinate system.
+   */
+  constexpr static Transform3d Convert(const Transform3d& transform,
+                                       const CoordinateSystem& from,
+                                       const CoordinateSystem& to) {
+    const auto coordRot = from.m_rotation - to.m_rotation;
+    return Transform3d{
+        Convert(transform.Translation(), from, to),
+        (-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))};
+  }
+
+ private:
+  // Rotation from this coordinate system to NWU coordinate system
+  Rotation3d m_rotation;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/geometry/Ellipse2d.hpp

@@ -0,0 +1,216 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdexcept>
+
+#include <gcem.hpp>
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Transform2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+
+/**
+ * Represents a 2d ellipse space containing translational, rotational, and
+ * scaling components.
+ */
+class WPILIB_DLLEXPORT Ellipse2d {
+ public:
+  /**
+   * Constructs an ellipse around a center point and two semi-axes, a horizontal
+   * and vertical one.
+   *
+   * @param center The center of the ellipse.
+   * @param xSemiAxis The x semi-axis.
+   * @param ySemiAxis The y semi-axis.
+   */
+  constexpr Ellipse2d(const Pose2d& center, units::meter_t xSemiAxis,
+                      units::meter_t ySemiAxis)
+      : m_center{center}, m_xSemiAxis{xSemiAxis}, m_ySemiAxis{ySemiAxis} {
+    if (xSemiAxis <= 0_m || ySemiAxis <= 0_m) {
+      throw std::invalid_argument("Ellipse2d semi-axes must be positive");
+    }
+  }
+
+  /**
+   * Constructs a perfectly circular ellipse with the specified radius.
+   *
+   * @param center The center of the circle.
+   * @param radius The radius of the circle.
+   */
+  constexpr Ellipse2d(const Translation2d& center, double radius)
+      : m_center{center, Rotation2d{}},
+        m_xSemiAxis{radius},
+        m_ySemiAxis{radius} {}
+
+  /**
+   * Returns the center of the ellipse.
+   *
+   * @return The center of the ellipse.
+   */
+  constexpr const Pose2d& Center() const { return m_center; }
+
+  /**
+   * Returns the rotational component of the ellipse.
+   *
+   * @return The rotational component of the ellipse.
+   */
+  constexpr const Rotation2d& Rotation() const { return m_center.Rotation(); }
+
+  /**
+   * Returns the x semi-axis.
+   *
+   * @return The x semi-axis.
+   */
+  constexpr units::meter_t XSemiAxis() const { return m_xSemiAxis; }
+
+  /**
+   * Returns the y semi-axis.
+   *
+   * @return The y semi-axis.
+   */
+  constexpr units::meter_t YSemiAxis() const { return m_ySemiAxis; }
+
+  /**
+   * Returns the focal points of the ellipse. In a perfect circle, this will
+   * always return the center.
+   *
+   * @return The focal points.
+   */
+  constexpr wpi::array<Translation2d, 2> FocalPoints() const {
+    // Major semi-axis
+    auto a = units::math::max(m_xSemiAxis, m_ySemiAxis);
+
+    // Minor semi-axis
+    auto b = units::math::min(m_xSemiAxis, m_ySemiAxis);
+
+    auto c = units::math::sqrt(a * a - b * b);
+
+    if (m_xSemiAxis > m_ySemiAxis) {
+      return wpi::array{
+          (m_center + Transform2d{-c, 0_m, Rotation2d{}}).Translation(),
+          (m_center + Transform2d{c, 0_m, Rotation2d{}}).Translation()};
+    } else {
+      return wpi::array{
+          (m_center + Transform2d{0_m, -c, Rotation2d{}}).Translation(),
+          (m_center + Transform2d{0_m, c, Rotation2d{}}).Translation()};
+    }
+  }
+
+  /**
+   * Transforms the center of the ellipse and returns the new ellipse.
+   *
+   * @param other The transform to transform by.
+   * @return The transformed ellipse.
+   */
+  constexpr Ellipse2d TransformBy(const Transform2d& other) const {
+    return Ellipse2d{m_center.TransformBy(other), m_xSemiAxis, m_ySemiAxis};
+  }
+
+  /**
+   * Rotates the center of the ellipse and returns the new ellipse.
+   *
+   * @param other The rotation to transform by.
+   * @return The rotated ellipse.
+   */
+  constexpr Ellipse2d RotateBy(const Rotation2d& other) const {
+    return Ellipse2d{m_center.RotateBy(other), m_xSemiAxis, m_ySemiAxis};
+  }
+
+  /**
+   * Checks if a point is intersected by this ellipse's circumference.
+   *
+   * @param point The point to check.
+   * @return True, if this ellipse's circumference intersects the point.
+   */
+  constexpr bool Intersects(const Translation2d& point) const {
+    return gcem::abs(1.0 - SolveEllipseEquation(point)) <= 1E-9;
+  }
+
+  /**
+   * Checks if a point is contained within this ellipse. This is inclusive, if
+   * the point lies on the circumference this will return {@code true}.
+   *
+   * @param point The point to check.
+   * @return True, if the point is within or on the ellipse.
+   */
+  constexpr bool Contains(const Translation2d& point) const {
+    return SolveEllipseEquation(point) <= 1.0;
+  }
+
+  /**
+   * Returns the distance between the perimeter of the ellipse and the point.
+   *
+   * @param point The point to check.
+   * @return The distance (0, if the point is contained by the ellipse)
+   */
+  units::meter_t Distance(const Translation2d& point) const {
+    return Nearest(point).Distance(point);
+  }
+
+  /**
+   * Returns the nearest point that is contained within the ellipse.
+   *
+   * @param point The point that this will find the nearest point to.
+   * @return A new point that is nearest to {@code point} and contained in the
+   *   ellipse.
+   */
+  Translation2d Nearest(const Translation2d& point) const;
+
+  /**
+   * Checks equality between this Ellipse2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  constexpr bool operator==(const Ellipse2d& other) const {
+    return m_center == other.m_center &&
+           units::math::abs(m_xSemiAxis - other.m_xSemiAxis) < 1E-9_m &&
+           units::math::abs(m_ySemiAxis - other.m_ySemiAxis) < 1E-9_m;
+  }
+
+ private:
+  Pose2d m_center;
+  units::meter_t m_xSemiAxis;
+  units::meter_t m_ySemiAxis;
+
+  /**
+   * Solves the equation of an ellipse from the given point. This is a helper
+   * function used to determine if that point lies inside of or on an ellipse.
+   *
+   * <pre>
+   * (x - h)²/a² + (y - k)²/b² = 1
+   * </pre>
+   *
+   * @param point The point to solve for.
+   * @return < 1.0 if the point lies inside the ellipse, == 1.0 if a point lies
+   * on the ellipse, and > 1.0 if the point lies outsides the ellipse.
+   */
+  constexpr double SolveEllipseEquation(const Translation2d& point) const {
+    // Rotate the point by the inverse of the ellipse's rotation
+    auto rotPoint =
+        point.RotateAround(m_center.Translation(), -m_center.Rotation());
+
+    auto x = rotPoint.X() - m_center.X();
+    auto y = rotPoint.Y() - m_center.Y();
+
+    // NOLINTNEXTLINE (bugprone-integer-division)
+    return (x * x) / (m_xSemiAxis * m_xSemiAxis) +
+           // NOLINTNEXTLINE (bugprone-integer-division)
+           (y * y) / (m_ySemiAxis * m_ySemiAxis);
+  }
+};
+
+}  // namespace frc
+
+#include "frc/geometry/proto/Ellipse2dProto.h"
+#include "frc/geometry/struct/Ellipse2dStruct.h"

wpimath/src/main/native/include/wpi/math/geometry/Pose2d.hpp

@@ -0,0 +1,297 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <initializer_list>
+#include <span>
+#include <utility>
+
+#include <gcem.hpp>
+#include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "units/length.h"
+
+namespace frc {
+
+class Transform2d;
+
+/**
+ * Represents a 2D pose containing translational and rotational elements.
+ */
+class WPILIB_DLLEXPORT Pose2d {
+ public:
+  /**
+   * Constructs a pose at the origin facing toward the positive X axis.
+   */
+  constexpr Pose2d() = default;
+
+  /**
+   * Constructs a pose with the specified translation and rotation.
+   *
+   * @param translation The translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  constexpr Pose2d(Translation2d translation, Rotation2d rotation)
+      : m_translation{std::move(translation)},
+        m_rotation{std::move(rotation)} {}
+
+  /**
+   * Constructs a pose with x and y translations instead of a separate
+   * Translation2d.
+   *
+   * @param x The x component of the translational component of the pose.
+   * @param y The y component of the translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  constexpr Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
+      : m_translation{x, y}, m_rotation{std::move(rotation)} {}
+
+  /**
+   * Constructs a pose with the specified affine transformation matrix.
+   *
+   * @param matrix The affine transformation matrix.
+   * @throws std::domain_error if the affine transformation matrix is invalid.
+   */
+  constexpr explicit Pose2d(const Eigen::Matrix3d& matrix)
+      : m_translation{Eigen::Vector2d{{matrix(0, 2)}, {matrix(1, 2)}}},
+        m_rotation{Eigen::Matrix2d{{matrix(0, 0), matrix(0, 1)},
+                                   {matrix(1, 0), matrix(1, 1)}}} {
+    if (matrix(2, 0) != 0.0 || matrix(2, 1) != 0.0 || matrix(2, 2) != 1.0) {
+      throw std::domain_error("Affine transformation matrix is invalid");
+    }
+  }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new
+   * transformed pose.
+   *
+   * <pre>
+   * [x_new]    [cos, -sin, 0][transform.x]
+   * [y_new] += [sin,  cos, 0][transform.y]
+   * [t_new]    [  0,    0, 1][transform.t]
+   * </pre>
+   *
+   * @param other The transform to transform the pose by.
+   *
+   * @return The transformed pose.
+   */
+  constexpr Pose2d operator+(const Transform2d& other) const {
+    return TransformBy(other);
+  }
+
+  /**
+   * Returns the Transform2d that maps the one pose to another.
+   *
+   * @param other The initial pose of the transformation.
+   * @return The transform that maps the other pose to the current pose.
+   */
+  constexpr Transform2d operator-(const Pose2d& other) const;
+
+  /**
+   * Checks equality between this Pose2d and another object.
+   */
+  constexpr bool operator==(const Pose2d&) const = default;
+
+  /**
+   * Returns the underlying translation.
+   *
+   * @return Reference to the translational component of the pose.
+   */
+  constexpr const Translation2d& Translation() const { return m_translation; }
+
+  /**
+   * Returns the X component of the pose's translation.
+   *
+   * @return The x component of the pose's translation.
+   */
+  constexpr units::meter_t X() const { return m_translation.X(); }
+
+  /**
+   * Returns the Y component of the pose's translation.
+   *
+   * @return The y component of the pose's translation.
+   */
+  constexpr units::meter_t Y() const { return m_translation.Y(); }
+
+  /**
+   * Returns the underlying rotation.
+   *
+   * @return Reference to the rotational component of the pose.
+   */
+  constexpr const Rotation2d& Rotation() const { return m_rotation; }
+
+  /**
+   * Multiplies the current pose by a scalar.
+   *
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Pose2d.
+   */
+  constexpr Pose2d operator*(double scalar) const {
+    return Pose2d{m_translation * scalar, m_rotation * scalar};
+  }
+
+  /**
+   * Divides the current pose by a scalar.
+   *
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Pose2d.
+   */
+  constexpr Pose2d operator/(double scalar) const {
+    return *this * (1.0 / scalar);
+  }
+
+  /**
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by.
+   *
+   * @return The rotated pose.
+   */
+  constexpr Pose2d RotateBy(const Rotation2d& other) const {
+    return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
+  }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new pose.
+   * See + operator for the matrix multiplication performed.
+   *
+   * @param other The transform to transform the pose by.
+   *
+   * @return The transformed pose.
+   */
+  constexpr Pose2d TransformBy(const Transform2d& other) const;
+
+  /**
+   * Returns the current pose relative to the given pose.
+   *
+   * This function can often be used for trajectory tracking or pose
+   * stabilization algorithms to get the error between the reference and the
+   * current pose.
+   *
+   * @param other The pose that is the origin of the new coordinate frame that
+   * the current pose will be converted into.
+   *
+   * @return The current pose relative to the new origin pose.
+   */
+  constexpr Pose2d RelativeTo(const Pose2d& other) const;
+
+  /**
+   * Rotates the current pose around a point in 2D space.
+   *
+   * @param point The point in 2D space to rotate around.
+   * @param rot The rotation to rotate the pose by.
+   *
+   * @return The new rotated pose.
+   */
+  constexpr Pose2d RotateAround(const Translation2d& point,
+                                const Rotation2d& rot) const {
+    return {m_translation.RotateAround(point, rot), m_rotation.RotateBy(rot)};
+  }
+
+  /**
+   * Returns an affine transformation matrix representation of this pose.
+   */
+  constexpr Eigen::Matrix3d ToMatrix() const {
+    auto vec = m_translation.ToVector();
+    auto mat = m_rotation.ToMatrix();
+    return Eigen::Matrix3d{{mat(0, 0), mat(0, 1), vec(0)},
+                           {mat(1, 0), mat(1, 1), vec(1)},
+                           {0.0, 0.0, 1.0}};
+  }
+
+  /**
+   * Returns the nearest Pose2d from a collection of poses.
+   *
+   * If two or more poses in the collection have the same distance from this
+   * pose, return the one with the closest rotation component.
+   *
+   * @param poses The collection of poses.
+   * @return The nearest Pose2d from the collection.
+   */
+  constexpr Pose2d Nearest(std::span<const Pose2d> poses) const {
+    return *std::min_element(
+        poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) {
+          auto aDistance = this->Translation().Distance(a.Translation());
+          auto bDistance = this->Translation().Distance(b.Translation());
+
+          // If the distances are equal sort by difference in rotation
+          if (aDistance == bDistance) {
+            return gcem::abs(
+                       (this->Rotation() - a.Rotation()).Radians().value()) <
+                   gcem::abs(
+                       (this->Rotation() - b.Rotation()).Radians().value());
+          }
+          return aDistance < bDistance;
+        });
+  }
+
+  /**
+   * Returns the nearest Pose2d from a collection of poses.
+   *
+   * If two or more poses in the collection have the same distance from this
+   * pose, return the one with the closest rotation component.
+   *
+   * @param poses The collection of poses.
+   * @return The nearest Pose2d from the collection.
+   */
+  constexpr Pose2d Nearest(std::initializer_list<Pose2d> poses) const {
+    return *std::min_element(
+        poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) {
+          auto aDistance = this->Translation().Distance(a.Translation());
+          auto bDistance = this->Translation().Distance(b.Translation());
+
+          // If the distances are equal sort by difference in rotation
+          if (aDistance == bDistance) {
+            return gcem::abs(
+                       (this->Rotation() - a.Rotation()).Radians().value()) <
+                   gcem::abs(
+                       (this->Rotation() - b.Rotation()).Radians().value());
+          }
+          return aDistance < bDistance;
+        });
+  }
+
+ private:
+  Translation2d m_translation;
+  Rotation2d m_rotation;
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Pose2d& pose);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Pose2d& pose);
+
+}  // namespace frc
+
+#include "frc/geometry/proto/Pose2dProto.h"
+#include "frc/geometry/struct/Pose2dStruct.h"
+
+#include "frc/geometry/Transform2d.h"
+
+namespace frc {
+
+constexpr Transform2d Pose2d::operator-(const Pose2d& other) const {
+  const auto pose = this->RelativeTo(other);
+  return Transform2d{pose.Translation(), pose.Rotation()};
+}
+
+constexpr Pose2d Pose2d::TransformBy(const frc::Transform2d& other) const {
+  return {m_translation + (other.Translation().RotateBy(m_rotation)),
+          other.Rotation() + m_rotation};
+}
+
+constexpr Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
+  const Transform2d transform{other, *this};
+  return {transform.Translation(), transform.Rotation()};
+}
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/geometry/Pose3d.hpp

@@ -0,0 +1,329 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <initializer_list>
+#include <span>
+#include <stdexcept>
+#include <type_traits>
+#include <utility>
+
+#include <Eigen/Core>
+#include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+
+#include "frc/ct_matrix.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation3d.h"
+#include "frc/geometry/Translation3d.h"
+
+namespace frc {
+
+class Transform3d;
+
+/**
+ * Represents a 3D pose containing translational and rotational elements.
+ */
+class WPILIB_DLLEXPORT Pose3d {
+ public:
+  /**
+   * Constructs a pose at the origin facing toward the positive X axis.
+   */
+  constexpr Pose3d() = default;
+
+  /**
+   * Constructs a pose with the specified translation and rotation.
+   *
+   * @param translation The translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  constexpr Pose3d(Translation3d translation, Rotation3d rotation)
+      : m_translation{std::move(translation)},
+        m_rotation{std::move(rotation)} {}
+
+  /**
+   * Constructs a pose with x, y, and z translations instead of a separate
+   * Translation3d.
+   *
+   * @param x The x component of the translational component of the pose.
+   * @param y The y component of the translational component of the pose.
+   * @param z The z component of the translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  constexpr Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
+                   Rotation3d rotation)
+      : m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
+
+  /**
+   * Constructs a pose with the specified affine transformation matrix.
+   *
+   * @param matrix The affine transformation matrix.
+   * @throws std::domain_error if the affine transformation matrix is invalid.
+   */
+  constexpr explicit Pose3d(const Eigen::Matrix4d& matrix)
+      : m_translation{Eigen::Vector3d{
+            {matrix(0, 3)}, {matrix(1, 3)}, {matrix(2, 3)}}},
+        m_rotation{
+            Eigen::Matrix3d{{matrix(0, 0), matrix(0, 1), matrix(0, 2)},
+                            {matrix(1, 0), matrix(1, 1), matrix(1, 2)},
+                            {matrix(2, 0), matrix(2, 1), matrix(2, 2)}}} {
+    if (matrix(3, 0) != 0.0 || matrix(3, 1) != 0.0 || matrix(3, 2) != 0.0 ||
+        matrix(3, 3) != 1.0) {
+      throw std::domain_error("Affine transformation matrix is invalid");
+    }
+  }
+
+  /**
+   * Constructs a 3D pose from a 2D pose in the X-Y plane.
+   *
+   * @param pose The 2D pose.
+   * @see Rotation3d(Rotation2d)
+   * @see Translation3d(Translation2d)
+   */
+  constexpr explicit Pose3d(const Pose2d& pose)
+      : m_translation{pose.X(), pose.Y(), 0_m},
+        m_rotation{0_rad, 0_rad, pose.Rotation().Radians()} {}
+
+  /**
+   * Transforms the pose by the given transformation and returns the new
+   * transformed pose. The transform is applied relative to the pose's frame.
+   * Note that this differs from Pose3d::RotateBy(const Rotation3d&), which is
+   * applied relative to the global frame and around the origin.
+   *
+   * @param other The transform to transform the pose by.
+   *
+   * @return The transformed pose.
+   */
+  constexpr Pose3d operator+(const Transform3d& other) const {
+    return TransformBy(other);
+  }
+
+  /**
+   * Returns the Transform3d that maps the one pose to another.
+   *
+   * @param other The initial pose of the transformation.
+   * @return The transform that maps the other pose to the current pose.
+   */
+  constexpr Transform3d operator-(const Pose3d& other) const;
+
+  /**
+   * Checks equality between this Pose3d and another object.
+   */
+  constexpr bool operator==(const Pose3d&) const = default;
+
+  /**
+   * Returns the underlying translation.
+   *
+   * @return Reference to the translational component of the pose.
+   */
+  constexpr const Translation3d& Translation() const { return m_translation; }
+
+  /**
+   * Returns the X component of the pose's translation.
+   *
+   * @return The x component of the pose's translation.
+   */
+  constexpr units::meter_t X() const { return m_translation.X(); }
+
+  /**
+   * Returns the Y component of the pose's translation.
+   *
+   * @return The y component of the pose's translation.
+   */
+  constexpr units::meter_t Y() const { return m_translation.Y(); }
+
+  /**
+   * Returns the Z component of the pose's translation.
+   *
+   * @return The z component of the pose's translation.
+   */
+  constexpr units::meter_t Z() const { return m_translation.Z(); }
+
+  /**
+   * Returns the underlying rotation.
+   *
+   * @return Reference to the rotational component of the pose.
+   */
+  constexpr const Rotation3d& Rotation() const { return m_rotation; }
+
+  /**
+   * Multiplies the current pose by a scalar.
+   *
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Pose2d.
+   */
+  constexpr Pose3d operator*(double scalar) const {
+    return Pose3d{m_translation * scalar, m_rotation * scalar};
+  }
+
+  /**
+   * Divides the current pose by a scalar.
+   *
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Pose2d.
+   */
+  constexpr Pose3d operator/(double scalar) const {
+    return *this * (1.0 / scalar);
+  }
+
+  /**
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by, which is applied
+   * extrinsically (from the global frame).
+   *
+   * @return The rotated pose.
+   */
+  constexpr Pose3d RotateBy(const Rotation3d& other) const {
+    return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
+  }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new
+   * transformed pose. The transform is applied relative to the pose's frame.
+   * Note that this differs from Pose3d::RotateBy(const Rotation3d&), which is
+   * applied relative to the global frame and around the origin.
+   *
+   * @param other The transform to transform the pose by.
+   *
+   * @return The transformed pose.
+   */
+  constexpr Pose3d TransformBy(const Transform3d& other) const;
+
+  /**
+   * Returns the current pose relative to the given pose.
+   *
+   * This function can often be used for trajectory tracking or pose
+   * stabilization algorithms to get the error between the reference and the
+   * current pose.
+   *
+   * @param other The pose that is the origin of the new coordinate frame that
+   * the current pose will be converted into.
+   *
+   * @return The current pose relative to the new origin pose.
+   */
+  constexpr Pose3d RelativeTo(const Pose3d& other) const;
+
+  /**
+   * Rotates the current pose around a point in 3D space.
+   *
+   * @param point The point in 3D space to rotate around.
+   * @param rot The rotation to rotate the pose by.
+   *
+   * @return The new rotated pose.
+   */
+  constexpr Pose3d RotateAround(const Translation3d& point,
+                                const Rotation3d& rot) const {
+    return {m_translation.RotateAround(point, rot), m_rotation.RotateBy(rot)};
+  }
+
+  /**
+   * Returns an affine transformation matrix representation of this pose.
+   */
+  constexpr Eigen::Matrix4d ToMatrix() const {
+    auto vec = m_translation.ToVector();
+    auto mat = m_rotation.ToMatrix();
+    return Eigen::Matrix4d{{mat(0, 0), mat(0, 1), mat(0, 2), vec(0)},
+                           {mat(1, 0), mat(1, 1), mat(1, 2), vec(1)},
+                           {mat(2, 0), mat(2, 1), mat(2, 2), vec(2)},
+                           {0.0, 0.0, 0.0, 1.0}};
+  }
+
+  /**
+   * Returns a Pose2d representing this Pose3d projected into the X-Y plane.
+   */
+  constexpr Pose2d ToPose2d() const {
+    return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
+  }
+
+  /**
+   * Returns the nearest Pose3d from a collection of poses.
+   *
+   * If two or more poses in the collection have the same distance from this
+   * pose, return the one with the closest rotation component.
+   *
+   * @param poses The collection of poses.
+   * @return The nearest Pose3d from the collection.
+   */
+  constexpr Pose3d Nearest(std::span<const Pose3d> poses) const {
+    return *std::min_element(
+        poses.begin(), poses.end(), [this](const Pose3d& a, const Pose3d& b) {
+          auto aDistance = this->Translation().Distance(a.Translation());
+          auto bDistance = this->Translation().Distance(b.Translation());
+
+          // If the distances are equal sort by difference in rotation
+          if (aDistance == bDistance) {
+            return gcem::abs(
+                       (this->Rotation() - a.Rotation()).Angle().value()) <
+                   gcem::abs((this->Rotation() - b.Rotation()).Angle().value());
+          }
+          return aDistance < bDistance;
+        });
+  }
+
+  /**
+   * Returns the nearest Pose3d from a collection of poses.
+   *
+   * If two or more poses in the collection have the same distance from this
+   * pose, return the one with the closest rotation component.
+   *
+   * @param poses The collection of poses.
+   * @return The nearest Pose3d from the collection.
+   */
+  constexpr Pose3d Nearest(std::initializer_list<Pose3d> poses) const {
+    return *std::min_element(
+        poses.begin(), poses.end(), [this](const Pose3d& a, const Pose3d& b) {
+          auto aDistance = this->Translation().Distance(a.Translation());
+          auto bDistance = this->Translation().Distance(b.Translation());
+
+          // If the distances are equal sort by difference in rotation
+          if (aDistance == bDistance) {
+            return gcem::abs(
+                       (this->Rotation() - a.Rotation()).Angle().value()) <
+                   gcem::abs((this->Rotation() - b.Rotation()).Angle().value());
+          }
+          return aDistance < bDistance;
+        });
+  }
+
+ private:
+  Translation3d m_translation;
+  Rotation3d m_rotation;
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Pose3d& pose);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Pose3d& pose);
+
+}  // namespace frc
+
+#include "frc/geometry/Transform3d.h"
+
+namespace frc {
+
+constexpr Transform3d Pose3d::operator-(const Pose3d& other) const {
+  const auto pose = this->RelativeTo(other);
+  return Transform3d{pose.Translation(), pose.Rotation()};
+}
+
+constexpr Pose3d Pose3d::TransformBy(const Transform3d& other) const {
+  return {m_translation + (other.Translation().RotateBy(m_rotation)),
+          other.Rotation() + m_rotation};
+}
+
+constexpr Pose3d Pose3d::RelativeTo(const Pose3d& other) const {
+  const Transform3d transform{other, *this};
+  return {transform.Translation(), transform.Rotation()};
+}
+
+}  // namespace frc
+
+#include "frc/geometry/proto/Pose3dProto.h"
+#include "frc/geometry/struct/Pose3dStruct.h"

wpimath/src/main/native/include/wpi/math/geometry/Quaternion.hpp

@@ -0,0 +1,338 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <numbers>
+
+#include <Eigen/Core>
+#include <gcem.hpp>
+#include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+
+namespace frc {
+
+/**
+ * Represents a quaternion.
+ */
+class WPILIB_DLLEXPORT Quaternion {
+ public:
+  /**
+   * Constructs a quaternion with a default angle of 0 degrees.
+   */
+  constexpr Quaternion() = default;
+
+  /**
+   * Constructs a quaternion with the given components.
+   *
+   * @param w W component of the quaternion.
+   * @param x X component of the quaternion.
+   * @param y Y component of the quaternion.
+   * @param z Z component of the quaternion.
+   */
+  constexpr Quaternion(double w, double x, double y, double z)
+      : m_w{w}, m_x{x}, m_y{y}, m_z{z} {}
+
+  /**
+   * Adds with another quaternion.
+   *
+   * @param other the other quaternion
+   */
+  constexpr Quaternion operator+(const Quaternion& other) const {
+    return Quaternion{m_w + other.m_w, m_x + other.m_x, m_y + other.m_y,
+                      m_z + other.m_z};
+  }
+
+  /**
+   * Subtracts another quaternion.
+   *
+   * @param other the other quaternion
+   */
+  constexpr Quaternion operator-(const Quaternion& other) const {
+    return Quaternion{m_w - other.m_w, m_x - other.m_x, m_y - other.m_y,
+                      m_z - other.m_z};
+  }
+
+  /**
+   * Multiples with a scalar value.
+   *
+   * @param other the scalar value
+   */
+  constexpr Quaternion operator*(double other) const {
+    return Quaternion{m_w * other, m_x * other, m_y * other, m_z * other};
+  }
+
+  /**
+   * Divides by a scalar value.
+   *
+   * @param other the scalar value
+   */
+  constexpr Quaternion operator/(double other) const {
+    return Quaternion{m_w / other, m_x / other, m_y / other, m_z / other};
+  }
+
+  /**
+   * Multiply with another quaternion.
+   *
+   * @param other The other quaternion.
+   */
+  constexpr Quaternion operator*(const Quaternion& other) const {
+    // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
+    const auto& r1 = m_w;
+    const auto& r2 = other.m_w;
+
+    // v₁ ⋅ v₂
+    double dot = m_x * other.m_x + m_y * other.m_y + m_z * other.m_z;
+
+    // v₁ x v₂
+    double cross_x = m_y * other.m_z - other.m_y * m_z;
+    double cross_y = other.m_x * m_z - m_x * other.m_z;
+    double cross_z = m_x * other.m_y - other.m_x * m_y;
+
+    return Quaternion{// r = r₁r₂ − v₁ ⋅ v₂
+                      r1 * r2 - dot,
+                      // v = r₁v₂ + r₂v₁ + v₁ x v₂
+                      r1 * other.m_x + r2 * m_x + cross_x,
+                      r1 * other.m_y + r2 * m_y + cross_y,
+                      r1 * other.m_z + r2 * m_z + cross_z};
+  }
+
+  /**
+   * Checks equality between this Quaternion and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  constexpr bool operator==(const Quaternion& other) const {
+    return gcem::abs(Dot(other) - Norm() * other.Norm()) < 1e-9 &&
+           gcem::abs(Norm() - other.Norm()) < 1e-9;
+  }
+
+  /**
+   * Returns the elementwise product of two quaternions.
+   */
+  constexpr double Dot(const Quaternion& other) const {
+    return W() * other.W() + X() * other.X() + Y() * other.Y() +
+           Z() * other.Z();
+  }
+
+  /**
+   * Returns the conjugate of the quaternion.
+   */
+  constexpr Quaternion Conjugate() const {
+    return Quaternion{W(), -X(), -Y(), -Z()};
+  }
+
+  /**
+   * Returns the inverse of the quaternion.
+   */
+  constexpr Quaternion Inverse() const {
+    double norm = Norm();
+    return Conjugate() / (norm * norm);
+  }
+
+  /**
+   * Normalizes the quaternion.
+   */
+  constexpr Quaternion Normalize() const {
+    double norm = Norm();
+    if (norm == 0.0) {
+      return Quaternion{};
+    } else {
+      return Quaternion{W(), X(), Y(), Z()} / norm;
+    }
+  }
+
+  /**
+   * Calculates the L2 norm of the quaternion.
+   */
+  constexpr double Norm() const { return gcem::sqrt(Dot(*this)); }
+
+  /**
+   * Calculates this quaternion raised to a power.
+   *
+   * @param t the power to raise this quaternion to.
+   */
+  constexpr Quaternion Pow(double t) const { return (Log() * t).Exp(); }
+
+  /**
+   * Matrix exponential of a quaternion.
+   *
+   * @param other the "Twist" that will be applied to this quaternion.
+   */
+  constexpr Quaternion Exp(const Quaternion& other) const {
+    return other.Exp() * *this;
+  }
+
+  /**
+   * Matrix exponential of a quaternion.
+   *
+   * source: wpimath/algorithms.md
+   *
+   *  If this quaternion is in 𝖘𝖔(3) and you are looking for an element of
+   * SO(3), use FromRotationVector
+   */
+  constexpr Quaternion Exp() const {
+    double scalar = gcem::exp(m_w);
+
+    double axial_magnitude = gcem::hypot(m_x, m_y, m_z);
+    double cosine = gcem::cos(axial_magnitude);
+
+    double axial_scalar;
+
+    if (axial_magnitude < 1e-9) {
+      // Taylor series of sin(x)/x near x=0: 1 − x²/6 + x⁴/120 + O(n⁶)
+      double axial_magnitude_sq = axial_magnitude * axial_magnitude;
+      double axial_magnitude_sq_sq = axial_magnitude_sq * axial_magnitude_sq;
+      axial_scalar =
+          1.0 - axial_magnitude_sq / 6.0 + axial_magnitude_sq_sq / 120.0;
+    } else {
+      axial_scalar = gcem::sin(axial_magnitude) / axial_magnitude;
+    }
+
+    return Quaternion(cosine * scalar, X() * axial_scalar * scalar,
+                      Y() * axial_scalar * scalar, Z() * axial_scalar * scalar);
+  }
+
+  /**
+   * Log operator of a quaternion.
+   *
+   * @param other The quaternion to map this quaternion onto
+   */
+  constexpr Quaternion Log(const Quaternion& other) const {
+    return (other * Inverse()).Log();
+  }
+
+  /**
+   * Log operator of a quaternion.
+   *
+   * source:  wpimath/algorithms.md
+   *
+   * If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3),
+   * use ToRotationVector
+   */
+  constexpr Quaternion Log() const {
+    double norm = Norm();
+    double scalar = gcem::log(norm);
+
+    double v_norm = gcem::hypot(m_x, m_y, m_z);
+
+    double s_norm = W() / norm;
+
+    if (gcem::abs(s_norm + 1) < 1e-9) {
+      return Quaternion{scalar, -std::numbers::pi, 0, 0};
+    }
+
+    double v_scalar;
+
+    if (v_norm < 1e-9) {
+      // Taylor series expansion of atan2(y/x)/y at y = 0:
+      //
+      //   1/x - 1/3 y²/x³ + O(y⁴)
+      v_scalar = 1.0 / W() - 1.0 / 3.0 * v_norm * v_norm / (W() * W() * W());
+    } else {
+      v_scalar = gcem::atan2(v_norm, W()) / v_norm;
+    }
+
+    return Quaternion{scalar, v_scalar * m_x, v_scalar * m_y, v_scalar * m_z};
+  }
+
+  /**
+   * Returns W component of the quaternion.
+   */
+  constexpr double W() const { return m_w; }
+
+  /**
+   * Returns X component of the quaternion.
+   */
+  constexpr double X() const { return m_x; }
+
+  /**
+   * Returns Y component of the quaternion.
+   */
+  constexpr double Y() const { return m_y; }
+
+  /**
+   * Returns Z component of the quaternion.
+   */
+  constexpr double Z() const { return m_z; }
+
+  /**
+   * Returns the rotation vector representation of this quaternion.
+   *
+   * This is also the log operator of SO(3).
+   */
+  constexpr Eigen::Vector3d ToRotationVector() const {
+    // See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
+    // Sound State Representation through Encapsulation of Manifolds"
+    //
+    // https://arxiv.org/pdf/1107.1119.pdf
+
+    double norm = gcem::hypot(m_x, m_y, m_z);
+
+    double scalar;
+    if (norm < 1e-9) {
+      scalar = (2.0 / W() - 2.0 / 3.0 * norm * norm / (W() * W() * W()));
+    } else {
+      if (W() < 0.0) {
+        scalar = 2.0 * gcem::atan2(-norm, -W()) / norm;
+      } else {
+        scalar = 2.0 * gcem::atan2(norm, W()) / norm;
+      }
+    }
+
+    return Eigen::Vector3d{{scalar * m_x, scalar * m_y, scalar * m_z}};
+  }
+
+  /**
+   * Returns the quaternion representation of this rotation vector.
+   *
+   * This is also the exp operator of 𝖘𝖔(3).
+   *
+   * source: wpimath/algorithms.md
+   */
+  constexpr static Quaternion FromRotationVector(const Eigen::Vector3d& rvec) {
+    // 𝑣⃗ = θ * v̂
+    // v̂ = 𝑣⃗ / θ
+
+    // 𝑞 = cos(θ/2) + sin(θ/2) * v̂
+    // 𝑞 = cos(θ/2) + sin(θ/2) / θ * 𝑣⃗
+
+    double theta = gcem::hypot(rvec(0), rvec(1), rvec(2));
+    double cos = gcem::cos(theta / 2);
+
+    double axial_scalar;
+
+    if (theta < 1e-9) {
+      // taylor series expansion of sin(θ/2) / θ around θ = 0 = 1/2 - θ²/48 +
+      // O(θ⁴)
+      axial_scalar = 1.0 / 2.0 - theta * theta / 48.0;
+    } else {
+      axial_scalar = gcem::sin(theta / 2) / theta;
+    }
+
+    return Quaternion{cos, axial_scalar * rvec(0), axial_scalar * rvec(1),
+                      axial_scalar * rvec(2)};
+  }
+
+ private:
+  // Scalar r in versor form
+  double m_w = 1.0;
+
+  // Vector v in versor form
+  double m_x = 0.0;
+  double m_y = 0.0;
+  double m_z = 0.0;
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Quaternion& quaternion);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Quaternion& quaternion);
+
+}  // namespace frc
+
+#include "frc/geometry/proto/QuaternionProto.h"
+#include "frc/geometry/struct/QuaternionStruct.h"

wpimath/src/main/native/include/wpi/math/geometry/Rectangle2d.hpp

@@ -0,0 +1,210 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <stdexcept>
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Transform2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+
+/**
+ * Represents a 2d rectangular space containing translational, rotational, and
+ * scaling components.
+ */
+class WPILIB_DLLEXPORT Rectangle2d {
+ public:
+  /**
+   * Constructs a rectangle at the specified position with the specified width
+   * and height.
+   *
+   * @param center The position (translation and rotation) of the rectangle.
+   * @param xWidth The x size component of the rectangle, in unrotated
+   * coordinate frame.
+   * @param yWidth The y size component of the rectangle, in unrotated
+   * coordinate frame.
+   */
+  constexpr Rectangle2d(const Pose2d& center, units::meter_t xWidth,
+                        units::meter_t yWidth)
+      : m_center{center}, m_xWidth{xWidth}, m_yWidth{yWidth} {
+    if (xWidth < 0_m || yWidth < 0_m) {
+      throw std::invalid_argument(
+          "Rectangle2d dimensions cannot be less than 0!");
+    }
+  }
+
+  /**
+   * Creates an unrotated rectangle from the given corners. The corners should
+   * be diagonally opposite of each other.
+   *
+   * @param cornerA The first corner of the rectangle.
+   * @param cornerB The second corner of the rectangle.
+   */
+  constexpr Rectangle2d(const Translation2d& cornerA,
+                        const Translation2d& cornerB)
+      : m_center{(cornerA + cornerB) / 2.0, Rotation2d{}},
+        m_xWidth{units::math::abs(cornerA.X() - cornerB.X())},
+        m_yWidth{units::math::abs(cornerA.Y() - cornerB.Y())} {}
+
+  /**
+   * Returns the center of the rectangle.
+   *
+   * @return The center of the rectangle.
+   */
+  constexpr const Pose2d& Center() const { return m_center; }
+
+  /**
+   * Returns the rotational component of the rectangle.
+   *
+   * @return The rotational component of the rectangle.
+   */
+  constexpr const Rotation2d& Rotation() const { return m_center.Rotation(); }
+
+  /**
+   * Returns the x size component of the rectangle.
+   *
+   * @return The x size component of the rectangle.
+   */
+  constexpr units::meter_t XWidth() const { return m_xWidth; }
+
+  /**
+   * Returns the y size component of the rectangle.
+   *
+   * @return The y size component of the rectangle.
+   */
+  constexpr units::meter_t YWidth() const { return m_yWidth; }
+
+  /**
+   * Transforms the center of the rectangle and returns the new rectangle.
+   *
+   * @param other The transform to transform by.
+   * @return The transformed rectangle
+   */
+  constexpr Rectangle2d TransformBy(const Transform2d& other) const {
+    return Rectangle2d{m_center.TransformBy(other), m_xWidth, m_yWidth};
+  }
+
+  /**
+   * Rotates the center of the rectangle and returns the new rectangle.
+   *
+   * @param other The rotation to transform by.
+   * @return The rotated rectangle.
+   */
+  constexpr Rectangle2d RotateBy(const Rotation2d& other) const {
+    return Rectangle2d{m_center.RotateBy(other), m_xWidth, m_yWidth};
+  }
+
+  /**
+   * Checks if a point is intersected by the rectangle's perimeter.
+   *
+   * @param point The point to check.
+   * @return True, if the rectangle's perimeter intersects the point.
+   */
+  constexpr bool Intersects(const Translation2d& point) const {
+    // Move the point into the rectangle's coordinate frame
+    auto pointInRect = point - m_center.Translation();
+    pointInRect = pointInRect.RotateBy(-m_center.Rotation());
+
+    if (units::math::abs(units::math::abs(pointInRect.X()) - m_xWidth / 2.0) <=
+        1E-9_m) {
+      // Point rests on left/right perimeter
+      return units::math::abs(pointInRect.Y()) <= m_yWidth / 2.0;
+    } else if (units::math::abs(units::math::abs(pointInRect.Y()) -
+                                m_yWidth / 2.0) <= 1E-9_m) {
+      // Point rests on top/bottom perimeter
+      return units::math::abs(pointInRect.X()) <= m_xWidth / 2.0;
+    }
+
+    return false;
+  }
+
+  /**
+   * Checks if a point is contained within the rectangle. This is inclusive, if
+   * the point lies on the perimeter it will return true.
+   *
+   * @param point The point to check.
+   * @return True, if the rectangle contains the point or the perimeter
+   * intersects the point.
+   */
+  constexpr bool Contains(const Translation2d& point) const {
+    // Rotate the point into the rectangle's coordinate frame
+    auto rotPoint =
+        point.RotateAround(m_center.Translation(), -m_center.Rotation());
+
+    // Check if within bounding box
+    return rotPoint.X() >= (m_center.X() - m_xWidth / 2.0) &&
+           rotPoint.X() <= (m_center.X() + m_xWidth / 2.0) &&
+           rotPoint.Y() >= (m_center.Y() - m_yWidth / 2.0) &&
+           rotPoint.Y() <= (m_center.Y() + m_yWidth / 2.0);
+  }
+
+  /**
+   * Returns the distance between the perimeter of the rectangle and the point.
+   *
+   * @param point The point to check.
+   * @return The distance (0, if the point is contained by the rectangle)
+   */
+  constexpr units::meter_t Distance(const Translation2d& point) const {
+    return Nearest(point).Distance(point);
+  }
+
+  /**
+   * Returns the nearest point that is contained within the rectangle.
+   *
+   * @param point The point that this will find the nearest point to.
+   * @return A new point that is nearest to {@code point} and contained in the
+   * rectangle.
+   */
+  constexpr Translation2d Nearest(const Translation2d& point) const {
+    // Check if already in rectangle
+    if (Contains(point)) {
+      return point;
+    }
+
+    // Rotate the point by the inverse of the rectangle's rotation
+    auto rotPoint =
+        point.RotateAround(m_center.Translation(), -m_center.Rotation());
+
+    // Find nearest point
+    rotPoint =
+        Translation2d{std::clamp(rotPoint.X(), m_center.X() - m_xWidth / 2.0,
+                                 m_center.X() + m_xWidth / 2.0),
+                      std::clamp(rotPoint.Y(), m_center.Y() - m_yWidth / 2.0,
+                                 m_center.Y() + m_yWidth / 2.0)};
+
+    // Undo rotation
+    return rotPoint.RotateAround(m_center.Translation(), m_center.Rotation());
+  }
+
+  /**
+   * Checks equality between this Rectangle2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  constexpr bool operator==(const Rectangle2d& other) const {
+    return m_center == other.m_center &&
+           units::math::abs(m_xWidth - other.m_xWidth) < 1E-9_m &&
+           units::math::abs(m_yWidth - other.m_yWidth) < 1E-9_m;
+  }
+
+ private:
+  Pose2d m_center;
+  units::meter_t m_xWidth;
+  units::meter_t m_yWidth;
+};
+
+}  // namespace frc
+
+#include "frc/geometry/proto/Rectangle2dProto.h"
+#include "frc/geometry/struct/Rectangle2dStruct.h"

wpimath/src/main/native/include/wpi/math/geometry/Rotation2d.hpp

@@ -0,0 +1,252 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <type_traits>
+#include <utility>
+
+#include <Eigen/Core>
+#include <gcem.hpp>
+#include <wpi/StackTrace.h>
+#include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+
+#include "frc/ct_matrix.h"
+#include "units/angle.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+/**
+ * A rotation in a 2D coordinate frame represented by a point on the unit circle
+ * (cosine and sine).
+ */
+class WPILIB_DLLEXPORT Rotation2d {
+ public:
+  /**
+   * Constructs a Rotation2d with a default angle of 0 degrees.
+   */
+  constexpr Rotation2d() = default;
+
+  /**
+   * Constructs a Rotation2d with the given angle.
+   *
+   * @param value The value of the angle.
+   */
+  constexpr Rotation2d(units::angle_unit auto value)  // NOLINT
+      : m_cos{gcem::cos(value.template convert<units::radian>().value())},
+        m_sin{gcem::sin(value.template convert<units::radian>().value())} {}
+
+  /**
+   * Constructs a Rotation2d with the given x and y (cosine and sine)
+   * components. The x and y don't have to be normalized.
+   *
+   * @param x The x component or cosine of the rotation.
+   * @param y The y component or sine of the rotation.
+   */
+  constexpr Rotation2d(double x, double y) {
+    double magnitude = gcem::hypot(x, y);
+    if (magnitude > 1e-6) {
+      m_cos = x / magnitude;
+      m_sin = y / magnitude;
+    } else {
+      m_cos = 1.0;
+      m_sin = 0.0;
+      if (!std::is_constant_evaluated()) {
+        wpi::math::MathSharedStore::ReportError(
+            "x and y components of Rotation2d are zero\n{}",
+            wpi::GetStackTrace(1));
+      }
+    }
+  }
+
+  /**
+   * Constructs a Rotation2d from a rotation matrix.
+   *
+   * @param rotationMatrix The rotation matrix.
+   * @throws std::domain_error if the rotation matrix isn't special orthogonal.
+   */
+  constexpr explicit Rotation2d(const Eigen::Matrix2d& rotationMatrix) {
+    auto impl =
+        []<typename Matrix2d>(const Matrix2d& R) -> std::pair<double, double> {
+      // Require that the rotation matrix is special orthogonal. This is true if
+      // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
+      if ((R * R.transpose() - Matrix2d::Identity()).norm() > 1e-9) {
+        throw std::domain_error("Rotation matrix isn't orthogonal");
+      }
+      // HACK: Uses ct_matrix instead of <Eigen/LU> for determinant because
+      //       including <Eigen/LU> doubles compilation times on MSVC, even if
+      //       this constructor is unused. MSVC's frontend inefficiently parses
+      //       large headers; GCC and Clang are largely unaffected.
+      if (gcem::abs(ct_matrix{R}.determinant() - 1.0) > 1e-9) {
+        throw std::domain_error(
+            "Rotation matrix is orthogonal but not special orthogonal");
+      }
+
+      // R = [cosθ  −sinθ]
+      //     [sinθ   cosθ]
+      return {R(0, 0), R(1, 0)};
+    };
+
+    if (std::is_constant_evaluated()) {
+      auto cossin = impl(ct_matrix2d{rotationMatrix});
+      m_cos = std::get<0>(cossin);
+      m_sin = std::get<1>(cossin);
+    } else {
+      auto cossin = impl(rotationMatrix);
+      m_cos = std::get<0>(cossin);
+      m_sin = std::get<1>(cossin);
+    }
+  }
+
+  /**
+   * Adds two rotations together, with the result being bounded between -π and
+   * π.
+   *
+   * For example, <code>Rotation2d{30_deg} + Rotation2d{60_deg}</code> equals
+   * <code>Rotation2d{units::radian_t{std::numbers::pi/2.0}}</code>
+   *
+   * @param other The rotation to add.
+   *
+   * @return The sum of the two rotations.
+   */
+  constexpr Rotation2d operator+(const Rotation2d& other) const {
+    return RotateBy(other);
+  }
+
+  /**
+   * Subtracts the new rotation from the current rotation and returns the new
+   * rotation.
+   *
+   * For example, <code>Rotation2d{10_deg} - Rotation2d{100_deg}</code> equals
+   * <code>Rotation2d{units::radian_t{-std::numbers::pi/2.0}}</code>
+   *
+   * @param other The rotation to subtract.
+   *
+   * @return The difference between the two rotations.
+   */
+  constexpr Rotation2d operator-(const Rotation2d& other) const {
+    return *this + -other;
+  }
+
+  /**
+   * Takes the inverse of the current rotation. This is simply the negative of
+   * the current angular value.
+   *
+   * @return The inverse of the current rotation.
+   */
+  constexpr Rotation2d operator-() const { return Rotation2d{m_cos, -m_sin}; }
+
+  /**
+   * Multiplies the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Rotation2d.
+   */
+  constexpr Rotation2d operator*(double scalar) const {
+    return Rotation2d{Radians() * scalar};
+  }
+
+  /**
+   * Divides the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Rotation2d.
+   */
+  constexpr Rotation2d operator/(double scalar) const {
+    return *this * (1.0 / scalar);
+  }
+
+  /**
+   * Checks equality between this Rotation2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  constexpr bool operator==(const Rotation2d& other) const {
+    return gcem::hypot(Cos() - other.Cos(), Sin() - other.Sin()) < 1E-9;
+  }
+
+  /**
+   * Adds the new rotation to the current rotation using a rotation matrix.
+   *
+   * <pre>
+   * [cos_new]   [other.cos, -other.sin][cos]
+   * [sin_new] = [other.sin,  other.cos][sin]
+   * value_new = std::atan2(sin_new, cos_new)
+   * </pre>
+   *
+   * @param other The rotation to rotate by.
+   *
+   * @return The new rotated Rotation2d.
+   */
+  constexpr Rotation2d RotateBy(const Rotation2d& other) const {
+    return {Cos() * other.Cos() - Sin() * other.Sin(),
+            Cos() * other.Sin() + Sin() * other.Cos()};
+  }
+
+  /**
+   * Returns matrix representation of this rotation.
+   */
+  constexpr Eigen::Matrix2d ToMatrix() const {
+    // R = [cosθ  −sinθ]
+    //     [sinθ   cosθ]
+    return Eigen::Matrix2d{{m_cos, -m_sin}, {m_sin, m_cos}};
+  }
+
+  /**
+   * Returns the radian value of the rotation constrained within [-π, π].
+   *
+   * @return The radian value of the rotation constrained within [-π, π].
+   */
+  constexpr units::radian_t Radians() const {
+    return units::radian_t{gcem::atan2(m_sin, m_cos)};
+  }
+
+  /**
+   * Returns the degree value of the rotation constrained within [-180, 180].
+   *
+   * @return The degree value of the rotation constrained within [-180, 180].
+   */
+  constexpr units::degree_t Degrees() const { return Radians(); }
+
+  /**
+   * Returns the cosine of the rotation.
+   *
+   * @return The cosine of the rotation.
+   */
+  constexpr double Cos() const { return m_cos; }
+
+  /**
+   * Returns the sine of the rotation.
+   *
+   * @return The sine of the rotation.
+   */
+  constexpr double Sin() const { return m_sin; }
+
+  /**
+   * Returns the tangent of the rotation.
+   *
+   * @return The tangent of the rotation.
+   */
+  constexpr double Tan() const { return Sin() / Cos(); }
+
+ private:
+  double m_cos = 1;
+  double m_sin = 0;
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Rotation2d& rotation);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Rotation2d& rotation);
+
+}  // namespace frc
+
+#include "frc/geometry/proto/Rotation2dProto.h"
+#include "frc/geometry/struct/Rotation2dStruct.h"

wpimath/src/main/native/include/wpi/math/geometry/Rotation3d.hpp

@@ -0,0 +1,453 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <type_traits>
+
+#include <Eigen/Core>
+#include <fmt/format.h>
+#include <gcem.hpp>
+#include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+
+#include "frc/ct_matrix.h"
+#include "frc/fmt/Eigen.h"
+#include "frc/geometry/Quaternion.h"
+#include "frc/geometry/Rotation2d.h"
+#include "units/angle.h"
+#include "units/math.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+/**
+ * A rotation in a 3D coordinate frame represented by a quaternion.
+ */
+class WPILIB_DLLEXPORT Rotation3d {
+ public:
+  /**
+   * Constructs a Rotation3d representing no rotation.
+   */
+  constexpr Rotation3d() = default;
+
+  /**
+   * Constructs a Rotation3d from a quaternion.
+   *
+   * @param q The quaternion.
+   */
+  constexpr explicit Rotation3d(const Quaternion& q) { m_q = q.Normalize(); }
+
+  /**
+   * Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
+   *
+   * Extrinsic rotations occur in that order around the axes in the fixed global
+   * frame rather than the body frame.
+   *
+   * Angles are measured counterclockwise with the rotation axis pointing "out
+   * of the page". If you point your right thumb along the positive axis
+   * direction, your fingers curl in the direction of positive rotation.
+   *
+   * @param roll The counterclockwise rotation angle around the X axis (roll).
+   * @param pitch The counterclockwise rotation angle around the Y axis (pitch).
+   * @param yaw The counterclockwise rotation angle around the Z axis (yaw).
+   */
+  constexpr Rotation3d(units::radian_t roll, units::radian_t pitch,
+                       units::radian_t yaw) {
+    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
+    double cr = units::math::cos(roll * 0.5);
+    double sr = units::math::sin(roll * 0.5);
+
+    double cp = units::math::cos(pitch * 0.5);
+    double sp = units::math::sin(pitch * 0.5);
+
+    double cy = units::math::cos(yaw * 0.5);
+    double sy = units::math::sin(yaw * 0.5);
+
+    m_q = Quaternion{cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy,
+                     cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy};
+  }
+
+  /**
+   * Constructs a Rotation3d with the given axis-angle representation. The axis
+   * doesn't have to be normalized.
+   *
+   * @param axis The rotation axis.
+   * @param angle The rotation around the axis.
+   */
+  constexpr Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle) {
+    double norm = ct_matrix{axis}.norm();
+    if (norm == 0.0) {
+      return;
+    }
+
+    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
+    Eigen::Vector3d v{{axis(0) / norm * units::math::sin(angle / 2.0),
+                       axis(1) / norm * units::math::sin(angle / 2.0),
+                       axis(2) / norm * units::math::sin(angle / 2.0)}};
+    m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
+  }
+
+  /**
+   * Constructs a Rotation3d with the given rotation vector representation. This
+   * representation is equivalent to axis-angle, where the normalized axis is
+   * multiplied by the rotation around the axis in radians.
+   *
+   * @param rvec The rotation vector.
+   */
+  constexpr explicit Rotation3d(const Eigen::Vector3d& rvec)
+      : Rotation3d{rvec, units::radian_t{ct_matrix{rvec}.norm()}} {}
+
+  /**
+   * Constructs a Rotation3d from a rotation matrix.
+   *
+   * @param rotationMatrix The rotation matrix.
+   * @throws std::domain_error if the rotation matrix isn't special orthogonal.
+   */
+  constexpr explicit Rotation3d(const Eigen::Matrix3d& rotationMatrix) {
+    auto impl = []<typename Matrix3d>(const Matrix3d& R) -> Quaternion {
+      // Require that the rotation matrix is special orthogonal. This is true if
+      // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
+      if ((R * R.transpose() - Matrix3d::Identity()).norm() > 1e-9) {
+        throw std::domain_error("Rotation matrix isn't orthogonal");
+      }
+      // HACK: Uses ct_matrix instead of <Eigen/LU> for determinant because
+      //       including <Eigen/LU> doubles compilation times on MSVC, even if
+      //       this constructor is unused. MSVC's frontend inefficiently parses
+      //       large headers; GCC and Clang are largely unaffected.
+      if (gcem::abs(ct_matrix{R}.determinant() - 1.0) > 1e-9) {
+        throw std::domain_error(
+            "Rotation matrix is orthogonal but not special orthogonal");
+      }
+
+      // Turn rotation matrix into a quaternion
+      // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+      double trace = R(0, 0) + R(1, 1) + R(2, 2);
+      double w;
+      double x;
+      double y;
+      double z;
+
+      if (trace > 0.0) {
+        double s = 0.5 / gcem::sqrt(trace + 1.0);
+        w = 0.25 / s;
+        x = (R(2, 1) - R(1, 2)) * s;
+        y = (R(0, 2) - R(2, 0)) * s;
+        z = (R(1, 0) - R(0, 1)) * s;
+      } else {
+        if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) {
+          double s = 2.0 * gcem::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2));
+          w = (R(2, 1) - R(1, 2)) / s;
+          x = 0.25 * s;
+          y = (R(0, 1) + R(1, 0)) / s;
+          z = (R(0, 2) + R(2, 0)) / s;
+        } else if (R(1, 1) > R(2, 2)) {
+          double s = 2.0 * gcem::sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2));
+          w = (R(0, 2) - R(2, 0)) / s;
+          x = (R(0, 1) + R(1, 0)) / s;
+          y = 0.25 * s;
+          z = (R(1, 2) + R(2, 1)) / s;
+        } else {
+          double s = 2.0 * gcem::sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1));
+          w = (R(1, 0) - R(0, 1)) / s;
+          x = (R(0, 2) + R(2, 0)) / s;
+          y = (R(1, 2) + R(2, 1)) / s;
+          z = 0.25 * s;
+        }
+      }
+
+      return Quaternion{w, x, y, z};
+    };
+
+    if (std::is_constant_evaluated()) {
+      m_q = impl(ct_matrix3d{rotationMatrix});
+    } else {
+      m_q = impl(rotationMatrix);
+    }
+  }
+
+  /**
+   * Constructs a Rotation3d that rotates the initial vector onto the final
+   * vector.
+   *
+   * This is useful for turning a 3D vector (final) into an orientation relative
+   * to a coordinate system vector (initial).
+   *
+   * @param initial The initial vector.
+   * @param final The final vector.
+   */
+  constexpr Rotation3d(const Eigen::Vector3d& initial,
+                       const Eigen::Vector3d& final) {
+    double dot = ct_matrix{initial}.dot(ct_matrix{final});
+    double normProduct = ct_matrix{initial}.norm() * ct_matrix{final}.norm();
+    double dotNorm = dot / normProduct;
+
+    if (dotNorm > 1.0 - 1E-9) {
+      // If the dot product is 1, the two vectors point in the same direction so
+      // there's no rotation. The default initialization of m_q will work.
+      return;
+    } else if (dotNorm < -1.0 + 1E-9) {
+      // If the dot product is -1, the two vectors are antiparallel, so a 180°
+      // rotation is required. Any other vector can be used to generate an
+      // orthogonal one.
+
+      double x = gcem::abs(initial(0));
+      double y = gcem::abs(initial(1));
+      double z = gcem::abs(initial(2));
+
+      // Find vector that is most orthogonal to initial vector
+      Eigen::Vector3d other;
+      if (x < y) {
+        if (x < z) {
+          // Use x-axis
+          other = Eigen::Vector3d{{1, 0, 0}};
+        } else {
+          // Use z-axis
+          other = Eigen::Vector3d{{0, 0, 1}};
+        }
+      } else {
+        if (y < z) {
+          // Use y-axis
+          other = Eigen::Vector3d{{0, 1, 0}};
+        } else {
+          // Use z-axis
+          other = Eigen::Vector3d{{0, 0, 1}};
+        }
+      }
+
+      auto axis = ct_matrix{initial}.cross(ct_matrix{other});
+
+      double axisNorm = axis.norm();
+      m_q = Quaternion{0.0, axis(0) / axisNorm, axis(1) / axisNorm,
+                       axis(2) / axisNorm};
+    } else {
+      auto axis = ct_matrix{initial}.cross(final);
+
+      // https://stackoverflow.com/a/11741520
+      m_q =
+          Quaternion{normProduct + dot, axis(0), axis(1), axis(2)}.Normalize();
+    }
+  }
+
+  /**
+   * Constructs a 3D rotation from a 2D rotation in the X-Y plane.
+   *
+   * @param rotation The 2D rotation.
+   * @see Pose3d(Pose2d)
+   * @see Transform3d(Transform2d)
+   */
+  constexpr explicit Rotation3d(const Rotation2d& rotation)
+      : Rotation3d{0_rad, 0_rad, rotation.Radians()} {}
+
+  /**
+   * Adds two rotations together.
+   *
+   * @param other The rotation to add.
+   *
+   * @return The sum of the two rotations.
+   */
+  constexpr Rotation3d operator+(const Rotation3d& other) const {
+    return RotateBy(other);
+  }
+
+  /**
+   * Subtracts the new rotation from the current rotation and returns the new
+   * rotation.
+   *
+   * @param other The rotation to subtract.
+   *
+   * @return The difference between the two rotations.
+   */
+  constexpr Rotation3d operator-(const Rotation3d& other) const {
+    return *this + -other;
+  }
+
+  /**
+   * Takes the inverse of the current rotation.
+   *
+   * @return The inverse of the current rotation.
+   */
+  constexpr Rotation3d operator-() const { return Rotation3d{m_q.Inverse()}; }
+
+  /**
+   * Multiplies the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Rotation3d.
+   */
+  constexpr Rotation3d operator*(double scalar) const {
+    // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
+    if (m_q.W() >= 0.0) {
+      return Rotation3d{Eigen::Vector3d{{m_q.X(), m_q.Y(), m_q.Z()}},
+                        2.0 * units::radian_t{scalar * gcem::acos(m_q.W())}};
+    } else {
+      return Rotation3d{Eigen::Vector3d{{-m_q.X(), -m_q.Y(), -m_q.Z()}},
+                        2.0 * units::radian_t{scalar * gcem::acos(-m_q.W())}};
+    }
+  }
+
+  /**
+   * Divides the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Rotation3d.
+   */
+  constexpr Rotation3d operator/(double scalar) const {
+    return *this * (1.0 / scalar);
+  }
+
+  /**
+   * Checks equality between this Rotation3d and another object.
+   */
+  constexpr bool operator==(const Rotation3d& other) const {
+    return gcem::abs(gcem::abs(m_q.Dot(other.m_q)) -
+                     m_q.Norm() * other.m_q.Norm()) < 1e-9;
+  }
+
+  /**
+   * Adds the new rotation to the current rotation. The other rotation is
+   * applied extrinsically, which means that it rotates around the global axes.
+   * For example, Rotation3d{90_deg, 0, 0}.RotateBy(Rotation3d{0, 45_deg, 0})
+   * rotates by 90 degrees around the +X axis and then by 45 degrees around the
+   * global +Y axis. (This is equivalent to Rotation3d{90_deg, 45_deg, 0})
+   *
+   * @param other The extrinsic rotation to rotate by.
+   *
+   * @return The new rotated Rotation3d.
+   */
+  constexpr Rotation3d RotateBy(const Rotation3d& other) const {
+    return Rotation3d{other.m_q * m_q};
+  }
+
+  /**
+   * Returns the quaternion representation of the Rotation3d.
+   */
+  constexpr const Quaternion& GetQuaternion() const { return m_q; }
+
+  /**
+   * Returns the counterclockwise rotation angle around the X axis (roll).
+   */
+  constexpr units::radian_t X() const {
+    double w = m_q.W();
+    double x = m_q.X();
+    double y = m_q.Y();
+    double z = m_q.Z();
+
+    // wpimath/algorithms.md
+    double cxcy = 1.0 - 2.0 * (x * x + y * y);
+    double sxcy = 2.0 * (w * x + y * z);
+    double cy_sq = cxcy * cxcy + sxcy * sxcy;
+    if (cy_sq > 1e-20) {
+      return units::radian_t{gcem::atan2(sxcy, cxcy)};
+    } else {
+      return 0_rad;
+    }
+  }
+
+  /**
+   * Returns the counterclockwise rotation angle around the Y axis (pitch).
+   */
+  constexpr units::radian_t Y() const {
+    double w = m_q.W();
+    double x = m_q.X();
+    double y = m_q.Y();
+    double z = m_q.Z();
+
+    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
+    double ratio = 2.0 * (w * y - z * x);
+    if (gcem::abs(ratio) >= 1.0) {
+      return units::radian_t{gcem::copysign(std::numbers::pi / 2.0, ratio)};
+    } else {
+      return units::radian_t{gcem::asin(ratio)};
+    }
+  }
+
+  /**
+   * Returns the counterclockwise rotation angle around the Z axis (yaw).
+   */
+  constexpr units::radian_t Z() const {
+    double w = m_q.W();
+    double x = m_q.X();
+    double y = m_q.Y();
+    double z = m_q.Z();
+
+    // wpimath/algorithms.md
+    double cycz = 1.0 - 2.0 * (y * y + z * z);
+    double cysz = 2.0 * (w * z + x * y);
+    double cy_sq = cycz * cycz + cysz * cysz;
+    if (cy_sq > 1e-20) {
+      return units::radian_t{gcem::atan2(cysz, cycz)};
+    } else {
+      return units::radian_t{gcem::atan2(2.0 * w * z, w * w - z * z)};
+    }
+  }
+
+  /**
+   * Returns the axis in the axis-angle representation of this rotation.
+   */
+  constexpr Eigen::Vector3d Axis() const {
+    double norm = gcem::hypot(m_q.X(), m_q.Y(), m_q.Z());
+    if (norm == 0.0) {
+      return Eigen::Vector3d{{0.0, 0.0, 0.0}};
+    } else {
+      return Eigen::Vector3d{{m_q.X() / norm, m_q.Y() / norm, m_q.Z() / norm}};
+    }
+  }
+
+  /**
+   * Returns the angle in the axis-angle representation of this rotation.
+   */
+  constexpr units::radian_t Angle() const {
+    double norm = gcem::hypot(m_q.X(), m_q.Y(), m_q.Z());
+    return units::radian_t{2.0 * gcem::atan2(norm, m_q.W())};
+  }
+
+  /**
+   * Returns rotation matrix representation of this rotation.
+   */
+  constexpr Eigen::Matrix3d ToMatrix() const {
+    double w = m_q.W();
+    double x = m_q.X();
+    double y = m_q.Y();
+    double z = m_q.Z();
+
+    // https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix
+    return Eigen::Matrix3d{{1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - w * z),
+                            2.0 * (x * z + w * y)},
+                           {2.0 * (x * y + w * z), 1.0 - 2.0 * (x * x + z * z),
+                            2.0 * (y * z - w * x)},
+                           {2.0 * (x * z - w * y), 2.0 * (y * z + w * x),
+                            1.0 - 2.0 * (x * x + y * y)}};
+  }
+
+  /**
+   * Returns rotation vector representation of this rotation.
+   *
+   * @return Rotation vector representation of this rotation.
+   */
+  constexpr Eigen::Vector3d ToVector() const { return m_q.ToRotationVector(); }
+
+  /**
+   * Returns a Rotation2d representing this Rotation3d projected into the X-Y
+   * plane.
+   */
+  constexpr Rotation2d ToRotation2d() const { return Rotation2d{Z()}; }
+
+ private:
+  Quaternion m_q;
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Rotation3d& rotation);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Rotation3d& rotation);
+
+}  // namespace frc
+
+#include "frc/geometry/proto/Rotation3dProto.h"
+#include "frc/geometry/struct/Rotation3dStruct.h"

wpimath/src/main/native/include/wpi/math/geometry/Transform2d.hpp

@@ -0,0 +1,217 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <utility>
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Translation2d.h"
+
+namespace frc {
+
+class Pose2d;
+struct Twist2d;
+
+/**
+ * Represents a transformation for a Pose2d in the pose's frame.
+ */
+class WPILIB_DLLEXPORT Transform2d {
+ public:
+  /**
+   * Constructs the transform that maps the initial pose to the final pose.
+   *
+   * @param initial The initial pose for the transformation.
+   * @param final The final pose for the transformation.
+   */
+  constexpr Transform2d(const Pose2d& initial, const Pose2d& final);
+
+  /**
+   * Constructs a transform with the given translation and rotation components.
+   *
+   * @param translation Translational component of the transform.
+   * @param rotation Rotational component of the transform.
+   */
+  constexpr Transform2d(Translation2d translation, Rotation2d rotation)
+      : m_translation{std::move(translation)},
+        m_rotation{std::move(rotation)} {}
+
+  /**
+   * Constructs a transform with x and y translations instead of a separate
+   * Translation2d.
+   *
+   * @param x The x component of the translational component of the transform.
+   * @param y The y component of the translational component of the transform.
+   * @param rotation The rotational component of the transform.
+   */
+  constexpr Transform2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
+      : m_translation{x, y}, m_rotation{std::move(rotation)} {}
+
+  /**
+   * Constructs a pose with the specified affine transformation matrix.
+   *
+   * @param matrix The affine transformation matrix.
+   * @throws std::domain_error if the affine transformation matrix is invalid.
+   */
+  constexpr explicit Transform2d(const Eigen::Matrix3d& matrix)
+      : m_translation{Eigen::Vector2d{{matrix(0, 2)}, {matrix(1, 2)}}},
+        m_rotation{Eigen::Matrix2d{{matrix(0, 0), matrix(0, 1)},
+                                   {matrix(1, 0), matrix(1, 1)}}} {
+    if (matrix(2, 0) != 0.0 || matrix(2, 1) != 0.0 || matrix(2, 2) != 1.0) {
+      throw std::domain_error("Affine transformation matrix is invalid");
+    }
+  }
+
+  /**
+   * Constructs the identity transform -- maps an initial pose to itself.
+   */
+  constexpr Transform2d() = default;
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return Reference to the translational component of the transform.
+   */
+  constexpr const Translation2d& Translation() const { return m_translation; }
+
+  /**
+   * Returns the X component of the transformation's translation.
+   *
+   * @return The x component of the transformation's translation.
+   */
+  constexpr units::meter_t X() const { return m_translation.X(); }
+
+  /**
+   * Returns the Y component of the transformation's translation.
+   *
+   * @return The y component of the transformation's translation.
+   */
+  constexpr units::meter_t Y() const { return m_translation.Y(); }
+
+  /**
+   * Returns an affine transformation matrix representation of this
+   * transformation.
+   */
+  constexpr Eigen::Matrix3d ToMatrix() const {
+    auto vec = m_translation.ToVector();
+    auto mat = m_rotation.ToMatrix();
+    return Eigen::Matrix3d{{mat(0, 0), mat(0, 1), vec(0)},
+                           {mat(1, 0), mat(1, 1), vec(1)},
+                           {0.0, 0.0, 1.0}};
+  }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return Reference to the rotational component of the transform.
+   */
+  constexpr const Rotation2d& Rotation() const { return m_rotation; }
+
+  /**
+   * Returns a Twist2d of the current transform (pose delta). If b is the output
+   * of {@code a.Log()}, then {@code b.Exp()} would yield a.
+   *
+   * @return The twist that maps the current transform.
+   */
+  constexpr Twist2d Log() const;
+
+  /**
+   * Invert the transformation. This is useful for undoing a transformation.
+   *
+   * @return The inverted transformation.
+   */
+  constexpr Transform2d Inverse() const {
+    // We are rotating the difference between the translations
+    // using a clockwise rotation matrix. This transforms the global
+    // delta into a local delta (relative to the initial pose).
+    return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
+  }
+
+  /**
+   * Multiplies the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform2d.
+   */
+  constexpr Transform2d operator*(double scalar) const {
+    return Transform2d(m_translation * scalar, m_rotation * scalar);
+  }
+
+  /**
+   * Divides the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform2d.
+   */
+  constexpr Transform2d operator/(double scalar) const {
+    return *this * (1.0 / scalar);
+  }
+
+  /**
+   * Composes two transformations. The second transform is applied relative to
+   * the orientation of the first.
+   *
+   * @param other The transform to compose with this one.
+   * @return The composition of the two transformations.
+   */
+  constexpr Transform2d operator+(const Transform2d& other) const;
+
+  /**
+   * Checks equality between this Transform2d and another object.
+   */
+  constexpr bool operator==(const Transform2d&) const = default;
+
+ private:
+  Translation2d m_translation;
+  Rotation2d m_rotation;
+};
+
+}  // namespace frc
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Twist2d.h"
+
+namespace frc {
+
+constexpr Transform2d::Transform2d(const Pose2d& initial, const Pose2d& final) {
+  // We are rotating the difference between the translations
+  // using a clockwise rotation matrix. This transforms the global
+  // delta into a local delta (relative to the initial pose).
+  m_translation = (final.Translation() - initial.Translation())
+                      .RotateBy(-initial.Rotation());
+
+  m_rotation = final.Rotation() - initial.Rotation();
+}
+
+constexpr Transform2d Transform2d::operator+(const Transform2d& other) const {
+  return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
+}
+
+constexpr Twist2d Transform2d::Log() const {
+  const auto dtheta = m_rotation.Radians().value();
+  const auto halfDtheta = dtheta / 2.0;
+
+  const auto cosMinusOne = m_rotation.Cos() - 1;
+
+  double halfThetaByTanOfHalfDtheta;
+
+  if (gcem::abs(cosMinusOne) < 1E-9) {
+    halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
+  } else {
+    halfThetaByTanOfHalfDtheta = -(halfDtheta * m_rotation.Sin()) / cosMinusOne;
+  }
+
+  const Translation2d translationPart =
+      m_translation.RotateBy({halfThetaByTanOfHalfDtheta, -halfDtheta}) *
+      gcem::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
+
+  return {translationPart.X(), translationPart.Y(), units::radian_t{dtheta}};
+}
+
+}  // namespace frc
+
+#include "frc/geometry/proto/Transform2dProto.h"
+#include "frc/geometry/struct/Transform2dStruct.h"

wpimath/src/main/native/include/wpi/math/geometry/Transform3d.hpp

@@ -0,0 +1,275 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <utility>
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Transform2d.h"
+#include "frc/geometry/Translation3d.h"
+
+namespace frc {
+
+class Pose3d;
+struct Twist3d;
+
+/**
+ * Represents a transformation for a Pose3d in the pose's frame.
+ */
+class WPILIB_DLLEXPORT Transform3d {
+ public:
+  /**
+   * Constructs the transform that maps the initial pose to the final pose.
+   *
+   * @param initial The initial pose for the transformation.
+   * @param final The final pose for the transformation.
+   */
+  constexpr Transform3d(const Pose3d& initial, const Pose3d& final);
+
+  /**
+   * Constructs a transform with the given translation and rotation components.
+   *
+   * @param translation Translational component of the transform.
+   * @param rotation Rotational component of the transform.
+   */
+  constexpr Transform3d(Translation3d translation, Rotation3d rotation)
+      : m_translation{std::move(translation)},
+        m_rotation{std::move(rotation)} {}
+
+  /**
+   * Constructs a transform with x, y, and z translations instead of a separate
+   * Translation3d.
+   *
+   * @param x The x component of the translational component of the transform.
+   * @param y The y component of the translational component of the transform.
+   * @param z The z component of the translational component of the transform.
+   * @param rotation The rotational component of the transform.
+   */
+  constexpr Transform3d(units::meter_t x, units::meter_t y, units::meter_t z,
+                        Rotation3d rotation)
+      : m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
+
+  /**
+   * Constructs a transform with the specified affine transformation matrix.
+   *
+   * @param matrix The affine transformation matrix.
+   * @throws std::domain_error if the affine transformation matrix is invalid.
+   */
+  constexpr explicit Transform3d(const Eigen::Matrix4d& matrix)
+      : m_translation{Eigen::Vector3d{
+            {matrix(0, 3)}, {matrix(1, 3)}, {matrix(2, 3)}}},
+        m_rotation{
+            Eigen::Matrix3d{{matrix(0, 0), matrix(0, 1), matrix(0, 2)},
+                            {matrix(1, 0), matrix(1, 1), matrix(1, 2)},
+                            {matrix(2, 0), matrix(2, 1), matrix(2, 2)}}} {
+    if (matrix(3, 0) != 0.0 || matrix(3, 1) != 0.0 || matrix(3, 2) != 0.0 ||
+        matrix(3, 3) != 1.0) {
+      throw std::domain_error("Affine transformation matrix is invalid");
+    }
+  }
+
+  /**
+   * Constructs the identity transform -- maps an initial pose to itself.
+   */
+  constexpr Transform3d() = default;
+
+  /**
+   * Constructs a 3D transform from a 2D transform in the X-Y plane.
+   **
+   * @param transform The 2D transform.
+   * @see Rotation3d(Rotation2d)
+   * @see Translation3d(Translation2d)
+   */
+  constexpr explicit Transform3d(const frc::Transform2d& transform)
+      : m_translation{frc::Translation3d{transform.Translation()}},
+        m_rotation{frc::Rotation3d{transform.Rotation()}} {}
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return Reference to the translational component of the transform.
+   */
+  constexpr const Translation3d& Translation() const { return m_translation; }
+
+  /**
+   * Returns the X component of the transformation's translation.
+   *
+   * @return The x component of the transformation's translation.
+   */
+  constexpr units::meter_t X() const { return m_translation.X(); }
+
+  /**
+   * Returns the Y component of the transformation's translation.
+   *
+   * @return The y component of the transformation's translation.
+   */
+  constexpr units::meter_t Y() const { return m_translation.Y(); }
+
+  /**
+   * Returns the Z component of the transformation's translation.
+   *
+   * @return The z component of the transformation's translation.
+   */
+  constexpr units::meter_t Z() const { return m_translation.Z(); }
+
+  /**
+   * Returns an affine transformation matrix representation of this
+   * transformation.
+   */
+  constexpr Eigen::Matrix4d ToMatrix() const {
+    auto vec = m_translation.ToVector();
+    auto mat = m_rotation.ToMatrix();
+    return Eigen::Matrix4d{{mat(0, 0), mat(0, 1), mat(0, 2), vec(0)},
+                           {mat(1, 0), mat(1, 1), mat(1, 2), vec(1)},
+                           {mat(2, 0), mat(2, 1), mat(2, 2), vec(2)},
+                           {0.0, 0.0, 0.0, 1.0}};
+  }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return Reference to the rotational component of the transform.
+   */
+  constexpr const Rotation3d& Rotation() const { return m_rotation; }
+
+  /**
+   * Returns a Twist3d of the current transform (pose delta). If b is the output
+   * of {@code a.Log()}, then {@code b.Exp()} would yield a.
+   *
+   * @return The twist that maps the current transform.
+   */
+  constexpr Twist3d Log() const;
+
+  /**
+   * Invert the transformation. This is useful for undoing a transformation.
+   *
+   * @return The inverted transformation.
+   */
+  constexpr Transform3d Inverse() const {
+    // We are rotating the difference between the translations
+    // using a clockwise rotation matrix. This transforms the global
+    // delta into a local delta (relative to the initial pose).
+    return Transform3d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
+  }
+
+  /**
+   * Multiplies the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform3d.
+   */
+  constexpr Transform3d operator*(double scalar) const {
+    return Transform3d(m_translation * scalar, m_rotation * scalar);
+  }
+
+  /**
+   * Divides the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform3d.
+   */
+  constexpr Transform3d operator/(double scalar) const {
+    return *this * (1.0 / scalar);
+  }
+
+  /**
+   * Composes two transformations. The second transform is applied relative to
+   * the orientation of the first.
+   *
+   * @param other The transform to compose with this one.
+   * @return The composition of the two transformations.
+   */
+  constexpr Transform3d operator+(const Transform3d& other) const;
+
+  /**
+   * Checks equality between this Transform3d and another object.
+   */
+  constexpr bool operator==(const Transform3d&) const = default;
+
+ private:
+  Translation3d m_translation;
+  Rotation3d m_rotation;
+};
+
+}  // namespace frc
+
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Twist3d.h"
+#include "frc/geometry/detail/RotationVectorToMatrix.h"
+
+namespace frc {
+
+constexpr Transform3d::Transform3d(const Pose3d& initial, const Pose3d& final) {
+  // We are rotating the difference between the translations
+  // using a clockwise rotation matrix. This transforms the global
+  // delta into a local delta (relative to the initial pose).
+  m_translation = (final.Translation() - initial.Translation())
+                      .RotateBy(-initial.Rotation());
+
+  m_rotation = final.Rotation() - initial.Rotation();
+}
+
+constexpr Transform3d Transform3d::operator+(const Transform3d& other) const {
+  return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
+}
+
+constexpr Twist3d Transform3d::Log() const {
+  // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
+
+  auto impl = [this]<typename Matrix3d, typename Vector3d>() -> Twist3d {
+    Vector3d u{{m_translation.X().value(), m_translation.Y().value(),
+                m_translation.Z().value()}};
+    Vector3d rvec = m_rotation.ToVector();
+    Matrix3d omega = detail::RotationVectorToMatrix(rvec);
+    Matrix3d omegaSq = omega * omega;
+    double theta = rvec.norm();
+    double thetaSq = theta * theta;
+
+    double C;
+    if (gcem::abs(theta) < 1E-7) {
+      // Taylor Expansions around θ = 0
+      // A = 1/1! - θ²/3! + θ⁴/5!
+      // B = 1/2! - θ²/4! + θ⁴/6!
+      // C = 1/6 * (1/2 + θ²/5! + θ⁴/7!)
+      // sources:
+      // A:
+      // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
+      // B:
+      // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+      // C:
+      // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5BDivide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2C2Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+      C = 1 / 12.0 + thetaSq / 720 + thetaSq * thetaSq / 30240;
+    } else {
+      // A = std::sin(θ)/θ
+      // B = (1 - std::cos(θ)) / θ²
+      // C = (1 - A/(2*B)) / θ²
+      double A = gcem::sin(theta) / theta;
+      double B = (1 - gcem::cos(theta)) / thetaSq;
+      C = (1 - A / (2 * B)) / thetaSq;
+    }
+
+    Matrix3d V_inv = Matrix3d::Identity() - 0.5 * omega + C * omegaSq;
+
+    Vector3d translation_component = V_inv * u;
+
+    return Twist3d{units::meter_t{translation_component(0)},
+                   units::meter_t{translation_component(1)},
+                   units::meter_t{translation_component(2)},
+                   units::radian_t{rvec(0)},
+                   units::radian_t{rvec(1)},
+                   units::radian_t{rvec(2)}};
+  };
+
+  if (std::is_constant_evaluated()) {
+    return impl.template operator()<ct_matrix3d, ct_vector3d>();
+  }
+  return impl.template operator()<Eigen::Matrix3d, Eigen::Vector3d>();
+}
+
+}  // namespace frc
+
+#include "frc/geometry/proto/Transform3dProto.h"
+#include "frc/geometry/struct/Transform3dStruct.h"

wpimath/src/main/native/include/wpi/math/geometry/Translation2d.hpp

@@ -0,0 +1,331 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <initializer_list>
+#include <span>
+
+#include <Eigen/Core>
+#include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+
+#include "frc/geometry/Rotation2d.h"
+#include "units/area.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+
+/**
+ * Represents a translation in 2D space.
+ * This object can be used to represent a point or a vector.
+ *
+ * This assumes that you are using conventional mathematical axes.
+ * When the robot is at the origin facing in the positive X direction, forward
+ * is positive X and left is positive Y.
+ */
+class WPILIB_DLLEXPORT Translation2d {
+ public:
+  /**
+   * Constructs a Translation2d with X and Y components equal to zero.
+   */
+  constexpr Translation2d() = default;
+
+  /**
+   * Constructs a Translation2d with the X and Y components equal to the
+   * provided values.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   */
+  constexpr Translation2d(units::meter_t x, units::meter_t y)
+      : m_x{x}, m_y{y} {}
+
+  /**
+   * Constructs a Translation2d with the provided distance and angle. This is
+   * essentially converting from polar coordinates to Cartesian coordinates.
+   *
+   * @param distance The distance from the origin to the end of the translation.
+   * @param angle The angle between the x-axis and the translation vector.
+   */
+  constexpr Translation2d(units::meter_t distance, const Rotation2d& angle)
+      : m_x{distance * angle.Cos()}, m_y{distance * angle.Sin()} {}
+
+  /**
+   * Constructs a Translation2d from a 2D translation vector. The values are
+   * assumed to be in meters.
+   *
+   * @param vector The translation vector.
+   */
+  constexpr explicit Translation2d(const Eigen::Vector2d& vector)
+      : m_x{units::meter_t{vector.x()}}, m_y{units::meter_t{vector.y()}} {}
+
+  /**
+   * Calculates the distance between two translations in 2D space.
+   *
+   * The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²).
+   *
+   * @param other The translation to compute the distance to.
+   *
+   * @return The distance between the two translations.
+   */
+  constexpr units::meter_t Distance(const Translation2d& other) const {
+    return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
+  }
+
+  /**
+   * Calculates the square of the distance between two translations in 2D space.
+   * This is equivalent to squaring the result of Distance(Translation2d), but
+   * avoids computing a square root.
+   *
+   * The square of the distance between translations is defined as
+   * (x₂−x₁)²+(y₂−y₁)².
+   *
+   * @param other The translation to compute the squared distance to.
+   * @return The square of the distance between the two translations.
+   */
+  constexpr units::square_meter_t SquaredDistance(
+      const Translation2d& other) const {
+    return units::math::pow<2>(other.m_x - m_x) +
+           units::math::pow<2>(other.m_y - m_y);
+  }
+
+  /**
+   * Returns the X component of the translation.
+   *
+   * @return The X component of the translation.
+   */
+  constexpr units::meter_t X() const { return m_x; }
+
+  /**
+   * Returns the Y component of the translation.
+   *
+   * @return The Y component of the translation.
+   */
+  constexpr units::meter_t Y() const { return m_y; }
+
+  /**
+   * Returns a 2D translation vector representation of this translation.
+   *
+   * @return A 2D translation vector representation of this translation.
+   */
+  constexpr Eigen::Vector2d ToVector() const {
+    return Eigen::Vector2d{{m_x.value(), m_y.value()}};
+  }
+
+  /**
+   * Returns the norm, or distance from the origin to the translation.
+   *
+   * @return The norm of the translation.
+   */
+  constexpr units::meter_t Norm() const { return units::math::hypot(m_x, m_y); }
+
+  /**
+   * Returns the squared norm, or squared distance from the origin to the
+   * translation. This is equivalent to squaring the result of Norm(), but
+   * avoids computing a square root.
+   *
+   * @return The squared norm of the translation.
+   */
+  constexpr units::square_meter_t SquaredNorm() const {
+    return units::math::pow<2>(m_x) + units::math::pow<2>(m_y);
+  }
+
+  /**
+   * Returns the angle this translation forms with the positive X axis.
+   *
+   * @return The angle of the translation
+   */
+  constexpr Rotation2d Angle() const {
+    return Rotation2d{m_x.value(), m_y.value()};
+  }
+
+  /**
+   * Applies a rotation to the translation in 2D space.
+   *
+   * This multiplies the translation vector by a counterclockwise rotation
+   * matrix of the given angle.
+   *
+   * <pre>
+   * [x_new]   [other.cos, -other.sin][x]
+   * [y_new] = [other.sin,  other.cos][y]
+   * </pre>
+   *
+   * For example, rotating a Translation2d of &lt;2, 0&gt; by 90 degrees will
+   * return a Translation2d of &lt;0, 2&gt;.
+   *
+   * @param other The rotation to rotate the translation by.
+   *
+   * @return The new rotated translation.
+   */
+  constexpr Translation2d RotateBy(const Rotation2d& other) const {
+    return {m_x * other.Cos() - m_y * other.Sin(),
+            m_x * other.Sin() + m_y * other.Cos()};
+  }
+
+  /**
+   * Rotates this translation around another translation in 2D space.
+   *
+   * <pre>
+   * [x_new]   [rot.cos, -rot.sin][x - other.x]   [other.x]
+   * [y_new] = [rot.sin,  rot.cos][y - other.y] + [other.y]
+   * </pre>
+   *
+   * @param other The other translation to rotate around.
+   * @param rot The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  constexpr Translation2d RotateAround(const Translation2d& other,
+                                       const Rotation2d& rot) const {
+    return {(m_x - other.X()) * rot.Cos() - (m_y - other.Y()) * rot.Sin() +
+                other.X(),
+            (m_x - other.X()) * rot.Sin() + (m_y - other.Y()) * rot.Cos() +
+                other.Y()};
+  }
+
+  /**
+   * Computes the dot product between this translation and another translation
+   * in 2D space.
+   *
+   * The dot product between two translations is defined as x₁x₂+y₁y₂.
+   *
+   * @param other The translation to compute the dot product with.
+   * @return The dot product between the two translations.
+   */
+  constexpr units::square_meter_t Dot(const Translation2d& other) const {
+    return m_x * other.X() + m_y * other.Y();
+  }
+
+  /**
+   * Computes the cross product between this translation and another translation
+   * in 2D space.
+   *
+   * The 2D cross product between two translations is defined as x₁y₂-x₂y₁.
+   *
+   * @param other The translation to compute the cross product with.
+   * @return The cross product between the two translations.
+   */
+  constexpr units::square_meter_t Cross(const Translation2d& other) const {
+    return m_x * other.Y() - m_y * other.X();
+  }
+
+  /**
+   * Returns the sum of two translations in 2D space.
+   *
+   * For example, Translation3d{1.0, 2.5} + Translation3d{2.0, 5.5} =
+   * Translation3d{3.0, 8.0}.
+   *
+   * @param other The translation to add.
+   *
+   * @return The sum of the translations.
+   */
+  constexpr Translation2d operator+(const Translation2d& other) const {
+    return {X() + other.X(), Y() + other.Y()};
+  }
+
+  /**
+   * Returns the difference between two translations.
+   *
+   * For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
+   * Translation2d{4.0, 2.0}.
+   *
+   * @param other The translation to subtract.
+   *
+   * @return The difference between the two translations.
+   */
+  constexpr Translation2d operator-(const Translation2d& other) const {
+    return *this + -other;
+  }
+
+  /**
+   * Returns the inverse of the current translation. This is equivalent to
+   * rotating by 180 degrees, flipping the point over both axes, or negating all
+   * components of the translation.
+   *
+   * @return The inverse of the current translation.
+   */
+  constexpr Translation2d operator-() const { return {-m_x, -m_y}; }
+
+  /**
+   * Returns the translation multiplied by a scalar.
+   *
+   * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}.
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The scaled translation.
+   */
+  constexpr Translation2d operator*(double scalar) const {
+    return {scalar * m_x, scalar * m_y};
+  }
+
+  /**
+   * Returns the translation divided by a scalar.
+   *
+   * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}.
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The scaled translation.
+   */
+  constexpr Translation2d operator/(double scalar) const {
+    return operator*(1.0 / scalar);
+  }
+
+  /**
+   * Checks equality between this Translation2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  constexpr bool operator==(const Translation2d& other) const {
+    return units::math::abs(m_x - other.m_x) < 1E-9_m &&
+           units::math::abs(m_y - other.m_y) < 1E-9_m;
+  }
+
+  /**
+   * Returns the nearest Translation2d from a collection of translations
+   * @param translations The collection of translations.
+   * @return The nearest Translation2d from the collection.
+   */
+  constexpr Translation2d Nearest(
+      std::span<const Translation2d> translations) const {
+    return *std::min_element(
+        translations.begin(), translations.end(),
+        [this](const Translation2d& a, const Translation2d& b) {
+          return this->Distance(a) < this->Distance(b);
+        });
+  }
+
+  /**
+   * Returns the nearest Translation2d from a collection of translations
+   * @param translations The collection of translations.
+   * @return The nearest Translation2d from the collection.
+   */
+  constexpr Translation2d Nearest(
+      std::initializer_list<Translation2d> translations) const {
+    return *std::min_element(
+        translations.begin(), translations.end(),
+        [this](const Translation2d& a, const Translation2d& b) {
+          return this->Distance(a) < this->Distance(b);
+        });
+  }
+
+ private:
+  units::meter_t m_x = 0_m;
+  units::meter_t m_y = 0_m;
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Translation2d& state);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Translation2d& state);
+
+}  // namespace frc
+
+#include "frc/geometry/proto/Translation2dProto.h"
+#include "frc/geometry/struct/Translation2dStruct.h"

wpimath/src/main/native/include/wpi/math/geometry/Translation3d.hpp

@@ -0,0 +1,356 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <initializer_list>
+#include <span>
+
+#include <Eigen/Core>
+#include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+
+#include "frc/geometry/Rotation3d.h"
+#include "frc/geometry/Translation2d.h"
+#include "units/area.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+
+/**
+ * Represents a translation in 3D space.
+ * This object can be used to represent a point or a vector.
+ *
+ * This assumes that you are using conventional mathematical axes. When the
+ * robot is at the origin facing in the positive X direction, forward is
+ * positive X, left is positive Y, and up is positive Z.
+ */
+class WPILIB_DLLEXPORT Translation3d {
+ public:
+  /**
+   * Constructs a Translation3d with X, Y, and Z components equal to zero.
+   */
+  constexpr Translation3d() = default;
+
+  /**
+   * Constructs a Translation3d with the X, Y, and Z components equal to the
+   * provided values.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   * @param z The z component of the translation.
+   */
+  constexpr Translation3d(units::meter_t x, units::meter_t y, units::meter_t z)
+      : m_x{x}, m_y{y}, m_z{z} {}
+
+  /**
+   * Constructs a Translation3d with the provided distance and angle. This is
+   * essentially converting from polar coordinates to Cartesian coordinates.
+   *
+   * @param distance The distance from the origin to the end of the translation.
+   * @param angle The angle between the x-axis and the translation vector.
+   */
+  constexpr Translation3d(units::meter_t distance, const Rotation3d& angle) {
+    auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
+    m_x = rectangular.X();
+    m_y = rectangular.Y();
+    m_z = rectangular.Z();
+  }
+
+  /**
+   * Constructs a Translation3d from a 3D translation vector. The values are
+   * assumed to be in meters.
+   *
+   * @param vector The translation vector.
+   */
+  constexpr explicit Translation3d(const Eigen::Vector3d& vector)
+      : m_x{units::meter_t{vector.x()}},
+        m_y{units::meter_t{vector.y()}},
+        m_z{units::meter_t{vector.z()}} {}
+
+  /**
+   * Constructs a 3D translation from a 2D translation in the X-Y plane.
+   *
+   * @param translation The 2D translation.
+   * @see Pose3d(Pose2d)
+   * @see Transform3d(Transform2d)
+   */
+  constexpr explicit Translation3d(const Translation2d& translation)
+      : Translation3d{translation.X(), translation.Y(), 0_m} {}
+
+  /**
+   * Calculates the distance between two translations in 3D space.
+   *
+   * The distance between translations is defined as
+   * √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²).
+   *
+   * @param other The translation to compute the distance to.
+   *
+   * @return The distance between the two translations.
+   */
+  constexpr units::meter_t Distance(const Translation3d& other) const {
+    return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) +
+                             units::math::pow<2>(other.m_y - m_y) +
+                             units::math::pow<2>(other.m_z - m_z));
+  }
+
+  /**
+   * Calculates the squared distance between two translations in 3D space. This
+   * is equivalent to squaring the result of Distance(Translation3d), but avoids
+   * computing a square root.
+   *
+   * The squared distance between translations is defined as
+   * (x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)².
+   *
+   * @param other The translation to compute the squared distance to.
+   * @return The squared distance between the two translations.
+   */
+  constexpr units::square_meter_t SquaredDistance(
+      const Translation3d& other) const {
+    return units::math::pow<2>(other.m_x - m_x) +
+           units::math::pow<2>(other.m_y - m_y) +
+           units::math::pow<2>(other.m_z - m_z);
+  }
+
+  /**
+   * Returns the X component of the translation.
+   *
+   * @return The Z component of the translation.
+   */
+  constexpr units::meter_t X() const { return m_x; }
+
+  /**
+   * Returns the Y component of the translation.
+   *
+   * @return The Y component of the translation.
+   */
+  constexpr units::meter_t Y() const { return m_y; }
+
+  /**
+   * Returns the Z component of the translation.
+   *
+   * @return The Z component of the translation.
+   */
+  constexpr units::meter_t Z() const { return m_z; }
+
+  /**
+   * Returns a 3D translation vector representation of this translation.
+   *
+   * @return A 3D translation vector representation of this translation.
+   */
+  constexpr Eigen::Vector3d ToVector() const {
+    return Eigen::Vector3d{{m_x.value(), m_y.value(), m_z.value()}};
+  }
+
+  /**
+   * Returns the norm, or distance from the origin to the translation.
+   *
+   * @return The norm of the translation.
+   */
+  constexpr units::meter_t Norm() const {
+    return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
+  }
+
+  /**
+   * Returns the squared norm, or squared distance from the origin to the
+   * translation. This is equivalent to squaring the result of Norm(), but
+   * avoids computing a square root.
+   *
+   * @return The squared norm of the translation.
+   */
+  constexpr units::square_meter_t SquaredNorm() const {
+    return m_x * m_x + m_y * m_y + m_z * m_z;
+  }
+
+  /**
+   * Applies a rotation to the translation in 3D space.
+   *
+   * For example, rotating a Translation3d of &lt;2, 0, 0&gt; by 90 degrees
+   * around the Z axis will return a Translation3d of &lt;0, 2, 0&gt;.
+   *
+   * @param other The rotation to rotate the translation by.
+   *
+   * @return The new rotated translation.
+   */
+  constexpr Translation3d RotateBy(const Rotation3d& other) const {
+    Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()};
+    auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse();
+    return Translation3d{units::meter_t{qprime.X()}, units::meter_t{qprime.Y()},
+                         units::meter_t{qprime.Z()}};
+  }
+
+  /**
+   * Rotates this translation around another translation in 3D space.
+   *
+   * @param other The other translation to rotate around.
+   * @param rot The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  constexpr Translation3d RotateAround(const Translation3d& other,
+                                       const Rotation3d& rot) const {
+    return (*this - other).RotateBy(rot) + other;
+  }
+
+  /**
+   * Computes the dot product between this translation and another translation
+   * in 3D space.
+   *
+   * The dot product between two translations is defined as x₁x₂+y₁y₂+z₁z₂.
+   *
+   * @param other The translation to compute the dot product with.
+   * @return The dot product between the two translations.
+   */
+  constexpr units::square_meter_t Dot(const Translation3d& other) const {
+    return m_x * other.X() + m_y * other.Y() + m_z * other.Z();
+  }
+
+  /**
+   * Computes the cross product between this translation and another
+   * translation in 3D space. The resulting translation will be perpendicular to
+   * both translations.
+   *
+   * The 3D cross product between two translations is defined as <y₁z₂-y₂z₁,
+   * z₁x₂-z₂x₁, x₁y₂-x₂y₁>.
+   *
+   * @param other The translation to compute the cross product with.
+   * @return The cross product between the two translations.
+   */
+  constexpr Eigen::Vector<units::square_meter_t, 3> Cross(
+      const Translation3d& other) const {
+    return Eigen::Vector<units::square_meter_t, 3>{
+        {m_y * other.Z() - other.Y() * m_z},
+        {m_z * other.X() - other.Z() * m_x},
+        {m_x * other.Y() - other.X() * m_y}};
+  }
+
+  /**
+   * Returns a Translation2d representing this Translation3d projected into the
+   * X-Y plane.
+   */
+  constexpr Translation2d ToTranslation2d() const {
+    return Translation2d{m_x, m_y};
+  }
+
+  /**
+   * Returns the sum of two translations in 3D space.
+   *
+   * For example, Translation3d{1.0, 2.5, 3.5} + Translation3d{2.0, 5.5, 7.5} =
+   * Translation3d{3.0, 8.0, 11.0}.
+   *
+   * @param other The translation to add.
+   *
+   * @return The sum of the translations.
+   */
+  constexpr Translation3d operator+(const Translation3d& other) const {
+    return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};
+  }
+
+  /**
+   * Returns the difference between two translations.
+   *
+   * For example, Translation3d{5.0, 4.0, 3.0} - Translation3d{1.0, 2.0, 3.0} =
+   * Translation3d{4.0, 2.0, 0.0}.
+   *
+   * @param other The translation to subtract.
+   *
+   * @return The difference between the two translations.
+   */
+  constexpr Translation3d operator-(const Translation3d& other) const {
+    return operator+(-other);
+  }
+
+  /**
+   * Returns the inverse of the current translation. This is equivalent to
+   * negating all components of the translation.
+   *
+   * @return The inverse of the current translation.
+   */
+  constexpr Translation3d operator-() const { return {-m_x, -m_y, -m_z}; }
+
+  /**
+   * Returns the translation multiplied by a scalar.
+   *
+   * For example, Translation3d{2.0, 2.5, 4.5} * 2 = Translation3d{4.0, 5.0,
+   * 9.0}.
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The scaled translation.
+   */
+  constexpr Translation3d operator*(double scalar) const {
+    return {scalar * m_x, scalar * m_y, scalar * m_z};
+  }
+
+  /**
+   * Returns the translation divided by a scalar.
+   *
+   * For example, Translation3d{2.0, 2.5, 4.5} / 2 = Translation3d{1.0, 1.25,
+   * 2.25}.
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The scaled translation.
+   */
+  constexpr Translation3d operator/(double scalar) const {
+    return operator*(1.0 / scalar);
+  }
+
+  /**
+   * Checks equality between this Translation3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  constexpr bool operator==(const Translation3d& other) const {
+    return units::math::abs(m_x - other.m_x) < 1E-9_m &&
+           units::math::abs(m_y - other.m_y) < 1E-9_m &&
+           units::math::abs(m_z - other.m_z) < 1E-9_m;
+  }
+
+  /**
+   * Returns the nearest Translation3d from a collection of translations
+   * @param translations The collection of translations.
+   * @return The nearest Translation3d from the collection.
+   */
+  constexpr Translation3d Nearest(
+      std::span<const Translation3d> translations) const {
+    return *std::min_element(
+        translations.begin(), translations.end(),
+        [this](const Translation3d& a, const Translation3d& b) {
+          return this->Distance(a) < this->Distance(b);
+        });
+  }
+
+  /**
+   * Returns the nearest Translation3d from a collection of translations
+   * @param translations The collection of translations.
+   * @return The nearest Translation3d from the collection.
+   */
+  constexpr Translation3d Nearest(
+      std::initializer_list<Translation3d> translations) const {
+    return *std::min_element(
+        translations.begin(), translations.end(),
+        [this](const Translation3d& a, const Translation3d& b) {
+          return this->Distance(a) < this->Distance(b);
+        });
+  }
+
+ private:
+  units::meter_t m_x = 0_m;
+  units::meter_t m_y = 0_m;
+  units::meter_t m_z = 0_m;
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Translation3d& state);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Translation3d& state);
+
+}  // namespace frc
+
+#include "frc/geometry/proto/Translation3dProto.h"
+#include "frc/geometry/struct/Translation3dStruct.h"

wpimath/src/main/native/include/wpi/math/geometry/Twist2d.hpp

@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "units/angle.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+
+class Transform2d;
+
+/**
+ * A change in distance along a 2D arc since the last pose update. We can use
+ * ideas from differential calculus to create new Pose2ds from a Twist2d and
+ * vice versa.
+ *
+ * A Twist can be used to represent a difference between two poses.
+ */
+struct WPILIB_DLLEXPORT Twist2d {
+  /**
+   * Linear "dx" component
+   */
+  units::meter_t dx = 0_m;
+
+  /**
+   * Linear "dy" component
+   */
+  units::meter_t dy = 0_m;
+
+  /**
+   * Angular "dtheta" component (radians)
+   */
+  units::radian_t dtheta = 0_rad;
+
+  /**
+   * Obtain a new Transform2d from a (constant curvature) velocity.
+   *
+   * See "https://file.tavsys.net/control/controls-engineering-in-frc.pdf"
+   * Controls Engineering in the FIRST Robotics Competition</a> section 10.2
+   * "Pose exponential" for a derivation.
+   *
+   * The twist is a change in pose in the robot's coordinate frame since the
+   * previous pose update. When the user runs Exp() on the twist, the user will
+   * receive the pose delta.
+   *
+   * "Exp" represents the pose exponential, which is solving a differential
+   * equation moving the pose forward in time.
+   *
+   * @return The pose delta of the robot.
+   */
+  constexpr Transform2d Exp() const;
+
+  /**
+   * Checks equality between this Twist2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  constexpr bool operator==(const Twist2d& other) const {
+    return units::math::abs(dx - other.dx) < 1E-9_m &&
+           units::math::abs(dy - other.dy) < 1E-9_m &&
+           units::math::abs(dtheta - other.dtheta) < 1E-9_rad;
+  }
+
+  /**
+   * Scale this by a given factor.
+   *
+   * @param factor The factor by which to scale.
+   * @return The scaled Twist2d.
+   */
+  constexpr Twist2d operator*(double factor) const {
+    return Twist2d{dx * factor, dy * factor, dtheta * factor};
+  }
+};
+
+}  // namespace frc
+
+#include "frc/geometry/Transform2d.h"
+
+namespace frc {
+
+constexpr Transform2d Twist2d::Exp() const {
+  const auto theta = dtheta.value();
+  const auto sinTheta = gcem::sin(theta);
+  const auto cosTheta = gcem::cos(theta);
+
+  double s, c;
+  if (gcem::abs(theta) < 1E-9) {
+    s = 1.0 - 1.0 / 6.0 * theta * theta;
+    c = 0.5 * theta;
+  } else {
+    s = sinTheta / theta;
+    c = (1 - cosTheta) / theta;
+  }
+
+  return Transform2d(Translation2d{dx * s - dy * c, dx * c + dy * s},
+                     Rotation2d{cosTheta, sinTheta});
+}
+
+}  // namespace frc
+
+#include "frc/geometry/proto/Twist2dProto.h"
+#include "frc/geometry/struct/Twist2dStruct.h"

wpimath/src/main/native/include/wpi/math/geometry/Twist3d.hpp

@@ -0,0 +1,168 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "units/angle.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+
+class Transform3d;
+
+/**
+ * A change in distance along a 3D arc since the last pose update. We can use
+ * ideas from differential calculus to create new Pose3ds from a Twist3d and
+ * vice versa.
+ *
+ * A Twist can be used to represent a difference between two poses.
+ */
+struct WPILIB_DLLEXPORT Twist3d {
+  /**
+   * Linear "dx" component
+   */
+  units::meter_t dx = 0_m;
+
+  /**
+   * Linear "dy" component
+   */
+  units::meter_t dy = 0_m;
+
+  /**
+   * Linear "dz" component
+   */
+  units::meter_t dz = 0_m;
+
+  /**
+   * Rotation vector x component.
+   */
+  units::radian_t rx = 0_rad;
+
+  /**
+   * Rotation vector y component.
+   */
+  units::radian_t ry = 0_rad;
+
+  /**
+   * Rotation vector z component.
+   */
+  units::radian_t rz = 0_rad;
+
+  /**
+   * Obtain a new Transform3d from a (constant curvature) velocity.
+   *
+   * See "https://file.tavsys.net/control/controls-engineering-in-frc.pdf"
+   * Controls Engineering in the FIRST Robotics Competition section 10.2
+   * "Pose exponential" for a derivation.
+   *
+   * The twist is a change in pose in the robot's coordinate frame since the
+   * previous pose update. When the user runs Exp() on the twist, the user will
+   * receive the pose delta.
+   *
+   * "Exp" represents the pose exponential, which is solving a differential
+   * equation moving the pose forward in time.
+   *
+   * @return The pose delta of the robot.
+   */
+  constexpr Transform3d Exp() const;
+
+  /**
+   * Checks equality between this Twist3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  constexpr bool operator==(const Twist3d& other) const {
+    return units::math::abs(dx - other.dx) < 1E-9_m &&
+           units::math::abs(dy - other.dy) < 1E-9_m &&
+           units::math::abs(dz - other.dz) < 1E-9_m &&
+           units::math::abs(rx - other.rx) < 1E-9_rad &&
+           units::math::abs(ry - other.ry) < 1E-9_rad &&
+           units::math::abs(rz - other.rz) < 1E-9_rad;
+  }
+
+  /**
+   * Scale this by a given factor.
+   *
+   * @param factor The factor by which to scale.
+   * @return The scaled Twist3d.
+   */
+  constexpr Twist3d operator*(double factor) const {
+    return Twist3d{dx * factor, dy * factor, dz * factor,
+                   rx * factor, ry * factor, rz * factor};
+  }
+};
+
+}  // namespace frc
+
+#include "frc/geometry/Transform3d.h"
+#include "frc/geometry/detail/RotationVectorToMatrix.h"
+
+namespace frc {
+
+constexpr Transform3d Twist3d::Exp() const {
+  // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
+
+  auto impl = [this]<typename Matrix3d, typename Vector3d>() -> Transform3d {
+    Vector3d u{{dx.value(), dy.value(), dz.value()}};
+    Vector3d rvec{{rx.value(), ry.value(), rz.value()}};
+    Matrix3d omega = detail::RotationVectorToMatrix(rvec);
+    Matrix3d omegaSq = omega * omega;
+    double theta = rvec.norm();
+    double thetaSq = theta * theta;
+
+    double A;
+    double B;
+    double C;
+    if (gcem::abs(theta) < 1E-7) {
+      // Taylor Expansions around θ = 0
+      // A = 1/1! - θ²/3! + θ⁴/5!
+      // B = 1/2! - θ²/4! + θ⁴/6!
+      // C = 1/3! - θ²/5! + θ⁴/7!
+      // sources:
+      // A:
+      // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
+      // B:
+      // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+      // C:
+      // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+      A = 1 - thetaSq / 6 + thetaSq * thetaSq / 120;
+      B = 1 / 2.0 - thetaSq / 24 + thetaSq * thetaSq / 720;
+      C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
+    } else {
+      // A = std::sin(θ)/θ
+      // B = (1 - std::cos(θ)) / θ²
+      // C = (1 - A) / θ²
+      A = gcem::sin(theta) / theta;
+      B = (1 - gcem::cos(theta)) / thetaSq;
+      C = (1 - A) / thetaSq;
+    }
+
+    Matrix3d R = Matrix3d::Identity() + A * omega + B * omegaSq;
+    Matrix3d V = Matrix3d::Identity() + B * omega + C * omegaSq;
+
+    Vector3d translation_component = V * u;
+
+    const Transform3d transform{
+        Translation3d{units::meter_t{translation_component(0)},
+                      units::meter_t{translation_component(1)},
+                      units::meter_t{translation_component(2)}},
+        Rotation3d{R}};
+
+    return transform;
+  };
+
+  if (std::is_constant_evaluated()) {
+    return impl.template operator()<ct_matrix3d, ct_vector3d>();
+  }
+  return impl.template operator()<Eigen::Matrix3d, Eigen::Vector3d>();
+}
+
+}  // namespace frc
+
+#include "frc/geometry/proto/Twist3dProto.h"
+#include "frc/geometry/struct/Twist3dStruct.h"

wpimath/src/main/native/include/wpi/math/geometry/detail/RotationVectorToMatrix.hpp

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <Eigen/Core>
+
+#include "frc/ct_matrix.h"
+
+namespace frc::detail {
+
+constexpr ct_matrix3d RotationVectorToMatrix(const ct_vector3d& rotation) {
+  return ct_matrix3d{{0.0, -rotation(2), rotation(1)},
+                     {rotation(2), 0.0, -rotation(0)},
+                     {-rotation(1), rotation(0), 0.0}};
+}
+
+constexpr Eigen::Matrix3d RotationVectorToMatrix(
+    const Eigen::Vector3d& rotation) {
+  return Eigen::Matrix3d{{0.0, -rotation(2), rotation(1)},
+                         {rotation(2), 0.0, -rotation(0)},
+                         {-rotation(1), rotation(0), 0.0}};
+}
+
+}  // namespace frc::detail

wpimath/src/main/native/include/wpi/math/geometry/proto/Ellipse2dProto.hpp

@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Ellipse2d.h"
+#include "wpimath/protobuf/geometry2d.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Ellipse2d> {
+  using MessageStruct = wpi_proto_ProtobufEllipse2d;
+  using InputStream = wpi::ProtoInputStream<frc::Ellipse2d>;
+  using OutputStream = wpi::ProtoOutputStream<frc::Ellipse2d>;
+  static std::optional<frc::Ellipse2d> Unpack(InputStream& stream);
+  static bool Pack(OutputStream& stream, const frc::Ellipse2d& value);
+};

wpimath/src/main/native/include/wpi/math/geometry/proto/Pose2dProto.hpp

@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "pb.h"
+#include "wpimath/protobuf/geometry2d.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose2d> {
+  using MessageStruct = wpi_proto_ProtobufPose2d;
+  using InputStream = wpi::ProtoInputStream<frc::Pose2d>;
+  using OutputStream = wpi::ProtoOutputStream<frc::Pose2d>;
+  static std::optional<frc::Pose2d> Unpack(InputStream& stream);
+  static bool Pack(OutputStream& stream, const frc::Pose2d& value);
+};

wpimath/src/main/native/include/wpi/math/geometry/proto/Pose3dProto.hpp

@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Pose3d.h"
+#include "wpimath/protobuf/geometry3d.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose3d> {
+  using MessageStruct = wpi_proto_ProtobufPose3d;
+  using InputStream = wpi::ProtoInputStream<frc::Pose3d>;
+  using OutputStream = wpi::ProtoOutputStream<frc::Pose3d>;
+  static std::optional<frc::Pose3d> Unpack(InputStream& stream);
+  static bool Pack(OutputStream& stream, const frc::Pose3d& value);
+};

wpimath/src/main/native/include/wpi/math/geometry/proto/QuaternionProto.hpp

@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Quaternion.h"
+#include "wpimath/protobuf/geometry3d.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Quaternion> {
+  using MessageStruct = wpi_proto_ProtobufQuaternion;
+  using InputStream = wpi::ProtoInputStream<frc::Quaternion>;
+  using OutputStream = wpi::ProtoOutputStream<frc::Quaternion>;
+  static std::optional<frc::Quaternion> Unpack(InputStream& stream);
+  static bool Pack(OutputStream& stream, const frc::Quaternion& value);
+};

wpimath/src/main/native/include/wpi/math/geometry/proto/Rectangle2dProto.hpp

@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Rectangle2d.h"
+#include "wpimath/protobuf/geometry2d.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rectangle2d> {
+  using MessageStruct = wpi_proto_ProtobufRectangle2d;
+  using InputStream = wpi::ProtoInputStream<frc::Rectangle2d>;
+  using OutputStream = wpi::ProtoOutputStream<frc::Rectangle2d>;
+  static std::optional<frc::Rectangle2d> Unpack(InputStream& stream);
+  static bool Pack(OutputStream& stream, const frc::Rectangle2d& value);
+};

wpimath/src/main/native/include/wpi/math/geometry/proto/Rotation2dProto.hpp

@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Rotation2d.h"
+#include "pb.h"
+#include "wpimath/protobuf/geometry2d.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation2d> {
+  using MessageStruct = wpi_proto_ProtobufRotation2d;
+  using InputStream = wpi::ProtoInputStream<frc::Rotation2d>;
+  using OutputStream = wpi::ProtoOutputStream<frc::Rotation2d>;
+  static std::optional<frc::Rotation2d> Unpack(InputStream& stream);
+  static bool Pack(OutputStream& stream, const frc::Rotation2d& value);
+};

wpimath/src/main/native/include/wpi/math/geometry/proto/Rotation3dProto.hpp

@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Rotation3d.h"
+#include "wpimath/protobuf/geometry3d.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation3d> {
+  using MessageStruct = wpi_proto_ProtobufRotation3d;
+  using InputStream = wpi::ProtoInputStream<frc::Rotation3d>;
+  using OutputStream = wpi::ProtoOutputStream<frc::Rotation3d>;
+  static std::optional<frc::Rotation3d> Unpack(InputStream& stream);
+  static bool Pack(OutputStream& stream, const frc::Rotation3d& value);
+};

wpimath/src/main/native/include/wpi/math/geometry/proto/Transform2dProto.hpp

@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Transform2d.h"
+#include "wpimath/protobuf/geometry2d.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform2d> {
+  using MessageStruct = wpi_proto_ProtobufTransform2d;
+  using InputStream = wpi::ProtoInputStream<frc::Transform2d>;
+  using OutputStream = wpi::ProtoOutputStream<frc::Transform2d>;
+  static std::optional<frc::Transform2d> Unpack(InputStream& stream);
+  static bool Pack(OutputStream& stream, const frc::Transform2d& value);
+};

wpimath/src/main/native/include/wpi/math/geometry/proto/Transform3dProto.hpp

@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Transform3d.h"
+#include "wpimath/protobuf/geometry3d.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform3d> {
+  using MessageStruct = wpi_proto_ProtobufTransform3d;
+  using InputStream = wpi::ProtoInputStream<frc::Transform3d>;
+  using OutputStream = wpi::ProtoOutputStream<frc::Transform3d>;
+  static std::optional<frc::Transform3d> Unpack(InputStream& stream);
+  static bool Pack(OutputStream& stream, const frc::Transform3d& value);
+};

wpimath/src/main/native/include/wpi/math/geometry/proto/Translation2dProto.hpp

@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Translation2d.h"
+#include "pb.h"
+#include "wpimath/protobuf/geometry2d.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation2d> {
+  using MessageStruct = wpi_proto_ProtobufTranslation2d;
+  using InputStream = wpi::ProtoInputStream<frc::Translation2d>;
+  using OutputStream = wpi::ProtoOutputStream<frc::Translation2d>;
+  static std::optional<frc::Translation2d> Unpack(InputStream& stream);
+  static bool Pack(OutputStream& stream, const frc::Translation2d& value);
+};

wpimath/src/main/native/include/wpi/math/geometry/proto/Translation3dProto.hpp

@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Translation3d.h"
+#include "wpimath/protobuf/geometry3d.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation3d> {
+  using MessageStruct = wpi_proto_ProtobufTranslation3d;
+  using InputStream = wpi::ProtoInputStream<frc::Translation3d>;
+  using OutputStream = wpi::ProtoOutputStream<frc::Translation3d>;
+  static std::optional<frc::Translation3d> Unpack(InputStream& stream);
+  static bool Pack(OutputStream& stream, const frc::Translation3d& value);
+};

wpimath/src/main/native/include/wpi/math/geometry/proto/Twist2dProto.hpp

@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Twist2d.h"
+#include "wpimath/protobuf/geometry2d.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Twist2d> {
+  using MessageStruct = wpi_proto_ProtobufTwist2d;
+  using InputStream = wpi::ProtoInputStream<frc::Twist2d>;
+  using OutputStream = wpi::ProtoOutputStream<frc::Twist2d>;
+  static std::optional<frc::Twist2d> Unpack(InputStream& stream);
+  static bool Pack(OutputStream& stream, const frc::Twist2d& value);
+};

wpimath/src/main/native/include/wpi/math/geometry/proto/Twist3dProto.hpp

@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/geometry/Twist3d.h"
+#include "wpimath/protobuf/geometry3d.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Twist3d> {
+  using MessageStruct = wpi_proto_ProtobufTwist3d;
+  using InputStream = wpi::ProtoInputStream<frc::Twist3d>;
+  using OutputStream = wpi::ProtoOutputStream<frc::Twist3d>;
+  static std::optional<frc::Twist3d> Unpack(InputStream& stream);
+  static bool Pack(OutputStream& stream, const frc::Twist3d& value);
+};

wpimath/src/main/native/include/wpi/math/geometry/struct/Ellipse2dStruct.hpp

@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Ellipse2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Ellipse2d> {
+  static constexpr std::string_view GetTypeName() { return "Ellipse2d"; }
+  static constexpr size_t GetSize() {
+    return wpi::GetStructSize<frc::Pose2d>() + 16;
+  }
+  static constexpr std::string_view GetSchema() {
+    return "Pose2d center;double xSemiAxis;double ySemiAxis";
+  }
+
+  static frc::Ellipse2d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Ellipse2d& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Pose2d>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::Ellipse2d>);
+static_assert(wpi::HasNestedStruct<frc::Ellipse2d>);

wpimath/src/main/native/include/wpi/math/geometry/struct/Pose2dStruct.hpp

@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Pose2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Pose2d> {
+  static constexpr std::string_view GetTypeName() { return "Pose2d"; }
+  static constexpr size_t GetSize() {
+    return wpi::GetStructSize<frc::Translation2d>() +
+           wpi::GetStructSize<frc::Rotation2d>();
+  }
+  static constexpr std::string_view GetSchema() {
+    return "Translation2d translation;Rotation2d rotation";
+  }
+
+  static frc::Pose2d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Pose2d& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation2d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation2d>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::Pose2d>);
+static_assert(wpi::HasNestedStruct<frc::Pose2d>);

wpimath/src/main/native/include/wpi/math/geometry/struct/Pose3dStruct.hpp

@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Pose3d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Pose3d> {
+  static constexpr std::string_view GetTypeName() { return "Pose3d"; }
+  static constexpr size_t GetSize() {
+    return wpi::GetStructSize<frc::Translation3d>() +
+           wpi::GetStructSize<frc::Rotation3d>();
+  }
+  static constexpr std::string_view GetSchema() {
+    return "Translation3d translation;Rotation3d rotation";
+  }
+
+  static frc::Pose3d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Pose3d& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation3d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation3d>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::Pose3d>);
+static_assert(wpi::HasNestedStruct<frc::Pose3d>);

wpimath/src/main/native/include/wpi/math/geometry/struct/QuaternionStruct.hpp

@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Quaternion.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Quaternion> {
+  static constexpr std::string_view GetTypeName() { return "Quaternion"; }
+  static constexpr size_t GetSize() { return 32; }
+  static constexpr std::string_view GetSchema() {
+    return "double w;double x;double y;double z";
+  }
+
+  static frc::Quaternion Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Quaternion& value);
+};
+
+static_assert(wpi::StructSerializable<frc::Quaternion>);

wpimath/src/main/native/include/wpi/math/geometry/struct/Rectangle2dStruct.hpp

@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Rectangle2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Rectangle2d> {
+  static constexpr std::string_view GetTypeName() { return "Rectangle2d"; }
+  static constexpr size_t GetSize() {
+    return wpi::GetStructSize<frc::Pose2d>() + 16;
+  }
+  static constexpr std::string_view GetSchema() {
+    return "Pose2d center;double xWidth;double yWidth";
+  }
+
+  static frc::Rectangle2d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Rectangle2d& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Pose2d>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::Rectangle2d>);
+static_assert(wpi::HasNestedStruct<frc::Rectangle2d>);

wpimath/src/main/native/include/wpi/math/geometry/struct/Rotation2dStruct.hpp

@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Rotation2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Rotation2d> {
+  static constexpr std::string_view GetTypeName() { return "Rotation2d"; }
+  static constexpr size_t GetSize() { return 8; }
+  static constexpr std::string_view GetSchema() { return "double value"; }
+
+  static frc::Rotation2d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Rotation2d& value);
+};
+
+static_assert(wpi::StructSerializable<frc::Rotation2d>);

wpimath/src/main/native/include/wpi/math/geometry/struct/Rotation3dStruct.hpp

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Rotation3d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Rotation3d> {
+  static constexpr std::string_view GetTypeName() { return "Rotation3d"; }
+  static constexpr size_t GetSize() {
+    return wpi::GetStructSize<frc::Quaternion>();
+  }
+  static constexpr std::string_view GetSchema() { return "Quaternion q"; }
+
+  static frc::Rotation3d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Rotation3d& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Quaternion>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::Rotation3d>);
+static_assert(wpi::HasNestedStruct<frc::Rotation3d>);

wpimath/src/main/native/include/wpi/math/geometry/struct/Transform2dStruct.hpp

@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Transform2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Transform2d> {
+  static constexpr std::string_view GetTypeName() { return "Transform2d"; }
+  static constexpr size_t GetSize() {
+    return wpi::GetStructSize<frc::Translation2d>() +
+           wpi::GetStructSize<frc::Rotation2d>();
+  }
+  static constexpr std::string_view GetSchema() {
+    return "Translation2d translation;Rotation2d rotation";
+  }
+
+  static frc::Transform2d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Transform2d& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation2d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation2d>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::Transform2d>);
+static_assert(wpi::HasNestedStruct<frc::Transform2d>);

wpimath/src/main/native/include/wpi/math/geometry/struct/Transform3dStruct.hpp

@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Transform3d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Transform3d> {
+  static constexpr std::string_view GetTypeName() { return "Transform3d"; }
+  static constexpr size_t GetSize() {
+    return wpi::GetStructSize<frc::Translation3d>() +
+           wpi::GetStructSize<frc::Rotation3d>();
+  }
+  static constexpr std::string_view GetSchema() {
+    return "Translation3d translation;Rotation3d rotation";
+  }
+
+  static frc::Transform3d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Transform3d& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation3d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation3d>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::Transform3d>);
+static_assert(wpi::HasNestedStruct<frc::Transform3d>);

wpimath/src/main/native/include/wpi/math/geometry/struct/Translation2dStruct.hpp

@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Translation2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Translation2d> {
+  static constexpr std::string_view GetTypeName() { return "Translation2d"; }
+  static constexpr size_t GetSize() { return 16; }
+  static constexpr std::string_view GetSchema() { return "double x;double y"; }
+
+  static frc::Translation2d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Translation2d& value);
+};
+
+static_assert(wpi::StructSerializable<frc::Translation2d>);

wpimath/src/main/native/include/wpi/math/geometry/struct/Translation3dStruct.hpp

@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Translation3d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Translation3d> {
+  static constexpr std::string_view GetTypeName() { return "Translation3d"; }
+  static constexpr size_t GetSize() { return 24; }
+  static constexpr std::string_view GetSchema() {
+    return "double x;double y;double z";
+  }
+
+  static frc::Translation3d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Translation3d& value);
+};
+
+static_assert(wpi::StructSerializable<frc::Translation3d>);

wpimath/src/main/native/include/wpi/math/geometry/struct/Twist2dStruct.hpp

@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Twist2d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Twist2d> {
+  static constexpr std::string_view GetTypeName() { return "Twist2d"; }
+  static constexpr size_t GetSize() { return 24; }
+  static constexpr std::string_view GetSchema() {
+    return "double dx;double dy;double dtheta";
+  }
+
+  static frc::Twist2d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Twist2d& value);
+};
+
+static_assert(wpi::StructSerializable<frc::Twist2d>);

wpimath/src/main/native/include/wpi/math/geometry/struct/Twist3dStruct.hpp

@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/geometry/Twist3d.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::Twist3d> {
+  static constexpr std::string_view GetTypeName() { return "Twist3d"; }
+  static constexpr size_t GetSize() { return 48; }
+  static constexpr std::string_view GetSchema() {
+    return "double dx;double dy;double dz;double rx;double ry;double rz";
+  }
+
+  static frc::Twist3d Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data, const frc::Twist3d& value);
+};
+
+static_assert(wpi::StructSerializable<frc::Twist3d>);

wpimath/src/main/native/include/wpi/math/interpolation/TimeInterpolatableBuffer.hpp

@@ -0,0 +1,175 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <functional>
+#include <optional>
+#include <utility>
+#include <vector>
+
+#include <wpi/MathExtras.h>
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * The TimeInterpolatableBuffer provides an easy way to estimate past
+ * measurements. One application might be in conjunction with the
+ * DifferentialDrivePoseEstimator, where knowledge of the robot pose at the time
+ * when vision or other global measurement were recorded is necessary, or for
+ * recording the past angles of mechanisms as measured by encoders.
+ *
+ * When sampling this buffer, a user-provided function or wpi::Lerp can be
+ * used. For Pose2ds, we use Twists.
+ *
+ * @tparam T The type stored in this buffer.
+ */
+template <typename T>
+class TimeInterpolatableBuffer {
+ public:
+  /**
+   * Create a new TimeInterpolatableBuffer.
+   *
+   * @param historySize  The history size of the buffer.
+   * @param func The function used to interpolate between values.
+   */
+  TimeInterpolatableBuffer(units::second_t historySize,
+                           std::function<T(const T&, const T&, double)> func)
+      : m_historySize(historySize), m_interpolatingFunc(func) {}
+
+  /**
+   * Create a new TimeInterpolatableBuffer. By default, the interpolation
+   * function is wpi::Lerp except for Pose2d, which uses the pose exponential.
+   *
+   * @param historySize  The history size of the buffer.
+   */
+  explicit TimeInterpolatableBuffer(units::second_t historySize)
+      : m_historySize(historySize),
+        m_interpolatingFunc([](const T& start, const T& end, double t) {
+          return wpi::Lerp(start, end, t);
+        }) {}
+
+  /**
+   * Add a sample to the buffer.
+   *
+   * @param time   The timestamp of the sample.
+   * @param sample The sample object.
+   */
+  void AddSample(units::second_t time, T sample) {
+    // Add the new state into the vector
+    if (m_pastSnapshots.size() == 0 || time > m_pastSnapshots.back().first) {
+      m_pastSnapshots.emplace_back(time, sample);
+    } else {
+      auto first_after = std::upper_bound(
+          m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
+          [](auto t, const auto& pair) { return t < pair.first; });
+
+      if (first_after == m_pastSnapshots.begin()) {
+        // All entries come after the sample
+        m_pastSnapshots.insert(first_after, std::pair{time, sample});
+      } else if (auto last_not_greater_than = first_after - 1;
+                 last_not_greater_than == m_pastSnapshots.begin() ||
+                 last_not_greater_than->first < time) {
+        // Some entries come before the sample, but none are recorded with the
+        // same time
+        m_pastSnapshots.insert(first_after, std::pair{time, sample});
+      } else {
+        // An entry exists with the same recorded time
+        last_not_greater_than->second = sample;
+      }
+    }
+    while (time - m_pastSnapshots[0].first > m_historySize) {
+      m_pastSnapshots.erase(m_pastSnapshots.begin());
+    }
+  }
+
+  /** Clear all old samples. */
+  void Clear() { m_pastSnapshots.clear(); }
+
+  /**
+   * Sample the buffer at the given time. If the buffer is empty, an empty
+   * optional is returned.
+   *
+   * @param time The time at which to sample the buffer.
+   */
+  std::optional<T> Sample(units::second_t time) const {
+    if (m_pastSnapshots.empty()) {
+      return {};
+    }
+
+    // We will perform a binary search to find the index of the element in the
+    // vector that has a timestamp that is equal to or greater than the vision
+    // measurement timestamp.
+
+    if (time <= m_pastSnapshots.front().first) {
+      return m_pastSnapshots.front().second;
+    }
+    if (time > m_pastSnapshots.back().first) {
+      return m_pastSnapshots.back().second;
+    }
+    if (m_pastSnapshots.size() < 2) {
+      return m_pastSnapshots[0].second;
+    }
+
+    // Get the iterator which has a key no less than the requested key.
+    auto upper_bound = std::lower_bound(
+        m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
+        [](const auto& pair, auto t) { return t > pair.first; });
+
+    if (upper_bound == m_pastSnapshots.begin()) {
+      return upper_bound->second;
+    }
+
+    auto lower_bound = upper_bound - 1;
+
+    double t = ((time - lower_bound->first) /
+                (upper_bound->first - lower_bound->first));
+
+    return m_interpolatingFunc(lower_bound->second, upper_bound->second, t);
+  }
+
+  /**
+   * Grant access to the internal sample buffer. Used in Pose Estimation to
+   * replay odometry inputs stored within this buffer.
+   */
+  std::vector<std::pair<units::second_t, T>>& GetInternalBuffer() {
+    return m_pastSnapshots;
+  }
+
+  /**
+   * Grant access to the internal sample buffer.
+   */
+  const std::vector<std::pair<units::second_t, T>>& GetInternalBuffer() const {
+    return m_pastSnapshots;
+  }
+
+ private:
+  units::second_t m_historySize;
+  std::vector<std::pair<units::second_t, T>> m_pastSnapshots;
+  std::function<T(const T&, const T&, double)> m_interpolatingFunc;
+};
+
+// Template specialization to ensure that Pose2d uses pose exponential
+template <>
+inline TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
+    units::second_t historySize)
+    : m_historySize(historySize),
+      m_interpolatingFunc([](const Pose2d& start, const Pose2d& end, double t) {
+        if (t < 0) {
+          return start;
+        } else if (t >= 1) {
+          return end;
+        } else {
+          Twist2d twist = (end - start).Log();
+          Twist2d scaledTwist = twist * t;
+          return start + scaledTwist.Exp();
+        }
+      }) {}
+
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/kinematics/ChassisSpeeds.hpp

@@ -0,0 +1,201 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "units/angular_velocity.h"
+#include "units/velocity.h"
+
+namespace frc {
+/**
+ * Represents the speed of a robot chassis. Although this struct contains
+ * similar members compared to a Twist2d, they do NOT represent the same thing.
+ * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of
+ * reference, a ChassisSpeeds struct represents a robot's velocity.
+ *
+ * A strictly non-holonomic drivetrain, such as a differential drive, should
+ * never have a dy component because it can never move sideways. Holonomic
+ * drivetrains such as swerve and mecanum will often have all three components.
+ */
+struct WPILIB_DLLEXPORT ChassisSpeeds {
+  /**
+   * Velocity along the x-axis. (Fwd is +)
+   */
+  units::meters_per_second_t vx = 0_mps;
+
+  /**
+   * Velocity along the y-axis. (Left is +)
+   */
+  units::meters_per_second_t vy = 0_mps;
+
+  /**
+   * Represents the angular velocity of the robot frame. (CCW is +)
+   */
+  units::radians_per_second_t omega = 0_rad_per_s;
+
+  /**
+   * Creates a Twist2d from ChassisSpeeds.
+   *
+   * @param dt The duration of the timestep.
+   *
+   * @return Twist2d.
+   */
+  constexpr Twist2d ToTwist2d(units::second_t dt) const {
+    return Twist2d{vx * dt, vy * dt, omega * dt};
+  }
+
+  /**
+   * Discretizes a continuous-time chassis speed.
+   *
+   * This function converts a continuous-time chassis speed into a discrete-time
+   * one such that when the discrete-time chassis speed is applied for one
+   * timestep, the robot moves as if the velocity components are independent
+   * (i.e., the robot moves v_x * dt along the x-axis, v_y * dt along the
+   * y-axis, and omega * dt around the z-axis).
+   *
+   * This is useful for compensating for translational skew when translating and
+   * rotating a holonomic (swerve or mecanum) drivetrain. However, scaling down
+   * the ChassisSpeeds after discretizing (e.g., when desaturating swerve module
+   * speeds) rotates the direction of net motion in the opposite direction of
+   * rotational velocity, introducing a different translational skew which is
+   * not accounted for by discretization.
+   *
+   * @param dt The duration of the timestep the speeds should be applied for.
+   * @return Discretized ChassisSpeeds.
+   */
+  constexpr ChassisSpeeds Discretize(units::second_t dt) const {
+    // Construct the desired pose after a timestep, relative to the current
+    // pose. The desired pose has decoupled translation and rotation.
+    Transform2d desiredTransform{vx * dt, vy * dt, omega * dt};
+
+    // Find the chassis translation/rotation deltas in the robot frame that move
+    // the robot from its current pose to the desired pose
+    auto twist = desiredTransform.Log();
+
+    // Turn the chassis translation/rotation deltas into average velocities
+    return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt};
+  }
+
+  /**
+   * Converts this field-relative set of speeds into a robot-relative
+   * ChassisSpeeds object.
+   *
+   * @param robotAngle The angle of the robot as measured by a gyroscope. The
+   *     robot's angle is considered to be zero when it is facing directly away
+   *     from your alliance station wall. Remember that this should be CCW
+   *     positive.
+   * @return ChassisSpeeds object representing the speeds in the robot's frame
+   *     of reference.
+   */
+  constexpr ChassisSpeeds ToRobotRelative(const Rotation2d& robotAngle) const {
+    // CW rotation into chassis frame
+    auto rotated =
+        Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
+            .RotateBy(-robotAngle);
+    return {units::meters_per_second_t{rotated.X().value()},
+            units::meters_per_second_t{rotated.Y().value()}, omega};
+  }
+
+  /**
+   * Converts this robot-relative set of speeds into a field-relative
+   * ChassisSpeeds object.
+   *
+   * @param robotAngle The angle of the robot as measured by a gyroscope. The
+   *     robot's angle is considered to be zero when it is facing directly away
+   *     from your alliance station wall. Remember that this should be CCW
+   *     positive.
+   * @return ChassisSpeeds object representing the speeds in the field's frame
+   *     of reference.
+   */
+  constexpr ChassisSpeeds ToFieldRelative(const Rotation2d& robotAngle) const {
+    // CCW rotation out of chassis frame
+    auto rotated =
+        Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
+            .RotateBy(robotAngle);
+    return {units::meters_per_second_t{rotated.X().value()},
+            units::meters_per_second_t{rotated.Y().value()}, omega};
+  }
+
+  /**
+   * Adds two ChassisSpeeds and returns the sum.
+   *
+   * <p>For example, ChassisSpeeds{1.0, 0.5, 1.5} + ChassisSpeeds{2.0, 1.5, 0.5}
+   * = ChassisSpeeds{3.0, 2.0, 2.0}
+   *
+   * @param other The ChassisSpeeds to add.
+   *
+   * @return The sum of the ChassisSpeeds.
+   */
+  constexpr ChassisSpeeds operator+(const ChassisSpeeds& other) const {
+    return {vx + other.vx, vy + other.vy, omega + other.omega};
+  }
+
+  /**
+   * Subtracts the other ChassisSpeeds from the current ChassisSpeeds and
+   * returns the difference.
+   *
+   * <p>For example, ChassisSpeeds{5.0, 4.0, 2.0} - ChassisSpeeds{1.0, 2.0, 1.0}
+   * = ChassisSpeeds{4.0, 2.0, 1.0}
+   *
+   * @param other The ChassisSpeeds to subtract.
+   *
+   * @return The difference between the two ChassisSpeeds.
+   */
+  constexpr ChassisSpeeds operator-(const ChassisSpeeds& other) const {
+    return *this + -other;
+  }
+
+  /**
+   * Returns the inverse of the current ChassisSpeeds.
+   * This is equivalent to negating all components of the ChassisSpeeds.
+   *
+   * @return The inverse of the current ChassisSpeeds.
+   */
+  constexpr ChassisSpeeds operator-() const { return {-vx, -vy, -omega}; }
+
+  /**
+   * Multiplies the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
+   *
+   * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} * 2
+   * = ChassisSpeeds{4.0, 5.0, 1.0}
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The scaled ChassisSpeeds.
+   */
+  constexpr ChassisSpeeds operator*(double scalar) const {
+    return {scalar * vx, scalar * vy, scalar * omega};
+  }
+
+  /**
+   * Divides the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
+   *
+   * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} / 2
+   * = ChassisSpeeds{1.0, 1.25, 0.5}
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The scaled ChassisSpeeds.
+   */
+  constexpr ChassisSpeeds operator/(double scalar) const {
+    return operator*(1.0 / scalar);
+  }
+
+  /**
+   * Compares the ChassisSpeeds with another ChassisSpeed.
+   *
+   * @param other The other ChassisSpeeds.
+   *
+   * @return The result of the comparison. Is true if they are the same.
+   */
+  constexpr bool operator==(const ChassisSpeeds& other) const = default;
+};
+}  // namespace frc
+
+#include "frc/kinematics/proto/ChassisSpeedsProto.h"
+#include "frc/kinematics/struct/ChassisSpeedsStruct.h"

wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveKinematics.hpp

@@ -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.
+
+#pragma once
+
+#include <type_traits>
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Twist2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/DifferentialDriveWheelPositions.h"
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+#include "frc/kinematics/Kinematics.h"
+#include "units/angle.h"
+#include "units/length.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+/**
+ * Helper class that converts a chassis velocity (dx and dtheta components) to
+ * left and right wheel velocities for a differential drive.
+ *
+ * Inverse kinematics converts a desired chassis speed into left and right
+ * velocity components whereas forward kinematics converts left and right
+ * component velocities into a linear and angular chassis speed.
+ */
+class WPILIB_DLLEXPORT DifferentialDriveKinematics
+    : public Kinematics<DifferentialDriveWheelSpeeds,
+                        DifferentialDriveWheelPositions> {
+ public:
+  /**
+   * Constructs a differential drive kinematics object.
+   *
+   * @param trackwidth The trackwidth of the drivetrain. Theoretically, this is
+   * the distance between the left wheels and right wheels. However, the
+   * empirical value may be larger than the physical measured value due to
+   * scrubbing effects.
+   */
+  constexpr explicit DifferentialDriveKinematics(units::meter_t trackwidth)
+      : trackwidth(trackwidth) {
+    if (!std::is_constant_evaluated()) {
+      wpi::math::MathSharedStore::ReportUsage("DifferentialDriveKinematics",
+                                              "");
+    }
+  }
+
+  /**
+   * Returns a chassis speed from left and right component velocities using
+   * forward kinematics.
+   *
+   * @param wheelSpeeds The left and right velocities.
+   * @return The chassis speed.
+   */
+  constexpr ChassisSpeeds ToChassisSpeeds(
+      const DifferentialDriveWheelSpeeds& wheelSpeeds) const override {
+    return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
+            (wheelSpeeds.right - wheelSpeeds.left) / trackwidth * 1_rad};
+  }
+
+  /**
+   * Returns left and right component velocities from a chassis speed using
+   * inverse kinematics.
+   *
+   * @param chassisSpeeds The linear and angular (dx and dtheta) components that
+   * represent the chassis' speed.
+   * @return The left and right velocities.
+   */
+  constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(
+      const ChassisSpeeds& chassisSpeeds) const override {
+    return {chassisSpeeds.vx - trackwidth / 2 * chassisSpeeds.omega / 1_rad,
+            chassisSpeeds.vx + trackwidth / 2 * chassisSpeeds.omega / 1_rad};
+  }
+
+  /**
+   * Returns a twist from left and right distance deltas using
+   * forward kinematics.
+   *
+   * @param leftDistance The distance measured by the left encoder.
+   * @param rightDistance The distance measured by the right encoder.
+   * @return The resulting Twist2d.
+   */
+  constexpr Twist2d ToTwist2d(const units::meter_t leftDistance,
+                              const units::meter_t rightDistance) const {
+    return {(leftDistance + rightDistance) / 2, 0_m,
+            (rightDistance - leftDistance) / trackwidth * 1_rad};
+  }
+
+  constexpr Twist2d ToTwist2d(
+      const DifferentialDriveWheelPositions& start,
+      const DifferentialDriveWheelPositions& end) const override {
+    return ToTwist2d(end.left - start.left, end.right - start.right);
+  }
+
+  constexpr DifferentialDriveWheelPositions Interpolate(
+      const DifferentialDriveWheelPositions& start,
+      const DifferentialDriveWheelPositions& end, double t) const override {
+    return start.Interpolate(end, t);
+  }
+
+  /// Differential drive trackwidth.
+  units::meter_t trackwidth;
+};
+}  // namespace frc
+
+#include "frc/kinematics/proto/DifferentialDriveKinematicsProto.h"
+#include "frc/kinematics/struct/DifferentialDriveKinematicsStruct.h"

wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry.hpp

@@ -0,0 +1,87 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/DifferentialDriveWheelPositions.h"
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+#include "frc/kinematics/Odometry.h"
+#include "units/length.h"
+
+namespace frc {
+/**
+ * Class for differential drive odometry. Odometry allows you to track the
+ * robot's position on the field over the course of a match using readings from
+ * 2 encoders and a gyroscope.
+ *
+ * Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ *
+ * It is important that you reset your encoders to zero before using this class.
+ * Any subsequent pose resets also require the encoders to be reset to zero.
+ */
+class WPILIB_DLLEXPORT DifferentialDriveOdometry
+    : public Odometry<DifferentialDriveWheelSpeeds,
+                      DifferentialDriveWheelPositions> {
+ public:
+  /**
+   * Constructs a DifferentialDriveOdometry object.
+   *
+   * IF leftDistance and rightDistance are unspecified,
+   * You NEED to reset your encoders (to zero).
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @param initialPose The starting position of the robot on the field.
+   */
+  explicit DifferentialDriveOdometry(const Rotation2d& gyroAngle,
+                                     units::meter_t leftDistance,
+                                     units::meter_t rightDistance,
+                                     const Pose2d& initialPose = Pose2d{});
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * IF leftDistance and rightDistance are unspecified,
+   * You NEED to reset your encoders (to zero).
+   *
+   * The gyroscope angle does not need to be reset here on the user's robot
+   * code. The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param pose The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   */
+  void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
+                     units::meter_t rightDistance, const Pose2d& pose) {
+    Odometry::ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose);
+  }
+
+  /**
+   * Updates the robot position on the field using distance measurements from
+   * encoders. This method is more numerically accurate than using velocities to
+   * integrate the pose and is also advantageous for teams that are using lower
+   * CPR encoders.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @return The new pose of the robot.
+   */
+  const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
+                       units::meter_t rightDistance) {
+    return Odometry::Update(gyroAngle, {leftDistance, rightDistance});
+  }
+
+ private:
+  DifferentialDriveKinematics m_kinematicsImpl{units::meter_t{1}};
+};
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry3d.hpp

@@ -0,0 +1,87 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/DifferentialDriveWheelPositions.h"
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+#include "frc/kinematics/Odometry3d.h"
+#include "units/length.h"
+
+namespace frc {
+/**
+ * Class for differential drive odometry. Odometry allows you to track the
+ * robot's position on the field over the course of a match using readings from
+ * 2 encoders and a gyroscope.
+ *
+ * Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ *
+ * It is important that you reset your encoders to zero before using this class.
+ * Any subsequent pose resets also require the encoders to be reset to zero.
+ */
+class WPILIB_DLLEXPORT DifferentialDriveOdometry3d
+    : public Odometry3d<DifferentialDriveWheelSpeeds,
+                        DifferentialDriveWheelPositions> {
+ public:
+  /**
+   * Constructs a DifferentialDriveOdometry3d object.
+   *
+   * IF leftDistance and rightDistance are unspecified,
+   * You NEED to reset your encoders (to zero).
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @param initialPose The starting position of the robot on the field.
+   */
+  explicit DifferentialDriveOdometry3d(const Rotation3d& gyroAngle,
+                                       units::meter_t leftDistance,
+                                       units::meter_t rightDistance,
+                                       const Pose3d& initialPose = Pose3d{});
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * IF leftDistance and rightDistance are unspecified,
+   * You NEED to reset your encoders (to zero).
+   *
+   * The gyroscope angle does not need to be reset here on the user's robot
+   * code. The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param pose The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   */
+  void ResetPosition(const Rotation3d& gyroAngle, units::meter_t leftDistance,
+                     units::meter_t rightDistance, const Pose3d& pose) {
+    Odometry3d::ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose);
+  }
+
+  /**
+   * Updates the robot position on the field using distance measurements from
+   * encoders. This method is more numerically accurate than using velocities to
+   * integrate the pose and is also advantageous for teams that are using lower
+   * CPR encoders.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @return The new pose of the robot.
+   */
+  const Pose3d& Update(const Rotation3d& gyroAngle, units::meter_t leftDistance,
+                       units::meter_t rightDistance) {
+    return Odometry3d::Update(gyroAngle, {leftDistance, rightDistance});
+  }
+
+ private:
+  DifferentialDriveKinematics m_kinematicsImpl{units::meter_t{1}};
+};
+}  // namespace frc

wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelPositions.hpp

@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/MathExtras.h>
+#include <wpi/SymbolExports.h>
+
+#include "units/length.h"
+
+namespace frc {
+/**
+ * Represents the wheel positions for a differential drive drivetrain.
+ */
+struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions {
+  /**
+   * Distance driven by the left side.
+   */
+  units::meter_t left = 0_m;
+
+  /**
+   * Distance driven by the right side.
+   */
+  units::meter_t right = 0_m;
+
+  /**
+   * Checks equality between this DifferentialDriveWheelPositions and another
+   * object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  constexpr bool operator==(
+      const DifferentialDriveWheelPositions& other) const = default;
+
+  constexpr DifferentialDriveWheelPositions Interpolate(
+      const DifferentialDriveWheelPositions& endValue, double t) const {
+    return {wpi::Lerp(left, endValue.left, t),
+            wpi::Lerp(right, endValue.right, t)};
+  }
+};
+}  // namespace frc
+
+#include "frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h"
+#include "frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h"

wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp

@@ -0,0 +1,126 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "units/math.h"
+#include "units/velocity.h"
+
+namespace frc {
+/**
+ * Represents the wheel speeds for a differential drive drivetrain.
+ */
+struct WPILIB_DLLEXPORT DifferentialDriveWheelSpeeds {
+  /**
+   * Speed of the left side of the robot.
+   */
+  units::meters_per_second_t left = 0_mps;
+
+  /**
+   * Speed of the right side of the robot.
+   */
+  units::meters_per_second_t right = 0_mps;
+
+  /**
+   * Renormalizes the wheel speeds if either side is above the specified
+   * maximum.
+   *
+   * Sometimes, after inverse kinematics, the requested speed from one or more
+   * wheels may be above the max attainable speed for the driving motor on that
+   * wheel. To fix this issue, one can reduce all the wheel speeds to make sure
+   * that all requested module speeds are at-or-below the absolute threshold,
+   * while maintaining the ratio of speeds between wheels.
+   *
+   * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
+   */
+  constexpr void Desaturate(units::meters_per_second_t attainableMaxSpeed) {
+    auto realMaxSpeed =
+        units::math::max(units::math::abs(left), units::math::abs(right));
+
+    if (realMaxSpeed > attainableMaxSpeed) {
+      left = left / realMaxSpeed * attainableMaxSpeed;
+      right = right / realMaxSpeed * attainableMaxSpeed;
+    }
+  }
+
+  /**
+   * Adds two DifferentialDriveWheelSpeeds and returns the sum.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{1.0, 0.5} +
+   * DifferentialDriveWheelSpeeds{2.0, 1.5} =
+   * DifferentialDriveWheelSpeeds{3.0, 2.0}
+   *
+   * @param other The DifferentialDriveWheelSpeeds to add.
+   *
+   * @return The sum of the DifferentialDriveWheelSpeeds.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator+(
+      const DifferentialDriveWheelSpeeds& other) const {
+    return {left + other.left, right + other.right};
+  }
+
+  /**
+   * Subtracts the other DifferentialDriveWheelSpeeds from the current
+   * DifferentialDriveWheelSpeeds and returns the difference.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{5.0, 4.0} -
+   * DifferentialDriveWheelSpeeds{1.0, 2.0} =
+   * DifferentialDriveWheelSpeeds{4.0, 2.0}
+   *
+   * @param other The DifferentialDriveWheelSpeeds to subtract.
+   *
+   * @return The difference between the two DifferentialDriveWheelSpeeds.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator-(
+      const DifferentialDriveWheelSpeeds& other) const {
+    return *this + -other;
+  }
+
+  /**
+   * Returns the inverse of the current DifferentialDriveWheelSpeeds.
+   * This is equivalent to negating all components of the
+   * DifferentialDriveWheelSpeeds.
+   *
+   * @return The inverse of the current DifferentialDriveWheelSpeeds.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator-() const {
+    return {-left, -right};
+  }
+
+  /**
+   * Multiplies the DifferentialDriveWheelSpeeds by a scalar and returns the new
+   * DifferentialDriveWheelSpeeds.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} * 2
+   * = DifferentialDriveWheelSpeeds{4.0, 5.0}
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The scaled DifferentialDriveWheelSpeeds.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator*(double scalar) const {
+    return {scalar * left, scalar * right};
+  }
+
+  /**
+   * Divides the DifferentialDriveWheelSpeeds by a scalar and returns the new
+   * DifferentialDriveWheelSpeeds.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} / 2
+   * = DifferentialDriveWheelSpeeds{1.0, 1.25}
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The scaled DifferentialDriveWheelSpeeds.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator/(double scalar) const {
+    return operator*(1.0 / scalar);
+  }
+};
+}  // namespace frc
+
+#include "frc/kinematics/proto/DifferentialDriveWheelSpeedsProto.h"
+#include "frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h"

wpimath/src/main/native/include/wpi/math/kinematics/Kinematics.hpp

@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Twist2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+
+namespace frc {
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
+ * into individual wheel speeds. Robot code should not use this directly-
+ * Instead, use the particular type for your drivetrain (e.g.,
+ * DifferentialDriveKinematics).
+ *
+ * Inverse kinematics converts a desired chassis speed into wheel speeds whereas
+ * forward kinematics converts wheel speeds into chassis speed.
+ */
+template <typename WheelSpeeds, typename WheelPositions>
+  requires std::copy_constructible<WheelPositions> &&
+           std::assignable_from<WheelPositions&, const WheelPositions&>
+class WPILIB_DLLEXPORT Kinematics {
+ public:
+  virtual ~Kinematics() noexcept = default;
+
+  /**
+   * Performs forward kinematics to return the resulting chassis speed from the
+   * wheel speeds. This method is often used for odometry -- determining the
+   * robot's position on the field using data from the real-world speed of each
+   * wheel on the robot.
+   *
+   * @param wheelSpeeds The speeds of the wheels.
+   * @return The chassis speed.
+   */
+  virtual ChassisSpeeds ToChassisSpeeds(
+      const WheelSpeeds& wheelSpeeds) const = 0;
+
+  /**
+   * Performs inverse kinematics to return the wheel speeds from a desired
+   * chassis velocity. This method is often used to convert joystick values into
+   * wheel speeds.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @return The wheel speeds.
+   */
+  virtual WheelSpeeds ToWheelSpeeds(
+      const ChassisSpeeds& chassisSpeeds) const = 0;
+
+  /**
+   * Performs forward kinematics to return the resulting Twist2d from the given
+   * change in wheel positions. This method is often used for odometry --
+   * determining the robot's position on the field using changes in the distance
+   * driven by each wheel on the robot.
+   *
+   * @param start The starting distances driven by the wheels.
+   * @param end The ending distances driven by the wheels.
+   *
+   * @return The resulting Twist2d in the robot's movement.
+   */
+  virtual Twist2d ToTwist2d(const WheelPositions& start,
+                            const WheelPositions& end) const = 0;
+
+  /**
+   * Performs interpolation between two values.
+   *
+   * @param start The value to start at.
+   * @param end The value to end at.
+   * @param t How far between the two values to interpolate. This should be
+   * bounded to [0, 1].
+   * @return The interpolated value.
+   */
+  virtual WheelPositions Interpolate(const WheelPositions& start,
+                                     const WheelPositions& end,
+                                     double t) const = 0;
+};
+}  // namespace frc