[wpimath] Add 3D odometry and pose estimation (#7119)

wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h

@@ -89,9 +89,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
    */
   void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
                      units::meter_t rightDistance, const Pose2d& pose) {
-    PoseEstimator<DifferentialDriveWheelSpeeds,
-                  DifferentialDriveWheelPositions>::
-        ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose);
+    PoseEstimator::ResetPosition(gyroAngle, {leftDistance, rightDistance},
+                                 pose);
   }
 
   /**
@@ -106,10 +105,7 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
    */
   Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
                 units::meter_t rightDistance) {
-    return PoseEstimator<
-        DifferentialDriveWheelSpeeds,
-        DifferentialDriveWheelPositions>::Update(gyroAngle,
-                                                 {leftDistance, rightDistance});
+    return PoseEstimator::Update(gyroAngle, {leftDistance, rightDistance});
   }
 
   /**
@@ -127,11 +123,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
                         const Rotation2d& gyroAngle,
                         units::meter_t leftDistance,
                         units::meter_t rightDistance) {
-    return PoseEstimator<
-        DifferentialDriveWheelSpeeds,
-        DifferentialDriveWheelPositions>::UpdateWithTime(currentTime, gyroAngle,
-                                                         {leftDistance,
-                                                          rightDistance});
+    return PoseEstimator::UpdateWithTime(currentTime, gyroAngle,
+                                         {leftDistance, rightDistance});
   }
 
  private:

wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h

@@ -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/frc/estimator/MecanumDrivePoseEstimator3d.h

@@ -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/frc/estimator/PoseEstimator3d.h

@@ -0,0 +1,451 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source 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 = visionSample.value().Log(visionRobotPose);
+
+    // 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->Exp(scaledTwist), *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/frc/estimator/SwerveDrivePoseEstimator.h

@@ -82,8 +82,7 @@ class SwerveDrivePoseEstimator
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
       const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
       const wpi::array<double, 3>& visionMeasurementStdDevs)
-      : PoseEstimator<wpi::array<SwerveModuleState, NumModules>,
-                      wpi::array<SwerveModulePosition, NumModules>>(
+      : SwerveDrivePoseEstimator::PoseEstimator(
             kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
         m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {
     this->ResetPose(initialPose);

wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h

@@ -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/frc/kinematics/DifferentialDriveOdometry.h

@@ -62,11 +62,7 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry
    */
   void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
                      units::meter_t rightDistance, const Pose2d& pose) {
-    Odometry<DifferentialDriveWheelSpeeds,
-             DifferentialDriveWheelPositions>::ResetPosition(gyroAngle,
-                                                             {leftDistance,
-                                                              rightDistance},
-                                                             pose);
+    Odometry::ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose);
   }
 
   /**
@@ -82,10 +78,7 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry
    */
   const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
                        units::meter_t rightDistance) {
-    return Odometry<DifferentialDriveWheelSpeeds,
-                    DifferentialDriveWheelPositions>::Update(gyroAngle,
-                                                             {leftDistance,
-                                                              rightDistance});
+    return Odometry::Update(gyroAngle, {leftDistance, rightDistance});
   }
 
  private:

wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h

@@ -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/frc/kinematics/MecanumDriveOdometry3d.h

@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/timestamp.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveWheelPositions.h"
+#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+#include "frc/kinematics/Odometry3d.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Class for mecanum drive odometry. Odometry allows you to track the robot's
+ * position on the field over a course of a match using readings from your
+ * mecanum wheel encoders.
+ *
+ * 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.
+ */
+class WPILIB_DLLEXPORT MecanumDriveOdometry3d
+    : public Odometry3d<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
+ public:
+  /**
+   * Constructs a MecanumDriveOdometry3d object.
+   *
+   * @param kinematics The mecanum drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current distances measured by each wheel.
+   * @param initialPose The starting position of the robot on the field.
+   */
+  explicit MecanumDriveOdometry3d(
+      MecanumDriveKinematics kinematics, const Rotation3d& gyroAngle,
+      const MecanumDriveWheelPositions& wheelPositions,
+      const Pose3d& initialPose = Pose3d{});
+
+ private:
+  MecanumDriveKinematics m_kinematicsImpl;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/kinematics/Odometry3d.h

@@ -0,0 +1,153 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Rotation3d.h"
+#include "frc/geometry/Translation2d.h"
+#include "frc/geometry/Translation3d.h"
+#include "frc/kinematics/Kinematics.h"
+
+namespace frc {
+
+/**
+ * Class for odometry. Robot code should not use this directly- Instead, use the
+ * particular type for your drivetrain (e.g., DifferentialDriveOdometry3d).
+ * Odometry allows you to track the robot's position on the field over a course
+ * of a match using readings from your wheel encoders.
+ *
+ * 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.
+ *
+ * @tparam WheelSpeeds Wheel speeds type.
+ * @tparam WheelPositions Wheel positions type.
+ */
+template <typename WheelSpeeds, typename WheelPositions>
+class WPILIB_DLLEXPORT Odometry3d {
+ public:
+  /**
+   * Constructs an Odometry3d object.
+   *
+   * @param kinematics The kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current distances measured by each wheel.
+   * @param initialPose The starting position of the robot on the field.
+   */
+  explicit Odometry3d(const Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+                      const Rotation3d& gyroAngle,
+                      const WheelPositions& wheelPositions,
+                      const Pose3d& initialPose = Pose3d{})
+      : m_kinematics(kinematics),
+        m_pose(initialPose),
+        m_previousWheelPositions(wheelPositions) {
+    m_previousAngle = m_pose.Rotation();
+    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * The gyroscope angle does not need to be reset here on the user's robot
+   * code. The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current distances measured by each wheel.
+   * @param pose The position on the field that your robot is at.
+   */
+  void ResetPosition(const Rotation3d& gyroAngle,
+                     const WheelPositions& wheelPositions, const Pose3d& pose) {
+    m_pose = pose;
+    m_previousAngle = pose.Rotation();
+    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    m_previousWheelPositions = wheelPositions;
+  }
+
+  /**
+   * Resets the pose.
+   *
+   * @param pose The pose to reset to.
+   */
+  void ResetPose(const Pose3d& pose) {
+    m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation());
+    m_pose = pose;
+    m_previousAngle = pose.Rotation();
+  }
+
+  /**
+   * Resets the translation of the pose.
+   *
+   * @param translation The translation to reset to.
+   */
+  void ResetTranslation(const Translation3d& translation) {
+    m_pose = Pose3d{translation, m_pose.Rotation()};
+  }
+
+  /**
+   * Resets the rotation of the pose.
+   *
+   * @param rotation The rotation to reset to.
+   */
+  void ResetRotation(const Rotation3d& rotation) {
+    m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation());
+    m_pose = Pose3d{m_pose.Translation(), rotation};
+    m_previousAngle = rotation;
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   * @return The pose of the robot.
+   */
+  const Pose3d& GetPose() const { return m_pose; }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose over time. This method takes in an angle parameter
+   * which is used instead of the angular rate that is calculated from forward
+   * kinematics, in addition to the current distance measurement at each wheel.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current distances measured by each wheel.
+   *
+   * @return The new pose of the robot.
+   */
+  const Pose3d& Update(const Rotation3d& gyroAngle,
+                       const WheelPositions& wheelPositions) {
+    auto angle = gyroAngle + m_gyroOffset;
+    auto angle_difference =
+        (angle - m_previousAngle).GetQuaternion().ToRotationVector();
+
+    auto twist2d =
+        m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);
+    Twist3d twist{twist2d.dx,
+                  twist2d.dy,
+                  0_m,
+                  units::radian_t{angle_difference(0)},
+                  units::radian_t{angle_difference(1)},
+                  units::radian_t{angle_difference(2)}};
+
+    auto newPose = m_pose.Exp(twist);
+
+    m_previousWheelPositions = wheelPositions;
+    m_previousAngle = angle;
+    m_pose = {newPose.Translation(), angle};
+
+    return m_pose;
+  }
+
+ private:
+  const Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
+  Pose3d m_pose;
+
+  WheelPositions m_previousWheelPositions;
+  Rotation3d m_previousAngle;
+  Rotation3d m_gyroOffset;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h

@@ -45,9 +45,8 @@ class SwerveDriveOdometry
       SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
       const Pose2d& initialPose = Pose2d{})
-      : Odometry<wpi::array<SwerveModuleState, NumModules>,
-                 wpi::array<SwerveModulePosition, NumModules>>(
-            m_kinematicsImpl, gyroAngle, modulePositions, initialPose),
+      : SwerveDriveOdometry::Odometry(m_kinematicsImpl, gyroAngle,
+                                      modulePositions, initialPose),
         m_kinematicsImpl(kinematics) {
     wpi::math::MathSharedStore::ReportUsage(
         wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);

wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.h

@@ -0,0 +1,70 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <chrono>
+#include <cstddef>
+#include <ctime>
+
+#include <wpi/SymbolExports.h>
+#include <wpi/timestamp.h>
+
+#include "Odometry3d.h"
+#include "SwerveDriveKinematics.h"
+#include "SwerveModulePosition.h"
+#include "SwerveModuleState.h"
+#include "frc/geometry/Pose2d.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Class for swerve drive odometry. Odometry allows you to track the robot's
+ * position on the field over a course of a match using readings from your
+ * swerve drive encoders and swerve azimuth encoders.
+ *
+ * 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.
+ */
+template <size_t NumModules>
+class SwerveDriveOdometry3d
+    : public Odometry3d<wpi::array<SwerveModuleState, NumModules>,
+                        wpi::array<SwerveModulePosition, NumModules>> {
+ public:
+  /**
+   * Constructs a SwerveDriveOdometry3d object.
+   *
+   * @param kinematics The swerve drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param modulePositions The wheel positions reported by each module.
+   * @param initialPose The starting position of the robot on the field.
+   */
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif  // defined(__GNUC__) && !defined(__clang__)
+  SwerveDriveOdometry3d(
+      SwerveDriveKinematics<NumModules> kinematics, const Rotation3d& gyroAngle,
+      const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
+      const Pose3d& initialPose = Pose3d{})
+      : SwerveDriveOdometry3d::Odometry3d(m_kinematicsImpl, gyroAngle,
+                                          modulePositions, initialPose),
+        m_kinematicsImpl(kinematics) {
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
+  }
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif  // defined(__GNUC__) && !defined(__clang__)
+
+ private:
+  SwerveDriveKinematics<NumModules> m_kinematicsImpl;
+};
+
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    SwerveDriveOdometry3d<4>;
+
+}  // namespace frc