[wpilib] Add pose estimators (#2867)

Pose and state estimators can filter latency-compensated global measurements and fuse them with state-space drivetrain model information to estimate robot position. They are drop-in replacements for the existing odometry classes.

Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>

wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp

@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Drivetrain.h"
+
+#include <frc2/Timer.h>
+
+#include "ExampleGlobalMeasurementSensor.h"
+
+void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
+  const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
+  const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
+  const double leftOutput = m_leftPIDController.Calculate(
+      m_leftEncoder.GetRate(), speeds.left.to<double>());
+  const double rightOutput = m_rightPIDController.Calculate(
+      m_rightEncoder.GetRate(), speeds.right.to<double>());
+
+  m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
+  m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
+}
+
+void Drivetrain::Drive(units::meters_per_second_t xSpeed,
+                       units::radians_per_second_t rot) {
+  SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
+}
+
+void Drivetrain::UpdateOdometry() {
+  m_poseEstimator.Update(m_gyro.GetRotation2d(),
+                         {units::meters_per_second_t(m_leftEncoder.GetRate()),
+                          units::meters_per_second_t(m_rightEncoder.GetRate())},
+                         units::meter_t(m_leftEncoder.GetDistance()),
+                         units::meter_t(m_rightEncoder.GetDistance()));
+
+  // Also apply vision measurements. We use 0.3 seconds in the past as an
+  // example -- on a real robot, this must be calculated based either on latency
+  // or timestamps.
+  m_poseEstimator.AddVisionMeasurement(
+      ExampleGlobalMeasurementSensor::GetEstimatedGlobalPose(
+          m_poseEstimator.GetEstimatedPosition()),
+      frc2::Timer::GetFPGATimestamp() - 0.3_s);
+}

wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Robot.cpp

@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/SlewRateLimiter.h>
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+
+#include "Drivetrain.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void AutonomousPeriodic() override {
+    TeleopPeriodic();
+    m_drive.UpdateOdometry();
+  }
+
+  void TeleopPeriodic() override {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    const auto xSpeed = -m_speedLimiter.Calculate(
+                            m_controller.GetY(frc::GenericHID::kLeftHand)) *
+                        Drivetrain::kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    const auto rot = -m_rotLimiter.Calculate(
+                         m_controller.GetX(frc::GenericHID::kRightHand)) *
+                     Drivetrain::kMaxAngularSpeed;
+
+    m_drive.Drive(xSpeed, rot);
+  }
+
+ private:
+  frc::XboxController m_controller{0};
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
+  // to 1.
+  frc::SlewRateLimiter<units::scalar> m_speedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
+
+  Drivetrain m_drive;
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h

@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/AnalogGyro.h>
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/estimator/DifferentialDrivePoseEstimator.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/velocity.h>
+#include <wpi/math>
+
+/**
+ * Represents a differential drive style drivetrain.
+ */
+class Drivetrain {
+ public:
+  Drivetrain() {
+    m_gyro.Reset();
+    // Set the distance per pulse for the drive encoders. We can simply use the
+    // distance traveled for one rotation of the wheel divided by the encoder
+    // resolution.
+    m_leftEncoder.SetDistancePerPulse(
+        2 * wpi::math::pi * kWheelRadius.to<double>() / kEncoderResolution);
+    m_rightEncoder.SetDistancePerPulse(
+        2 * wpi::math::pi * kWheelRadius.to<double>() / kEncoderResolution);
+
+    m_leftEncoder.Reset();
+    m_rightEncoder.Reset();
+  }
+
+  static constexpr units::meters_per_second_t kMaxSpeed =
+      3.0_mps;  // 3 meters per second
+  static constexpr units::radians_per_second_t kMaxAngularSpeed{
+      wpi::math::pi};  // 1/2 rotation per second
+
+  void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
+  void Drive(units::meters_per_second_t xSpeed,
+             units::radians_per_second_t rot);
+  void UpdateOdometry();
+
+ private:
+  static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
+  static constexpr units::meter_t kWheelRadius = 0.0508_m;
+  static constexpr int kEncoderResolution = 4096;
+
+  frc::PWMVictorSPX m_leftLeader{1};
+  frc::PWMVictorSPX m_leftFollower{2};
+  frc::PWMVictorSPX m_rightLeader{3};
+  frc::PWMVictorSPX m_rightFollower{4};
+
+  frc::SpeedControllerGroup m_leftGroup{m_leftLeader, m_leftFollower};
+  frc::SpeedControllerGroup m_rightGroup{m_rightLeader, m_rightFollower};
+
+  frc::Encoder m_leftEncoder{0, 1};
+  frc::Encoder m_rightEncoder{2, 3};
+
+  frc2::PIDController m_leftPIDController{1.0, 0.0, 0.0};
+  frc2::PIDController m_rightPIDController{1.0, 0.0, 0.0};
+
+  frc::AnalogGyro m_gyro{0};
+
+  frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::DifferentialDrivePoseEstimator m_poseEstimator{
+      frc::Rotation2d(),
+      frc::Pose2d(),
+      {0.01, 0.01, 0.01, 0.01, 0.01},
+      {0.1, 0.1, 0.1},
+      {0.1, 0.1, 0.1}};
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
+};

wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/StateSpaceUtil.h>
+#include <frc/geometry/Pose2d.h>
+
+/**
+ * This dummy class represents a global measurement sensor, such as a computer
+ * vision solution.
+ */
+class ExampleGlobalMeasurementSensor {
+ public:
+  static frc::Pose2d GetEstimatedGlobalPose(
+      const frc::Pose2d& estimatedRobotPose) {
+    auto randVec = frc::MakeWhiteNoiseVector(0.1, 0.1, 0.1);
+    return frc::Pose2d(estimatedRobotPose.X() + units::meter_t(randVec(0)),
+                       estimatedRobotPose.Y() + units::meter_t(randVec(1)),
+                       estimatedRobotPose.Rotation() +
+                           frc::Rotation2d(units::radian_t(randVec(3))));
+  }
+};

wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp

@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Drivetrain.h"
+
+#include <frc2/Timer.h>
+
+#include "ExampleGlobalMeasurementSensor.h"
+
+frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
+  return {units::meters_per_second_t(m_frontLeftEncoder.GetRate()),
+          units::meters_per_second_t(m_frontRightEncoder.GetRate()),
+          units::meters_per_second_t(m_backLeftEncoder.GetRate()),
+          units::meters_per_second_t(m_backRightEncoder.GetRate())};
+}
+
+void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) {
+  std::function<void(units::meters_per_second_t, const frc::Encoder&,
+                     frc2::PIDController&, frc::PWMVictorSPX&)>
+      calcAndSetSpeeds =
+          [&m_feedforward = m_feedforward](units::meters_per_second_t speed,
+                                           const auto& encoder,
+                                           auto& controller, auto& motor) {
+            auto feedforward = m_feedforward.Calculate(speed);
+            double output =
+                controller.Calculate(encoder.GetRate(), speed.to<double>());
+            motor.SetVoltage(units::volt_t{output} + feedforward);
+          };
+
+  calcAndSetSpeeds(wheelSpeeds.frontLeft, m_frontLeftEncoder,
+                   m_frontLeftPIDController, m_frontLeftMotor);
+  calcAndSetSpeeds(wheelSpeeds.frontRight, m_frontRightEncoder,
+                   m_frontRightPIDController, m_frontRightMotor);
+  calcAndSetSpeeds(wheelSpeeds.rearLeft, m_backLeftEncoder,
+                   m_backLeftPIDController, m_backLeftMotor);
+  calcAndSetSpeeds(wheelSpeeds.rearRight, m_backRightEncoder,
+                   m_backRightPIDController, m_backRightMotor);
+}
+
+void Drivetrain::Drive(units::meters_per_second_t xSpeed,
+                       units::meters_per_second_t ySpeed,
+                       units::radians_per_second_t rot, bool fieldRelative) {
+  auto wheelSpeeds = m_kinematics.ToWheelSpeeds(
+      fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+                          xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
+                    : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
+  wheelSpeeds.Normalize(kMaxSpeed);
+  SetSpeeds(wheelSpeeds);
+}
+
+void Drivetrain::UpdateOdometry() {
+  m_poseEstimator.Update(m_gyro.GetRotation2d(), GetCurrentState());
+
+  // Also apply vision measurements. We use 0.3 seconds in the past as an
+  // example -- on a real robot, this must be calculated based either on latency
+  // or timestamps.
+  m_poseEstimator.AddVisionMeasurement(
+      ExampleGlobalMeasurementSensor::GetEstimatedGlobalPose(
+          m_poseEstimator.GetEstimatedPosition()),
+      frc2::Timer::GetFPGATimestamp() - 0.3_s);
+}

wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Robot.cpp

@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/SlewRateLimiter.h>
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+
+#include "Drivetrain.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void AutonomousPeriodic() override {
+    DriveWithJoystick(false);
+    m_mecanum.UpdateOdometry();
+  }
+
+  void TeleopPeriodic() override { DriveWithJoystick(true); }
+
+ private:
+  frc::XboxController m_controller{0};
+  Drivetrain m_mecanum;
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
+  // to 1.
+  frc::SlewRateLimiter<units::scalar> m_xspeedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_yspeedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
+
+  void DriveWithJoystick(bool fieldRelative) {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    const auto xSpeed = -m_xspeedLimiter.Calculate(
+                            m_controller.GetY(frc::GenericHID::kLeftHand)) *
+                        Drivetrain::kMaxSpeed;
+
+    // Get the y speed or sideways/strafe speed. We are inverting this because
+    // we want a positive value when we pull to the left. Xbox controllers
+    // return positive values when you pull to the right by default.
+    const auto ySpeed = -m_yspeedLimiter.Calculate(
+                            m_controller.GetX(frc::GenericHID::kLeftHand)) *
+                        Drivetrain::kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    const auto rot = -m_rotLimiter.Calculate(
+                         m_controller.GetX(frc::GenericHID::kRightHand)) *
+                     Drivetrain::kMaxAngularSpeed;
+
+    m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative);
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h

@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/AnalogGyro.h>
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/estimator/MecanumDrivePoseEstimator.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/MecanumDriveKinematics.h>
+#include <frc/kinematics/MecanumDriveOdometry.h>
+#include <frc/kinematics/MecanumDriveWheelSpeeds.h>
+#include <wpi/math>
+
+/**
+ * Represents a mecanum drive style drivetrain.
+ */
+class Drivetrain {
+ public:
+  Drivetrain() { m_gyro.Reset(); }
+
+  frc::MecanumDriveWheelSpeeds GetCurrentState() const;
+  void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds);
+  void Drive(units::meters_per_second_t xSpeed,
+             units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
+             bool fieldRelative);
+  void UpdateOdometry();
+
+  static constexpr auto kMaxSpeed = 3.0_mps;  // 3 meters per second
+  static constexpr units::radians_per_second_t kMaxAngularSpeed{
+      wpi::math::pi};  // 1/2 rotation per second
+
+ private:
+  frc::PWMVictorSPX m_frontLeftMotor{1};
+  frc::PWMVictorSPX m_frontRightMotor{2};
+  frc::PWMVictorSPX m_backLeftMotor{3};
+  frc::PWMVictorSPX m_backRightMotor{4};
+
+  frc::Encoder m_frontLeftEncoder{0, 1};
+  frc::Encoder m_frontRightEncoder{2, 3};
+  frc::Encoder m_backLeftEncoder{4, 5};
+  frc::Encoder m_backRightEncoder{6, 7};
+
+  frc::Translation2d m_frontLeftLocation{0.381_m, 0.381_m};
+  frc::Translation2d m_frontRightLocation{0.381_m, -0.381_m};
+  frc::Translation2d m_backLeftLocation{-0.381_m, 0.381_m};
+  frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m};
+
+  frc2::PIDController m_frontLeftPIDController{1.0, 0.0, 0.0};
+  frc2::PIDController m_frontRightPIDController{1.0, 0.0, 0.0};
+  frc2::PIDController m_backLeftPIDController{1.0, 0.0, 0.0};
+  frc2::PIDController m_backRightPIDController{1.0, 0.0, 0.0};
+
+  frc::AnalogGyro m_gyro{0};
+
+  frc::MecanumDriveKinematics m_kinematics{
+      m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
+      m_backRightLocation};
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::MecanumDrivePoseEstimator m_poseEstimator{
+      frc::Rotation2d(), frc::Pose2d(), m_kinematics,
+      {0.1, 0.1, 0.1},   {0.05},        {0.1, 0.1, 0.1}};
+};

wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/StateSpaceUtil.h>
+#include <frc/geometry/Pose2d.h>
+
+/**
+ * This dummy class represents a global measurement sensor, such as a computer
+ * vision solution.
+ */
+class ExampleGlobalMeasurementSensor {
+ public:
+  static frc::Pose2d GetEstimatedGlobalPose(
+      const frc::Pose2d& estimatedRobotPose) {
+    auto randVec = frc::MakeWhiteNoiseVector(0.1, 0.1, 0.1);
+    return frc::Pose2d(estimatedRobotPose.X() + units::meter_t(randVec(0)),
+                       estimatedRobotPose.Y() + units::meter_t(randVec(1)),
+                       estimatedRobotPose.Rotation() +
+                           frc::Rotation2d(units::radian_t(randVec(3))));
+  }
+};

wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp

@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Drivetrain.h"
+
+#include <frc2/Timer.h>
+
+#include "ExampleGlobalMeasurementSensor.h"
+
+void Drivetrain::Drive(units::meters_per_second_t xSpeed,
+                       units::meters_per_second_t ySpeed,
+                       units::radians_per_second_t rot, bool fieldRelative) {
+  auto states = m_kinematics.ToSwerveModuleStates(
+      fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+                          xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
+                    : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
+
+  m_kinematics.NormalizeWheelSpeeds(&states, kMaxSpeed);
+
+  auto [fl, fr, bl, br] = states;
+
+  m_frontLeft.SetDesiredState(fl);
+  m_frontRight.SetDesiredState(fr);
+  m_backLeft.SetDesiredState(bl);
+  m_backRight.SetDesiredState(br);
+}
+
+void Drivetrain::UpdateOdometry() {
+  m_poseEstimator.Update(m_gyro.GetRotation2d(), m_frontLeft.GetState(),
+                         m_frontRight.GetState(), m_backLeft.GetState(),
+                         m_backRight.GetState());
+
+  // Also apply vision measurements. We use 0.3 seconds in the past as an
+  // example -- on a real robot, this must be calculated based either on latency
+  // or timestamps.
+  m_poseEstimator.AddVisionMeasurement(
+      ExampleGlobalMeasurementSensor::GetEstimatedGlobalPose(
+          m_poseEstimator.GetEstimatedPosition()),
+      frc2::Timer::GetFPGATimestamp() - 0.3_s);
+}

wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Robot.cpp

@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/SlewRateLimiter.h>
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+
+#include "Drivetrain.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void AutonomousPeriodic() override {
+    DriveWithJoystick(false);
+    m_swerve.UpdateOdometry();
+  }
+
+  void TeleopPeriodic() override { DriveWithJoystick(true); }
+
+ private:
+  frc::XboxController m_controller{0};
+  Drivetrain m_swerve;
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
+  // to 1.
+  frc::SlewRateLimiter<units::scalar> m_xspeedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_yspeedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
+
+  void DriveWithJoystick(bool fieldRelative) {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    const auto xSpeed = -m_xspeedLimiter.Calculate(
+                            m_controller.GetY(frc::GenericHID::kLeftHand)) *
+                        Drivetrain::kMaxSpeed;
+
+    // Get the y speed or sideways/strafe speed. We are inverting this because
+    // we want a positive value when we pull to the left. Xbox controllers
+    // return positive values when you pull to the right by default.
+    const auto ySpeed = -m_yspeedLimiter.Calculate(
+                            m_controller.GetX(frc::GenericHID::kLeftHand)) *
+                        Drivetrain::kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    const auto rot = -m_rotLimiter.Calculate(
+                         m_controller.GetX(frc::GenericHID::kRightHand)) *
+                     Drivetrain::kMaxAngularSpeed;
+
+    m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative);
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp

@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "SwerveModule.h"
+
+#include <frc/geometry/Rotation2d.h>
+#include <wpi/math>
+
+SwerveModule::SwerveModule(const int driveMotorChannel,
+                           const int turningMotorChannel)
+    : m_driveMotor(driveMotorChannel), m_turningMotor(turningMotorChannel) {
+  // Set the distance per pulse for the drive encoder. We can simply use the
+  // distance traveled for one rotation of the wheel divided by the encoder
+  // resolution.
+  m_driveEncoder.SetDistancePerPulse(
+      2 * wpi::math::pi * kWheelRadius.to<double>() / kEncoderResolution);
+
+  // Set the distance (in this case, angle) per pulse for the turning encoder.
+  // This is the the angle through an entire rotation (2 * wpi::math::pi)
+  // divided by the encoder resolution.
+  m_turningEncoder.SetDistancePerPulse(2 * wpi::math::pi / kEncoderResolution);
+
+  // Limit the PID Controller's input range between -pi and pi and set the input
+  // to be continuous.
+  m_turningPIDController.EnableContinuousInput(-units::radian_t(wpi::math::pi),
+                                               units::radian_t(wpi::math::pi));
+}
+
+frc::SwerveModuleState SwerveModule::GetState() const {
+  return {units::meters_per_second_t{m_driveEncoder.GetRate()},
+          frc::Rotation2d(units::radian_t(m_turningEncoder.Get()))};
+}
+
+void SwerveModule::SetDesiredState(const frc::SwerveModuleState& state) {
+  // Calculate the drive output from the drive PID controller.
+  const auto driveOutput = m_drivePIDController.Calculate(
+      m_driveEncoder.GetRate(), state.speed.to<double>());
+
+  const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed);
+
+  // Calculate the turning motor output from the turning PID controller.
+  const auto turnOutput = m_turningPIDController.Calculate(
+      units::radian_t(m_turningEncoder.Get()), state.angle.Radians());
+
+  const auto turnFeedforward = m_turnFeedforward.Calculate(
+      m_turningPIDController.GetSetpoint().velocity);
+
+  // Set the motor outputs.
+  m_driveMotor.SetVoltage(units::volt_t{driveOutput} + driveFeedforward);
+  m_turningMotor.SetVoltage(units::volt_t{turnOutput} + turnFeedforward);
+}

wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h

@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/AnalogGyro.h>
+#include <frc/estimator/SwerveDrivePoseEstimator.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/kinematics/SwerveDriveOdometry.h>
+#include <wpi/math>
+
+#include "SwerveModule.h"
+
+/**
+ * Represents a swerve drive style drivetrain.
+ */
+class Drivetrain {
+ public:
+  Drivetrain() { m_gyro.Reset(); }
+
+  void Drive(units::meters_per_second_t xSpeed,
+             units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
+             bool fieldRelative);
+  void UpdateOdometry();
+
+  static constexpr auto kMaxSpeed = 3.0_mps;  // 3 meters per second
+  static constexpr units::radians_per_second_t kMaxAngularSpeed{
+      wpi::math::pi};  // 1/2 rotation per second
+
+ private:
+  frc::Translation2d m_frontLeftLocation{+0.381_m, +0.381_m};
+  frc::Translation2d m_frontRightLocation{+0.381_m, -0.381_m};
+  frc::Translation2d m_backLeftLocation{-0.381_m, +0.381_m};
+  frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m};
+
+  SwerveModule m_frontLeft{1, 2};
+  SwerveModule m_frontRight{2, 3};
+  SwerveModule m_backLeft{5, 6};
+  SwerveModule m_backRight{7, 8};
+
+  frc::AnalogGyro m_gyro{0};
+
+  frc::SwerveDriveKinematics<4> m_kinematics{
+      m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
+      m_backRightLocation};
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::SwerveDrivePoseEstimator<4> m_poseEstimator{
+      frc::Rotation2d(), frc::Pose2d(), m_kinematics,
+      {0.1, 0.1, 0.1},   {0.05},        {0.1, 0.1, 0.1}};
+};

wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/StateSpaceUtil.h>
+#include <frc/geometry/Pose2d.h>
+
+/**
+ * This dummy class represents a global measurement sensor, such as a computer
+ * vision solution.
+ */
+class ExampleGlobalMeasurementSensor {
+ public:
+  static frc::Pose2d GetEstimatedGlobalPose(
+      const frc::Pose2d& estimatedRobotPose) {
+    auto randVec = frc::MakeWhiteNoiseVector(0.1, 0.1, 0.1);
+    return frc::Pose2d(estimatedRobotPose.X() + units::meter_t(randVec(0)),
+                       estimatedRobotPose.Y() + units::meter_t(randVec(1)),
+                       estimatedRobotPose.Rotation() +
+                           frc::Rotation2d(units::radian_t(randVec(3))));
+  }
+};

wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h

@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <units/angular_velocity.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
+#include <wpi/math>
+
+class SwerveModule {
+ public:
+  SwerveModule(int driveMotorChannel, int turningMotorChannel);
+  frc::SwerveModuleState GetState() const;
+  void SetDesiredState(const frc::SwerveModuleState& state);
+
+ private:
+  static constexpr auto kWheelRadius = 0.0508_m;
+  static constexpr int kEncoderResolution = 4096;
+
+  static constexpr auto kModuleMaxAngularVelocity =
+      wpi::math::pi * 1_rad_per_s;  // radians per second
+  static constexpr auto kModuleMaxAngularAcceleration =
+      wpi::math::pi * 2_rad_per_s / 1_s;  // radians per second^2
+
+  frc::PWMVictorSPX m_driveMotor;
+  frc::PWMVictorSPX m_turningMotor;
+
+  frc::Encoder m_driveEncoder{0, 1};
+  frc::Encoder m_turningEncoder{2, 3};
+
+  frc2::PIDController m_drivePIDController{1.0, 0, 0};
+  frc::ProfiledPIDController<units::radians> m_turningPIDController{
+      1.0,
+      0.0,
+      0.0,
+      {kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}};
+
+  frc::SimpleMotorFeedforward<units::meters> m_driveFeedforward{1_V,
+                                                                3_V / 1_mps};
+  frc::SimpleMotorFeedforward<units::radians> m_turnFeedforward{
+      1_V, 0.5_V / 1_rad_per_s};
+};

wpilibcExamples/src/main/cpp/examples/examples.json

@@ -628,6 +628,38 @@
     "mainclass": "Main",
     "commandversion": 2
   },
+  {
+    "name": "DifferentialDrivePoseEstimator",
+    "description": "Demonstrates the use of the DifferentialDrivePoseEstimator as a replacement for differential drive odometry.",
+    "tags": [
+      "Drivetrain",
+      "State space",
+      "Vision",
+      "Filter",
+      "Odometry",
+      "Pose"
+    ],
+    "foldername": "DifferentialDrivePoseEstimator",
+    "gradlebase": "cpp",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "MecanumDrivePoseEstimator",
+    "description": "Demonstrates the use of the MecanumDrivePoseEstimator as a replacement for mecanum odometry.",
+    "tags": [
+      "Drivetrain",
+      "State space",
+      "Vision",
+      "Filter",
+      "Odometry",
+      "Pose"
+    ],
+    "foldername": "MecanumDrivePoseEstimator",
+    "gradlebase": "cpp",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
   {
     "name": "ArmSimulation",
     "description": "Demonstrates the use of physics simulation with a simple single-jointed arm.",
@@ -661,5 +693,22 @@
     "gradlebase": "cpp",
     "mainclass": "Main",
     "commandversion": 2
+  },
+  {
+    "name": "SwerveDrivePoseEstimator",
+    "description": "Demonstrates the use of the SwerveDrivePoseEstimator as a replacement for mecanum drive odometry.",
+    "tags": [
+      "Drivetrain",
+      "State space",
+      "Vision",
+      "Filter",
+      "Odometry",
+      "State",
+      "Swerve"
+    ],
+    "foldername": "SwerveDrivePoseEstimator",
+    "gradlebase": "cpp",
+    "mainclass": "Main",
+    "commandversion": 2
   }
 ]

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java

@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.estimator.DifferentialDrivePoseEstimator;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+
+import edu.wpi.first.wpilibj.examples.swervesdriveposeestimator.ExampleGlobalMeasurementSensor;
+
+/**
+ * Represents a differential drive style drivetrain.
+ */
+public class Drivetrain {
+  public static final double kMaxSpeed = 3.0; // meters per second
+  public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
+
+  private static final double kTrackWidth = 0.381 * 2; // meters
+  private static final double kWheelRadius = 0.0508; // meters
+  private static final int kEncoderResolution = 4096;
+
+  private final SpeedController m_leftLeader = new PWMVictorSPX(1);
+  private final SpeedController m_leftFollower = new PWMVictorSPX(2);
+  private final SpeedController m_rightLeader = new PWMVictorSPX(3);
+  private final SpeedController m_rightFollower = new PWMVictorSPX(4);
+
+  private final Encoder m_leftEncoder = new Encoder(0, 1);
+  private final Encoder m_rightEncoder = new Encoder(2, 3);
+
+  private final SpeedControllerGroup m_leftGroup
+      = new SpeedControllerGroup(m_leftLeader, m_leftFollower);
+  private final SpeedControllerGroup m_rightGroup
+      = new SpeedControllerGroup(m_rightLeader, m_rightFollower);
+
+  private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+  private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
+
+  private final DifferentialDriveKinematics m_kinematics
+      = new DifferentialDriveKinematics(kTrackWidth);
+
+  /* Here we use DifferentialDrivePoseEstimator so that we can fuse odometry readings. The
+  numbers used  below are robot specific, and should be tuned. */
+  private final DifferentialDrivePoseEstimator m_poseEstimator = new DifferentialDrivePoseEstimator(
+      m_gyro.getRotation2d(),
+      new Pose2d(),
+      VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.01, 0.01),
+      VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(1)),
+      VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+
+  // Gains are for example purposes only - must be determined for your own robot!
+  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
+
+  /**
+   * Constructs a differential drive object.
+   * Sets the encoder distance per pulse and resets the gyro.
+   */
+  public Drivetrain() {
+    m_gyro.reset();
+
+    // Set the distance per pulse for the drive encoders. We can simply use the
+    // distance traveled for one rotation of the wheel divided by the encoder
+    // resolution.
+    m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+    m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  /**
+   * Sets the desired wheel speeds.
+   *
+   * @param speeds The desired wheel speeds.
+   */
+  public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
+    final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
+    final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
+
+    final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(),
+        speeds.leftMetersPerSecond);
+    final double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(),
+        speeds.rightMetersPerSecond);
+    m_leftGroup.setVoltage(leftOutput + leftFeedforward);
+    m_rightGroup.setVoltage(rightOutput + rightFeedforward);
+  }
+
+  /**
+   * Drives the robot with the given linear velocity and angular velocity.
+   *
+   * @param xSpeed Linear velocity in m/s.
+   * @param rot    Angular velocity in rad/s.
+   */
+  @SuppressWarnings("ParameterName")
+  public void drive(double xSpeed, double rot) {
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot));
+    setSpeeds(wheelSpeeds);
+  }
+
+  /**
+   * Updates the field-relative position.
+   */
+  public void updateOdometry() {
+    m_poseEstimator.update(m_gyro.getRotation2d(),
+        new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()),
+        m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
+
+    // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
+    // a real robot, this must be calculated based either on latency or timestamps.
+    m_poseEstimator.addVisionMeasurement(
+        ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
+            m_poseEstimator.getEstimatedPosition()),
+        Timer.getFPGATimestamp() - 0.3);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Main.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java

@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.SlewRateLimiter;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class Robot extends TimedRobot {
+  private final XboxController m_controller = new XboxController(0);
+  private final Drivetrain m_drive = new Drivetrain();
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+  private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
+
+  @Override
+  public void autonomousPeriodic() {
+    teleopPeriodic();
+    m_drive.updateOdometry();
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    final var xSpeed =
+        -m_speedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
+            * Drivetrain.kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    final var rot =
+        -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
+            * Drivetrain.kMaxAngularSpeed;
+
+    m_drive.drive(xSpeed, rot);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json

@@ -680,5 +680,56 @@
     "gradlebase": "java",
     "mainclass": "Main",
     "commandversion": 2
+  },
+  {
+    "name": "DifferentialDrivePoseEstimator",
+    "description": "Demonstrates the use of the DifferentialDrivePoseEstimator as a replacement for differential drive odometry.",
+    "tags": [
+      "Drivetrain",
+      "State space",
+      "Vision",
+      "Filter",
+      "Odometry",
+      "Pose",
+      "Differential drive"
+    ],
+    "foldername": "differentialdriveposeestimator",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "MecanumDrivePoseEstimator",
+    "description": "Demonstrates the use of the MecanumDrivePoseEstimator as a replacement for mecanum drive odometry.",
+    "tags": [
+      "Drivetrain",
+      "State space",
+      "Vision",
+      "Filter",
+      "Odometry",
+      "Pose",
+      "Mecanum"
+    ],
+    "foldername": "mecanumdriveposeestimator",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "SwerveDrivePoseEstimator",
+    "description": "Demonstrates the use of the SwerveDrivePoseEstimator as a replacement for swerve drive odometry.",
+    "tags": [
+      "Drivetrain",
+      "State space",
+      "Vision",
+      "Filter",
+      "Odometry",
+      "Pose",
+      "Swerve"
+    ],
+    "foldername": "swervesdriveposeestimator",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
   }
 ]

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java

@@ -0,0 +1,158 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.estimator.MecanumDrivePoseEstimator;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+
+import edu.wpi.first.wpilibj.examples.swervesdriveposeestimator.ExampleGlobalMeasurementSensor;
+
+/**
+ * Represents a mecanum drive style drivetrain.
+ */
+@SuppressWarnings("PMD.TooManyFields")
+public class Drivetrain {
+  public static final double kMaxSpeed = 3.0; // 3 meters per second
+  public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
+
+  private final SpeedController m_frontLeftMotor = new PWMVictorSPX(1);
+  private final SpeedController m_frontRightMotor = new PWMVictorSPX(2);
+  private final SpeedController m_backLeftMotor = new PWMVictorSPX(3);
+  private final SpeedController m_backRightMotor = new PWMVictorSPX(4);
+
+  private final Encoder m_frontLeftEncoder = new Encoder(0, 1);
+  private final Encoder m_frontRightEncoder = new Encoder(2, 3);
+  private final Encoder m_backLeftEncoder = new Encoder(4, 5);
+  private final Encoder m_backRightEncoder = new Encoder(6, 7);
+
+  private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
+  private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
+  private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
+  private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
+
+  private final PIDController m_frontLeftPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_frontRightPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_backLeftPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_backRightPIDController = new PIDController(1, 0, 0);
+
+  private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+  private final MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics(
+      m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
+  );
+
+  /* Here we use MecanumDrivePoseEstimator so that we can fuse odometry readings. The numbers used
+   below are robot specific, and should be tuned. */
+  private final MecanumDrivePoseEstimator m_poseEstimator = new MecanumDrivePoseEstimator(
+      m_gyro.getRotation2d(),
+      new Pose2d(),
+      m_kinematics,
+      VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
+      VecBuilder.fill(Units.degreesToRadians(0.01)),
+      VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+
+  // Gains are for example purposes only - must be determined for your own robot!
+  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
+
+  /**
+   * Constructs a MecanumDrive and resets the gyro.
+   */
+  public Drivetrain() {
+    m_gyro.reset();
+  }
+
+  /**
+   * Returns the current state of the drivetrain.
+   *
+   * @return The current state of the drivetrain.
+   */
+  public MecanumDriveWheelSpeeds getCurrentState() {
+    return new MecanumDriveWheelSpeeds(
+        m_frontLeftEncoder.getRate(),
+        m_frontRightEncoder.getRate(),
+        m_backLeftEncoder.getRate(),
+        m_backRightEncoder.getRate()
+    );
+  }
+
+  /**
+   * Set the desired speeds for each wheel.
+   *
+   * @param speeds The desired wheel speeds.
+   */
+  public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
+    final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond);
+    final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond);
+    final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond);
+    final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond);
+
+    final double frontLeftOutput = m_frontLeftPIDController.calculate(
+        m_frontLeftEncoder.getRate(), speeds.frontLeftMetersPerSecond
+    );
+    final double frontRightOutput = m_frontRightPIDController.calculate(
+        m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond
+    );
+    final double backLeftOutput = m_backLeftPIDController.calculate(
+        m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond
+    );
+    final double backRightOutput = m_backRightPIDController.calculate(
+        m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond
+    );
+
+    m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward);
+    m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward);
+    m_backLeftMotor.setVoltage(backLeftOutput + backLeftFeedforward);
+    m_backRightMotor.setVoltage(backRightOutput + backRightFeedforward);
+  }
+
+  /**
+   * Method to drive the robot using joystick info.
+   *
+   * @param xSpeed        Speed of the robot in the x direction (forward).
+   * @param ySpeed        Speed of the robot in the y direction (sideways).
+   * @param rot           Angular rate of the robot.
+   * @param fieldRelative Whether the provided x and y speeds are relative to the field.
+   */
+  @SuppressWarnings("ParameterName")
+  public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+    var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(
+        fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
+            xSpeed, ySpeed, rot, m_gyro.getRotation2d()
+        ) : new ChassisSpeeds(xSpeed, ySpeed, rot)
+    );
+    mecanumDriveWheelSpeeds.normalize(kMaxSpeed);
+    setSpeeds(mecanumDriveWheelSpeeds);
+  }
+
+  /**
+   * Updates the field relative position of the robot.
+   */
+  public void updateOdometry() {
+    m_poseEstimator.update(m_gyro.getRotation2d(), getCurrentState());
+
+    // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
+    // a real robot, this must be calculated based either on latency or timestamps.
+    m_poseEstimator.addVisionMeasurement(
+        ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
+          m_poseEstimator.getEstimatedPosition()),
+        Timer.getFPGATimestamp() - 0.3);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/ExampleGlobalMeasurementSensor.java

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.math.StateSpaceUtil;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+
+/**
+ * This dummy class represents a global measurement sensor, such as a computer vision solution.
+ */
+public final class ExampleGlobalMeasurementSensor {
+  private ExampleGlobalMeasurementSensor() {
+    // Utility class
+  }
+
+  /**
+   * Get a "noisy" fake global pose reading.
+   *
+   * @param estimatedRobotPose The robot pose.
+   */
+  public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
+    var rand = StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5,
+        Units.degreesToRadians(30)));
+    return new Pose2d(estimatedRobotPose.getX() + rand.get(0, 0),
+        estimatedRobotPose.getY() + rand.get(1, 0),
+        estimatedRobotPose.getRotation().plus(new Rotation2d(rand.get(2, 0))));
+  }
+
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Main.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java

@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.SlewRateLimiter;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class Robot extends TimedRobot {
+  private final XboxController m_controller = new XboxController(0);
+  private final Drivetrain m_mecanum = new Drivetrain();
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+  private final SlewRateLimiter m_xspeedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_yspeedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
+  @Override
+  public void autonomousPeriodic() {
+    driveWithJoystick(false);
+    m_mecanum.updateOdometry();
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    driveWithJoystick(true);
+  }
+
+  private void driveWithJoystick(boolean fieldRelative) {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    final var xSpeed =
+        -m_xspeedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
+            * Drivetrain.kMaxSpeed;
+
+    // Get the y speed or sideways/strafe speed. We are inverting this because
+    // we want a positive value when we pull to the left. Xbox controllers
+    // return positive values when you pull to the right by default.
+    final var ySpeed =
+        -m_yspeedLimiter.calculate(m_controller.getX(GenericHID.Hand.kLeft))
+            * Drivetrain.kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    final var rot =
+        -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
+            * Drivetrain.kMaxAngularSpeed;
+
+    m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Drivetrain.java

@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.swervesdriveposeestimator;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+
+/**
+ * Represents a swerve drive style drivetrain.
+ */
+public class Drivetrain {
+  public static final double kMaxSpeed = 3.0; // 3 meters per second
+  public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
+
+  private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
+  private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
+  private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
+  private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
+
+  private final SwerveModule m_frontLeft = new SwerveModule(1, 2);
+  private final SwerveModule m_frontRight = new SwerveModule(3, 4);
+  private final SwerveModule m_backLeft = new SwerveModule(5, 6);
+  private final SwerveModule m_backRight = new SwerveModule(7, 8);
+
+  private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+  private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
+      m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
+  );
+
+  /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
+     below are robot specific, and should be tuned. */
+  private final SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator(
+      m_gyro.getRotation2d(),
+      new Pose2d(),
+      m_kinematics,
+      VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
+      VecBuilder.fill(Units.degreesToRadians(0.01)),
+      VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30))
+  );
+
+  public Drivetrain() {
+    m_gyro.reset();
+  }
+
+  /**
+   * Method to drive the robot using joystick info.
+   *
+   * @param xSpeed        Speed of the robot in the x direction (forward).
+   * @param ySpeed        Speed of the robot in the y direction (sideways).
+   * @param rot           Angular rate of the robot.
+   * @param fieldRelative Whether the provided x and y speeds are relative to the field.
+   */
+  @SuppressWarnings("ParameterName")
+  public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+    var swerveModuleStates = m_kinematics.toSwerveModuleStates(
+        fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
+            xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+            : new ChassisSpeeds(xSpeed, ySpeed, rot)
+    );
+    SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeed);
+    m_frontLeft.setDesiredState(swerveModuleStates[0]);
+    m_frontRight.setDesiredState(swerveModuleStates[1]);
+    m_backLeft.setDesiredState(swerveModuleStates[2]);
+    m_backRight.setDesiredState(swerveModuleStates[3]);
+  }
+
+  /**
+   * Updates the field relative position of the robot.
+   */
+  public void updateOdometry() {
+    m_poseEstimator.update(
+        m_gyro.getRotation2d(),
+        m_frontLeft.getState(),
+        m_frontRight.getState(),
+        m_backLeft.getState(),
+        m_backRight.getState()
+    );
+
+    // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
+    // a real robot, this must be calculated based either on latency or timestamps.
+    m_poseEstimator.addVisionMeasurement(
+        ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
+          m_poseEstimator.getEstimatedPosition()),
+        Timer.getFPGATimestamp() - 0.3);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/ExampleGlobalMeasurementSensor.java

@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.swervesdriveposeestimator;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.math.StateSpaceUtil;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+
+/**
+ * This dummy class represents a global measurement sensor, such as a computer vision solution.
+ */
+public final class ExampleGlobalMeasurementSensor {
+  private ExampleGlobalMeasurementSensor() {
+    // Utility class
+  }
+
+
+  /**
+   * Get a "noisy" fake global pose reading.
+   *
+   * @param estimatedRobotPose The robot pose.
+   */
+  public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
+    var rand = StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5,
+        Units.degreesToRadians(30)));
+    return new Pose2d(estimatedRobotPose.getX() + rand.get(0, 0),
+        estimatedRobotPose.getY() + rand.get(1, 0),
+        estimatedRobotPose.getRotation().plus(new Rotation2d(rand.get(2, 0))));
+  }
+
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Main.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.swervesdriveposeestimator;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Robot.java

@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.swervesdriveposeestimator;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.SlewRateLimiter;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class Robot extends TimedRobot {
+  private final XboxController m_controller = new XboxController(0);
+  private final Drivetrain m_swerve = new Drivetrain();
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+  private final SlewRateLimiter m_xspeedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_yspeedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
+  @Override
+  public void autonomousPeriodic() {
+    driveWithJoystick(false);
+    m_swerve.updateOdometry();
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    driveWithJoystick(true);
+  }
+
+  private void driveWithJoystick(boolean fieldRelative) {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    final var xSpeed =
+        -m_xspeedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
+            * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed;
+
+    // Get the y speed or sideways/strafe speed. We are inverting this because
+    // we want a positive value when we pull to the left. Xbox controllers
+    // return positive values when you pull to the right by default.
+    final var ySpeed =
+        -m_yspeedLimiter.calculate(m_controller.getX(GenericHID.Hand.kLeft))
+            * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    final var rot =
+        -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
+            * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxAngularSpeed;
+
+    m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/SwerveModule.java

@@ -0,0 +1,101 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.swervesdriveposeestimator;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+public class SwerveModule {
+  private static final double kWheelRadius = 0.0508;
+  private static final int kEncoderResolution = 4096;
+
+  private static final double kModuleMaxAngularVelocity = Drivetrain.kMaxAngularSpeed;
+  private static final double kModuleMaxAngularAcceleration
+      = 2 * Math.PI; // radians per second squared
+
+  private final SpeedController m_driveMotor;
+  private final SpeedController m_turningMotor;
+
+  private final Encoder m_driveEncoder = new Encoder(0, 1);
+  private final Encoder m_turningEncoder = new Encoder(2, 3);
+
+  private final PIDController m_drivePIDController = new PIDController(1, 0, 0);
+
+  private final ProfiledPIDController m_turningPIDController
+      = new ProfiledPIDController(1, 0, 0,
+      new TrapezoidProfile.Constraints(kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration));
+
+  // Gains are for example purposes only - must be determined for your own robot!
+  private final SimpleMotorFeedforward m_driveFeedforward = new SimpleMotorFeedforward(1, 3);
+  private final SimpleMotorFeedforward m_turnFeedforward = new SimpleMotorFeedforward(1, 0.5);
+
+  /**
+   * Constructs a SwerveModule.
+   *
+   * @param driveMotorChannel   ID for the drive motor.
+   * @param turningMotorChannel ID for the turning motor.
+   */
+  public SwerveModule(int driveMotorChannel, int turningMotorChannel) {
+    m_driveMotor = new PWMVictorSPX(driveMotorChannel);
+    m_turningMotor = new PWMVictorSPX(turningMotorChannel);
+
+    // Set the distance per pulse for the drive encoder. We can simply use the
+    // distance traveled for one rotation of the wheel divided by the encoder
+    // resolution.
+    m_driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+
+    // Set the distance (in this case, angle) per pulse for the turning encoder.
+    // This is the the angle through an entire rotation (2 * wpi::math::pi)
+    // divided by the encoder resolution.
+    m_turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution);
+
+    // Limit the PID Controller's input range between -pi and pi and set the input
+    // to be continuous.
+    m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
+  }
+
+  /**
+   * Returns the current state of the module.
+   *
+   * @return The current state of the module.
+   */
+  public SwerveModuleState getState() {
+    return new SwerveModuleState(m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.get()));
+  }
+
+  /**
+   * Sets the desired state for the module.
+   *
+   * @param state Desired state with speed and angle.
+   */
+  public void setDesiredState(SwerveModuleState state) {
+    // Calculate the drive output from the drive PID controller.
+    final double driveOutput = m_drivePIDController.calculate(
+        m_driveEncoder.getRate(), state.speedMetersPerSecond);
+
+    final double driveFeedforward = m_driveFeedforward.calculate(state.speedMetersPerSecond);
+
+    // Calculate the turning motor output from the turning PID controller.
+    final double turnOutput = m_turningPIDController.calculate(
+        m_turningEncoder.get(), state.angle.getRadians()
+    );
+
+    final double turnFeedforward =
+        m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);
+
+    m_driveMotor.setVoltage(driveOutput + driveFeedforward);
+    m_turningMotor.setVoltage(turnOutput + turnFeedforward);
+  }
+}

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

@@ -0,0 +1,130 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+
+import java.util.function.BiFunction;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+public final class AngleStatistics {
+  private AngleStatistics() {
+    // Utility class
+  }
+
+  /**
+   * Subtracts a and b while normalizing the resulting value in the selected row as if it were an
+   * angle.
+   *
+   * @param a A vector to subtract from.
+   * @param b A vector to subtract with.
+   * @param angleStateIdx The row containing angles to be normalized.
+   */
+  @SuppressWarnings("checkstyle:ParameterName")
+  public static <S extends Num> Matrix<S, N1> angleResidual(Matrix<S, N1> a, Matrix<S, N1> b,
+                                                            int angleStateIdx) {
+    Matrix<S, N1> ret = a.minus(b);
+    ret.set(angleStateIdx, 0, normalizeAngle(ret.get(angleStateIdx, 0)));
+
+    return ret;
+  }
+
+  /**
+   * Returns a function that subtracts two vectors while normalizing the resulting value in the
+   * selected row as if it were an angle.
+   *
+   * @param angleStateIdx The row containing angles to be normalized.
+   */
+  @SuppressWarnings("checkstyle:ParameterName")
+  public static <S extends Num> BiFunction<Matrix<S, N1>, Matrix<S, N1>, Matrix<S, N1>>
+      angleResidual(int angleStateIdx) {
+    return (a, b) -> angleResidual(a, b, angleStateIdx);
+  }
+
+  /**
+   * Adds a and b while normalizing the resulting value in the selected row as an angle.
+   *
+   * @param a A vector to add with.
+   * @param b A vector to add with.
+   * @param angleStateIdx The row containing angles to be normalized.
+   */
+  @SuppressWarnings("checkstyle:ParameterName")
+  public static <S extends Num> Matrix<S, N1> angleAdd(Matrix<S, N1> a, Matrix<S, N1> b,
+                                                       int angleStateIdx) {
+    Matrix<S, N1> ret = a.plus(b);
+    ret.set(angleStateIdx, 0, normalizeAngle(ret.get(angleStateIdx, 0)));
+
+    return ret;
+  }
+
+  /**
+   * Returns a function that adds two vectors while normalizing the resulting value in the selected
+   * row as an angle.
+   *
+   * @param angleStateIdx The row containing angles to be normalized.
+   */
+  @SuppressWarnings("checkstyle:ParameterName")
+  public static <S extends Num> BiFunction<Matrix<S, N1>, Matrix<S, N1>, Matrix<S, N1>>
+      angleAdd(int angleStateIdx) {
+    return (a, b) -> angleAdd(a, b, angleStateIdx);
+  }
+
+  static double normalizeAngle(double angle) {
+    final double tau = 2 * Math.PI;
+
+    angle -= Math.floor(angle / tau) * tau;
+    if (angle > Math.PI) {
+      angle -= tau;
+    }
+    return angle;
+  }
+
+  /**
+   * Computes the mean of sigmas with the weights Wm while computing a special angle mean for a
+   * select row.
+   *
+   * @param sigmas Sigma points.
+   * @param Wm Weights for the mean.
+   * @param angleStateIdx The row containing the angles.
+   */
+  @SuppressWarnings("checkstyle:ParameterName")
+  public static <S extends Num> Matrix<S, N1> angleMean(Matrix<S, ?> sigmas, Matrix<?, N1> Wm,
+                                                        int angleStateIdx) {
+    double[] angleSigmas = sigmas.extractRowVector(angleStateIdx).getData();
+    Matrix<N1, ?> sinAngleSigmas = new Matrix<>(new SimpleMatrix(1, sigmas.getNumCols()));
+    Matrix<N1, ?> cosAngleSigmas = new Matrix<>(new SimpleMatrix(1, sigmas.getNumCols()));
+    for (int i = 0; i < angleSigmas.length; i++) {
+      sinAngleSigmas.set(0, i, Math.sin(angleSigmas[i]));
+      cosAngleSigmas.set(0, i, Math.cos(angleSigmas[i]));
+    }
+
+    double sumSin = sinAngleSigmas.times(Matrix.changeBoundsUnchecked(Wm)).elementSum();
+    double sumCos = cosAngleSigmas.times(Matrix.changeBoundsUnchecked(Wm)).elementSum();
+
+    Matrix<S, N1> ret = sigmas.times(Matrix.changeBoundsUnchecked(Wm));
+    ret.set(angleStateIdx, 0, Math.atan2(sumSin, sumCos));
+
+    return ret;
+  }
+
+  /**
+   * Returns a function that computes the mean of sigmas with the weights Wm while computing a
+   * special angle mean for a select row.
+   *
+   * @param angleStateIdx The row containing the angles.
+   */
+  @SuppressWarnings("checkstyle:ParameterName")
+  public static <S extends Num> BiFunction<Matrix<S, ?>, Matrix<?, N1>, Matrix<S, N1>>
+      angleMean(int angleStateIdx) {
+    return (sigmas, Wm) -> angleMean(sigmas, Wm, angleStateIdx);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java

@@ -0,0 +1,297 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import java.util.function.BiConsumer;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.math.Discretization;
+import edu.wpi.first.wpilibj.math.StateSpaceUtil;
+import edu.wpi.first.wpiutil.WPIUtilJNI;
+import edu.wpi.first.wpiutil.math.MatBuilder;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N3;
+import edu.wpi.first.wpiutil.math.numbers.N5;
+
+/**
+ * This class wraps an
+ * {@link edu.wpi.first.wpilibj.estimator.UnscentedKalmanFilter Unscented Kalman Filter}
+ * to fuse latency-compensated vision
+ * measurements with differential drive encoder measurements. It will correct
+ * for noisy vision measurements and encoder drift. It is intended to be an easy
+ * drop-in for
+ * {@link edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry}; in fact,
+ * if you never call {@link DifferentialDrivePoseEstimator#addVisionMeasurement}
+ * and only call {@link DifferentialDrivePoseEstimator#update} then this will
+ * behave exactly the same as DifferentialDriveOdometry.
+ *
+ * <p>{@link DifferentialDrivePoseEstimator#update} should be called every robot
+ * loop (if your robot loops are faster than the default then you should change
+ * the {@link DifferentialDrivePoseEstimator#DifferentialDrivePoseEstimator(Rotation2d, Pose2d,
+ * Matrix, Matrix, Matrix, double) nominal delta time}.)
+ * {@link DifferentialDrivePoseEstimator#addVisionMeasurement} can be called as
+ * infrequently as you want; if you never call it then this class will behave
+ * exactly like regular encoder odometry.
+ *
+ * <p>Our state-space system is:
+ *
+ * <p><strong> x = [[x, y, theta, dist_l, dist_r]]^T </strong>
+ * in the field coordinate system (dist_* are wheel distances.)
+ *
+ * <p><strong> u = [[vx, vy, omega]]^T </strong> (robot-relative velocities)
+ *  -- NB: using velocities make things considerably easier, because it means that
+ * teams don't have to worry about getting an accurate model.
+ * Basically, we suspect that it's easier for teams to get good encoder data than it is for
+ * them to perform system identification well enough to get a good model.
+ *
+ * <p><strong>y = [[x, y, theta]]^T </strong> from vision,
+ * or <strong>y = [[dist_l, dist_r, theta]] </strong> from encoders and gyro.
+ */
+public class DifferentialDrivePoseEstimator {
+  final UnscentedKalmanFilter<N5, N3, N3> m_observer; // Package-private to allow for unit testing
+  private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
+  private final KalmanFilterLatencyCompensator<N5, N3, N3> m_latencyCompensator;
+
+  private final double m_nominalDt; // Seconds
+  private double m_prevTimeSeconds = -1.0;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  /**
+   * Constructs a DifferentialDrivePoseEstimator.
+   *
+   * @param gyroAngle                The current gyro angle.
+   * @param initialPoseMeters        The starting pose estimate.
+   * @param stateStdDevs             Standard deviations of model states. Increase these numbers to
+   *                                 trust your wheel and gyro velocities less.
+   * @param localMeasurementStdDevs  Standard deviations of the encoder and gyro measurements.
+   *                                 Increase these numbers to trust encoder distances and gyro
+   *                                 angle less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase
+   *                                 these numbers to trust vision less.
+   */
+  public DifferentialDrivePoseEstimator(
+          Rotation2d gyroAngle, Pose2d initialPoseMeters,
+          Matrix<N5, N1> stateStdDevs,
+          Matrix<N3, N1> localMeasurementStdDevs, Matrix<N3, N1> visionMeasurementStdDevs
+  ) {
+    this(gyroAngle, initialPoseMeters,
+            stateStdDevs, localMeasurementStdDevs, visionMeasurementStdDevs, 0.02);
+  }
+
+  /**
+   * Constructs a DifferentialDrivePoseEstimator.
+   *
+   * @param gyroAngle                The current gyro angle.
+   * @param initialPoseMeters        The starting pose estimate.
+   * @param stateStdDevs             Standard deviations of model states. Increase these numbers to
+   *                                 trust your wheel and gyro velocities less.
+   * @param localMeasurementStdDevs  Standard deviations of the encoder and gyro measurements.
+   *                                 Increase these numbers to trust encoder distances and gyro
+   *                                 angle less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase
+   *                                 these numbers to trust vision less.
+   * @param nominalDtSeconds         The time in seconds between each robot loop.
+   */
+  @SuppressWarnings("ParameterName")
+  public DifferentialDrivePoseEstimator(
+          Rotation2d gyroAngle, Pose2d initialPoseMeters,
+          Matrix<N5, N1> stateStdDevs,
+          Matrix<N3, N1> localMeasurementStdDevs, Matrix<N3, N1> visionMeasurementStdDevs,
+          double nominalDtSeconds
+  ) {
+    m_nominalDt = nominalDtSeconds;
+
+    m_observer = new UnscentedKalmanFilter<>(
+        Nat.N5(), Nat.N3(),
+        this::f,
+        (x, u) -> VecBuilder.fill(x.get(3, 0), x.get(4, 0), x.get(2, 0)),
+        stateStdDevs, localMeasurementStdDevs,
+        AngleStatistics.angleMean(2),
+        AngleStatistics.angleMean(2),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleAdd(2),
+        m_nominalDt
+    );
+    m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+
+    var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
+    var visionDiscR = Discretization.discretizeR(visionContR, m_nominalDt);
+    m_visionCorrect = (u, y) -> m_observer.correct(
+        Nat.N3(), u, y,
+        (x, u_) -> new Matrix<>(x.getStorage().extractMatrix(0, 3, 0, 1)),
+        visionDiscR,
+        AngleStatistics.angleMean(2),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleAdd(2)
+    );
+
+    m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPoseMeters.getRotation();
+    m_observer.setXhat(fillStateVector(initialPoseMeters, 0.0, 0.0));
+  }
+
+  @SuppressWarnings({"ParameterName", "MethodName"})
+  private Matrix<N5, N1> f(Matrix<N5, N1> x, Matrix<N3, N1> u) {
+    // Apply a rotation matrix. Note that we do *not* add x--Runge-Kutta does that for us.
+    var theta = x.get(2, 0);
+    var toFieldRotation = new MatBuilder<>(Nat.N5(), Nat.N5()).fill(
+            Math.cos(theta), -Math.sin(theta), 0, 0, 0,
+            Math.sin(theta), Math.cos(theta), 0, 0, 0,
+            0, 0, 1, 0, 0,
+            0, 0, 0, 1, 0,
+            0, 0, 0, 0, 1
+    );
+    return toFieldRotation.times(VecBuilder.fill(
+            u.get(0, 0), u.get(1, 0), u.get(2, 0), u.get(0, 0), u.get(1, 0)
+    ));
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>You NEED to reset your encoders (to zero) when calling this method.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code.
+   * The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param poseMeters The position on the field that your robot is at.
+   * @param gyroAngle  The angle reported by the gyroscope.
+   */
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+    m_previousAngle = poseMeters.getRotation();
+    m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
+    m_observer.setXhat(fillStateVector(poseMeters, 0.0, 0.0));
+  }
+
+  /**
+   * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  public Pose2d getEstimatedPosition() {
+    return new Pose2d(
+            m_observer.getXhat(0),
+            m_observer.getXhat(1),
+            new Rotation2d(m_observer.getXhat(2))
+    );
+  }
+
+  /**
+   * Add a vision measurement to the Unscented Kalman Filter. This will correct the
+   * odometry pose estimate while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are
+   * calling {@link DifferentialDrivePoseEstimator#update} every loop.
+   *
+   * @param visionRobotPoseMeters The pose of the robot as measured by the vision
+   *                              camera.
+   * @param timestampSeconds      The timestamp of the vision measurement in seconds. Note that if
+   *                              you don't use your own time source by calling
+   *                              {@link DifferentialDrivePoseEstimator#updateWithTime} then you
+   *                              must use a timestamp with an epoch since FPGA startup
+   *                              (i.e. the epoch of this timestamp is the same epoch as
+   *                              Timer.getFPGATimestamp.) This means that you should
+   *                              use Timer.getFPGATimestamp as your time source in
+   *                              this case.
+   */
+  public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+    m_latencyCompensator.applyPastGlobalMeasurement(
+            Nat.N3(),
+            m_observer, m_nominalDt,
+            StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
+            m_visionCorrect,
+            timestampSeconds
+    );
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder information.
+   * Note that this should be called every loop.
+   *
+   * @param gyroAngle                      The current gyro angle.
+   * @param wheelVelocitiesMetersPerSecond The velocities of the wheels in meters per second.
+   * @param distanceLeftMeters             The total distance travelled by the left wheel in meters
+   *                                       since the last time you called
+   *                                       {@link DifferentialDrivePoseEstimator#resetPosition}.
+   * @param distanceRightMeters            The total distance travelled by the right wheel in meters
+   *                                       since the last time you called
+   *                                       {@link DifferentialDrivePoseEstimator#resetPosition}.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose2d update(
+          Rotation2d gyroAngle,
+          DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond,
+          double distanceLeftMeters, double distanceRightMeters
+  ) {
+    return updateWithTime(
+            WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelVelocitiesMetersPerSecond,
+            distanceLeftMeters, distanceRightMeters
+    );
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder information.
+   * Note that this should be called every loop.
+   *
+   * @param currentTimeSeconds             Time at which this method was called, in seconds.
+   * @param gyroAngle                      The current gyro angle.
+   * @param wheelVelocitiesMetersPerSecond The velocities of the wheels in meters per second.
+   * @param distanceLeftMeters             The total distance travelled by the left wheel in meters
+   *                                       since the last time you called
+   *                                       {@link DifferentialDrivePoseEstimator#resetPosition}.
+   * @param distanceRightMeters            The total distance travelled by the right wheel in meters
+   *                                       since the last time you called
+   *                                       {@link DifferentialDrivePoseEstimator#resetPosition}.
+   * @return The estimated pose of the robot in meters.
+   */
+  @SuppressWarnings({"LocalVariableName", "ParameterName"})
+  public Pose2d updateWithTime(
+          double currentTimeSeconds, Rotation2d gyroAngle,
+          DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond,
+          double distanceLeftMeters, double distanceRightMeters
+  ) {
+    double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
+    m_prevTimeSeconds = currentTimeSeconds;
+
+    var angle = gyroAngle.plus(m_gyroOffset);
+    // Diff drive forward kinematics:
+    // v_c = (v_l + v_r) / 2
+    var wheelVels = wheelVelocitiesMetersPerSecond;
+    var u = VecBuilder.fill(
+            (wheelVels.leftMetersPerSecond + wheelVels.rightMetersPerSecond) / 2, 0,
+            angle.minus(m_previousAngle).getRadians() / dt
+    );
+    m_previousAngle = angle;
+
+    var localY = VecBuilder.fill(distanceLeftMeters, distanceRightMeters, angle.getRadians());
+    m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
+    m_observer.predict(u, dt);
+    m_observer.correct(u, localY);
+
+    return getEstimatedPosition();
+  }
+
+  private static Matrix<N5, N1> fillStateVector(Pose2d pose, double leftDist, double rightDist) {
+    return VecBuilder.fill(
+            pose.getTranslation().getX(),
+            pose.getTranslation().getY(),
+            pose.getRotation().getRadians(),
+            leftDist,
+            rightDist
+    );
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilterLatencyCompensator.java

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

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

@@ -0,0 +1,250 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import java.util.function.BiConsumer;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.math.Discretization;
+import edu.wpi.first.wpilibj.math.StateSpaceUtil;
+import edu.wpi.first.wpiutil.WPIUtilJNI;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N3;
+
+/**
+ * This class wraps an {@link UnscentedKalmanFilter Unscented Kalman Filter} to fuse
+ * latency-compensated vision measurements with mecanum drive encoder velocity measurements.
+ * It will correct for noisy measurements and encoder drift. It is intended to be an easy
+ * but more accurate drop-in for {@link edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry}.
+ *
+ * <p>{@link MecanumDrivePoseEstimator#update} should be called every robot loop. If
+ * your loops are faster or slower than the default of 0.02s, then you should change
+ * the nominal delta time using the secondary constructor:
+ * {@link MecanumDrivePoseEstimator#MecanumDrivePoseEstimator(Rotation2d, Pose2d,
+ * MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}.
+ *
+ * <p>{@link MecanumDrivePoseEstimator#addVisionMeasurement} can be called as
+ * infrequently as you want; if you never call it, then this class will behave mostly like regular
+ * encoder odometry.
+ *
+ * <p>Our state-space system is:
+ *
+ * <p><strong> x = [[x, y, theta]]^T </strong> in the field-coordinate system.
+ *
+ * <p><strong> u = [[vx, vy, theta]]^T </strong> in the field-coordinate system.
+ *
+ * <p><strong> y = [[x, y, theta]]^T </strong> in field coords from vision,
+ * or <strong> y = [[theta]]^T </strong> from the gyro.
+ */
+public class MecanumDrivePoseEstimator {
+  private final UnscentedKalmanFilter<N3, N3, N1> m_observer;
+  private final MecanumDriveKinematics m_kinematics;
+  private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
+  private final KalmanFilterLatencyCompensator<N3, N3, N1> m_latencyCompensator;
+
+  private final double m_nominalDt; // Seconds
+  private double m_prevTimeSeconds = -1.0;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  /**
+   * Constructs a MecanumDrivePoseEstimator.
+   *
+   * @param gyroAngle                The current gyro angle.
+   * @param initialPoseMeters        The starting pose estimate.
+   * @param kinematics               A correctly-configured kinematics object for your drivetrain.
+   * @param stateStdDevs             Standard deviations of model states. Increase these numbers to
+   *                                 trust your wheel and gyro velocities less.
+   * @param localMeasurementStdDevs  Standard deviations of the gyro measurement. Increase this
+   *                                 number to trust gyro angle measurements less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase
+   *                                 these numbers to trust vision less.
+   */
+  public MecanumDrivePoseEstimator(
+          Rotation2d gyroAngle, Pose2d initialPoseMeters, MecanumDriveKinematics kinematics,
+          Matrix<N3, N1> stateStdDevs, Matrix<N1, N1> localMeasurementStdDevs,
+          Matrix<N3, N1> visionMeasurementStdDevs
+  ) {
+    this(gyroAngle, initialPoseMeters, kinematics, stateStdDevs, localMeasurementStdDevs,
+            visionMeasurementStdDevs, 0.02);
+  }
+
+  /**
+   * Constructs a MecanumDrivePoseEstimator.
+   *
+   * @param gyroAngle                The current gyro angle.
+   * @param initialPoseMeters        The starting pose estimate.
+   * @param kinematics               A correctly-configured kinematics object for your drivetrain.
+   * @param stateStdDevs             Standard deviations of model states. Increase these numbers to
+   *                                 trust your wheel and gyro velocities less.
+   * @param localMeasurementStdDevs  Standard deviations of the gyro measurement. Increase this
+   *                                 number to trust gyro angle measurements less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase
+   *                                 these numbers to trust vision less.
+   * @param nominalDtSeconds         The time in seconds between each robot loop.
+   */
+  @SuppressWarnings("ParameterName")
+  public MecanumDrivePoseEstimator(
+          Rotation2d gyroAngle, Pose2d initialPoseMeters, MecanumDriveKinematics kinematics,
+          Matrix<N3, N1> stateStdDevs, Matrix<N1, N1> localMeasurementStdDevs,
+          Matrix<N3, N1> visionMeasurementStdDevs, double nominalDtSeconds
+  ) {
+    m_nominalDt = nominalDtSeconds;
+
+    m_observer = new UnscentedKalmanFilter<>(
+        Nat.N3(), Nat.N1(),
+        (x_, u) -> u,
+        (x, u_) -> x.extractRowVector(2),
+        stateStdDevs,
+        localMeasurementStdDevs,
+        AngleStatistics.angleMean(2),
+        AngleStatistics.angleMean(0),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleResidual(0),
+        AngleStatistics.angleAdd(2),
+        m_nominalDt
+    );
+    m_kinematics = kinematics;
+    m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+
+    var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
+    var visionDiscR = Discretization.discretizeR(visionContR, m_nominalDt);
+
+    m_visionCorrect = (u, y) -> m_observer.correct(
+        Nat.N3(), u, y,
+        (x, u_) -> x,
+        visionDiscR,
+        AngleStatistics.angleMean(2),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleAdd(2)
+    );
+
+    m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPoseMeters.getRotation();
+    m_observer.setXhat(StateSpaceUtil.poseTo3dVector(initialPoseMeters));
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>You NEED to reset your encoders (to zero) when calling this method.
+   *
+   * <p>The gyroscope angle does not need to be reset in the user's robot code.
+   * The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param poseMeters The position on the field that your robot is at.
+   * @param gyroAngle  The angle reported by the gyroscope.
+   */
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+    m_previousAngle = poseMeters.getRotation();
+    m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
+    m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
+  }
+
+  /**
+   * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  public Pose2d getEstimatedPosition() {
+    return new Pose2d(
+            m_observer.getXhat(0),
+            m_observer.getXhat(1),
+            new Rotation2d(m_observer.getXhat(2))
+    );
+  }
+
+  /**
+   * Add a vision measurement to the Unscented Kalman Filter. This will correct the
+   * odometry pose estimate while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are
+   * calling {@link MecanumDrivePoseEstimator#update} every loop.
+   *
+   * @param visionRobotPoseMeters The pose of the robot as measured by the vision
+   *                              camera.
+   * @param timestampSeconds      The timestamp of the vision measurement in seconds. Note that if
+   *                              you don't use your own time source by calling
+   *                              {@link MecanumDrivePoseEstimator#updateWithTime} then you
+   *                              must use a timestamp with an epoch since FPGA startup
+   *                              (i.e. the epoch of this timestamp is the same epoch as
+   *                              Timer.getFPGATimestamp.) This means that you should
+   *                              use Timer.getFPGATimestamp as your time source
+   *                              or sync the epochs.
+   */
+  public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+    m_latencyCompensator.applyPastGlobalMeasurement(
+            Nat.N3(),
+            m_observer, m_nominalDt,
+            StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
+            m_visionCorrect,
+            timestampSeconds
+    );
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder information.
+   * This should be called every loop, and the correct loop period must be passed
+   * into the constructor of this class.
+   *
+   * @param gyroAngle   The current gyro angle.
+   * @param wheelSpeeds The current speeds of the mecanum drive wheels.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
+    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds);
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder information.
+   * This should be called every loop, and the correct loop period must be passed
+   * into the constructor of this class.
+   *
+   * @param currentTimeSeconds Time at which this method was called, in seconds.
+   * @param gyroAngle          The current gyroscope angle.
+   * @param wheelSpeeds        The current speeds of the mecanum drive wheels.
+   * @return The estimated pose of the robot in meters.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
+                               MecanumDriveWheelSpeeds wheelSpeeds) {
+    double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
+    m_prevTimeSeconds = currentTimeSeconds;
+
+    var angle = gyroAngle.plus(m_gyroOffset);
+    var omega = angle.minus(m_previousAngle).getRadians() / dt;
+
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+    var fieldRelativeVelocities =
+            new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond)
+                    .rotateBy(angle);
+
+    var u = VecBuilder.fill(
+            fieldRelativeVelocities.getX(),
+            fieldRelativeVelocities.getY(),
+            omega
+    );
+    m_previousAngle = angle;
+
+    var localY = VecBuilder.fill(angle.getRadians());
+    m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
+    m_observer.predict(u, dt);
+    m_observer.correct(u, localY);
+
+    return getEstimatedPosition();
+  }
+}

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

@@ -0,0 +1,255 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import java.util.function.BiConsumer;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.math.Discretization;
+import edu.wpi.first.wpilibj.math.StateSpaceUtil;
+import edu.wpi.first.wpiutil.WPIUtilJNI;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N3;
+
+/**
+ * This class wraps an {@link UnscentedKalmanFilter Unscented Kalman Filter} to fuse
+ * latency-compensated vision measurements with swerve drive encoder velocity measurements.
+ * It will correct for noisy measurements and encoder drift. It is intended to be an easy
+ * but more accurate drop-in for {@link edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry}.
+ *
+ * <p>{@link SwerveDrivePoseEstimator#update} should be called every robot loop. If
+ * your loops are faster or slower than the default of 0.02s, then you should change
+ * the nominal delta time using the secondary constructor:
+ * {@link SwerveDrivePoseEstimator#SwerveDrivePoseEstimator(Rotation2d, Pose2d,
+ * SwerveDriveKinematics, Matrix, Matrix, Matrix, double)}.
+ *
+ * <p>{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as
+ * infrequently as you want; if you never call it, then this class will behave mostly like regular
+ * encoder odometry.
+ *
+ * <p>Our state-space system is:
+ *
+ * <p><strong> x = [[x, y, theta]]^T </strong> in the field-coordinate system.
+ *
+ * <p><strong> u = [[vx, vy, omega]]^T </strong> in the field-coordinate system.
+ *
+ * <p><strong> y = [[x, y, theta]]^T </strong> in field coords from vision,
+ * or <strong> y = [[theta]]^T </strong> from the gyro.
+ */
+public class SwerveDrivePoseEstimator {
+  private final UnscentedKalmanFilter<N3, N3, N1> m_observer;
+  private final SwerveDriveKinematics m_kinematics;
+  private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
+  private final KalmanFilterLatencyCompensator<N3, N3, N1> m_latencyCompensator;
+
+  private final double m_nominalDt; // Seconds
+  private double m_prevTimeSeconds = -1.0;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  /**
+   * Constructs a SwerveDrivePoseEstimator.
+   *
+   * @param gyroAngle                The current gyro angle.
+   * @param initialPoseMeters        The starting pose estimate.
+   * @param kinematics               A correctly-configured kinematics object for your drivetrain.
+   * @param stateStdDevs             Standard deviations of model states. Increase these numbers to
+   *                                 trust your wheel and gyro velocities less.
+   * @param localMeasurementStdDevs  Standard deviations of the gyro measurement. Increase this
+   *                                 number to trust gyro angle measurements less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase
+   *                                 these numbers to trust vision less.
+   */
+  public SwerveDrivePoseEstimator(
+          Rotation2d gyroAngle, Pose2d initialPoseMeters, SwerveDriveKinematics kinematics,
+          Matrix<N3, N1> stateStdDevs, Matrix<N1, N1> localMeasurementStdDevs,
+          Matrix<N3, N1> visionMeasurementStdDevs
+  ) {
+    this(gyroAngle, initialPoseMeters, kinematics, stateStdDevs, localMeasurementStdDevs,
+            visionMeasurementStdDevs, 0.02);
+  }
+
+  /**
+   * Constructs a SwerveDrivePoseEstimator.
+   *
+   * @param gyroAngle                The current gyro angle.
+   * @param initialPoseMeters        The starting pose estimate.
+   * @param kinematics               A correctly-configured kinematics object for your drivetrain.
+   * @param stateStdDevs             Standard deviations of model states. Increase these numbers to
+   *                                 trust your wheel and gyro velocities less.
+   * @param localMeasurementStdDevs  Standard deviations of the gyro measurement. Increase this
+   *                                 number to trust gyro angle measurements less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase
+   *                                 these numbers to trust vision less.
+   * @param nominalDtSeconds         The time in seconds between each robot loop.
+   */
+  @SuppressWarnings("ParameterName")
+  public SwerveDrivePoseEstimator(
+          Rotation2d gyroAngle, Pose2d initialPoseMeters, SwerveDriveKinematics kinematics,
+          Matrix<N3, N1> stateStdDevs, Matrix<N1, N1> localMeasurementStdDevs,
+          Matrix<N3, N1> visionMeasurementStdDevs, double nominalDtSeconds
+  ) {
+    m_nominalDt = nominalDtSeconds;
+
+    m_observer = new UnscentedKalmanFilter<>(
+        Nat.N3(), Nat.N1(),
+        (x_, u) -> u,
+        (x, u_) -> x.extractRowVector(2),
+        stateStdDevs,
+        localMeasurementStdDevs,
+        AngleStatistics.angleMean(2),
+        AngleStatistics.angleMean(0),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleResidual(0),
+        AngleStatistics.angleAdd(2),
+        m_nominalDt
+    );
+    m_kinematics = kinematics;
+    m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+
+    var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
+    var visionDiscR = Discretization.discretizeR(visionContR, m_nominalDt);
+
+    m_visionCorrect = (u, y) -> m_observer.correct(
+        Nat.N3(), u, y,
+        (x, u_) -> x,
+        visionDiscR,
+        AngleStatistics.angleMean(2),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleAdd(2)
+    );
+
+    m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPoseMeters.getRotation();
+    m_observer.setXhat(StateSpaceUtil.poseTo3dVector(initialPoseMeters));
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>You NEED to reset your encoders (to zero) when calling this method.
+   *
+   * <p>The gyroscope angle does not need to be reset in the user's robot code.
+   * The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param poseMeters The position on the field that your robot is at.
+   * @param gyroAngle  The angle reported by the gyroscope.
+   */
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+    m_previousAngle = poseMeters.getRotation();
+    m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
+    m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
+  }
+
+  /**
+   * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  public Pose2d getEstimatedPosition() {
+    return new Pose2d(
+            m_observer.getXhat(0),
+            m_observer.getXhat(1),
+            new Rotation2d(m_observer.getXhat(2))
+    );
+  }
+
+  /**
+   * Add a vision measurement to the Unscented Kalman Filter. This will correct the
+   * odometry pose estimate while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are
+   * calling {@link SwerveDrivePoseEstimator#update} every loop.
+   *
+   * @param visionRobotPoseMeters The pose of the robot as measured by the vision
+   *                              camera.
+   * @param timestampSeconds      The timestamp of the vision measurement in seconds. Note that if
+   *                              you don't use your own time source by calling
+   *                              {@link SwerveDrivePoseEstimator#updateWithTime} then you
+   *                              must use a timestamp with an epoch since FPGA startup
+   *                              (i.e. the epoch of this timestamp is the same epoch as
+   *                              Timer.getFPGATimestamp.) This means that you should
+   *                              use Timer.getFPGATimestamp as your time source or
+   *                              sync the epochs.
+   */
+  public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+    m_latencyCompensator.applyPastGlobalMeasurement(
+            Nat.N3(),
+            m_observer, m_nominalDt,
+            StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
+            m_visionCorrect,
+            timestampSeconds
+    );
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder information.
+   * This should be called every loop, and the correct loop period must be passed
+   * into the constructor of this class.
+   *
+   * @param gyroAngle    The current gyro angle.
+   * @param moduleStates The current velocities and rotations of the swerve modules.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose2d update(
+          Rotation2d gyroAngle,
+          SwerveModuleState... moduleStates
+  ) {
+    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder information.
+   * This should be called every loop, and the correct loop period must be passed
+   * into the constructor of this class.
+   *
+   * @param currentTimeSeconds Time at which this method was called, in seconds.
+   * @param gyroAngle          The current gyroscope angle.
+   * @param moduleStates       The current velocities and rotations of the swerve modules.
+   * @return The estimated pose of the robot in meters.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public Pose2d updateWithTime(
+          double currentTimeSeconds,
+          Rotation2d gyroAngle, SwerveModuleState... moduleStates
+  ) {
+    double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
+    m_prevTimeSeconds = currentTimeSeconds;
+
+    var angle = gyroAngle.plus(m_gyroOffset);
+    var omega = angle.minus(m_previousAngle).getRadians() / dt;
+
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(moduleStates);
+    var fieldRelativeVelocities = new Translation2d(
+            chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond
+    ).rotateBy(angle);
+
+    var u = VecBuilder.fill(
+            fieldRelativeVelocities.getX(),
+            fieldRelativeVelocities.getY(),
+            omega
+    );
+    m_previousAngle = angle;
+
+    var localY = VecBuilder.fill(angle.getRadians());
+    m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
+    m_observer.predict(u, dt);
+    m_observer.correct(u, localY);
+
+    return getEstimatedPosition();
+  }
+}

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

@@ -31,7 +31,7 @@ import edu.wpi.first.wpiutil.math.numbers.N1;
  * an estimate of the true covariance (as opposed to a linearized version of it). This means that
  * the UKF works with nonlinear systems.
  */
-@SuppressWarnings({"MemberName", "ClassTypeParameterName"})
+@SuppressWarnings({"MemberName", "ClassTypeParameterName", "PMD.TooManyFields"})
 public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
       Outputs extends Num> implements KalmanTypeFilter<States, Inputs, Outputs> {
 
@@ -41,6 +41,12 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
   private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
   private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> m_h;
 
+  private BiFunction<Matrix<States, ?>, Matrix<?, N1>, Matrix<States, N1>> m_meanFuncX;
+  private BiFunction<Matrix<Outputs, ?>, Matrix<?, N1>, Matrix<Outputs, N1>> m_meanFuncY;
+  private BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> m_residualFuncX;
+  private BiFunction<Matrix<Outputs, N1>, Matrix<Outputs, N1>, Matrix<Outputs, N1>> m_residualFuncY;
+  private BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> m_addFuncX;
+
   private Matrix<States, N1> m_xHat;
   private Matrix<States, States> m_P;
   private final Matrix<States, States> m_contQ;
@@ -61,7 +67,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
    *                           the measurement vector.
    * @param stateStdDevs       Standard deviations of model states.
    * @param measurementStdDevs Standard deviations of measurements.
-   * @param dtSeconds          Nominal discretization timestep.
+   * @param nominalDtSeconds   Nominal discretization timestep.
    */
   @SuppressWarnings("ParameterName")
   public UnscentedKalmanFilter(Nat<States> states, Nat<Outputs> outputs,
@@ -71,18 +77,75 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
                                    Matrix<Outputs, N1>> h,
                                Matrix<States, N1> stateStdDevs,
                                Matrix<Outputs, N1> measurementStdDevs,
-                               double dtSeconds) {
+                               double nominalDtSeconds) {
+    this(
+        states, outputs,
+        f, h,
+        stateStdDevs, measurementStdDevs,
+        (sigmas, Wm) -> sigmas.times(Matrix.changeBoundsUnchecked(Wm)),
+        (sigmas, Wm) -> sigmas.times(Matrix.changeBoundsUnchecked(Wm)),
+        Matrix::minus,
+        Matrix::minus,
+        Matrix::plus,
+        nominalDtSeconds
+    );
+  }
+
+  /**
+   * Constructs an unscented Kalman filter with custom mean, residual, and addition functions. Using
+   * custom functions for arithmetic can be useful if you have angles in the state or measurements,
+   * because they allow you to correctly account for the modular nature of angle arithmetic.
+   *
+   * @param states             A Nat representing the number of states.
+   * @param outputs            A Nat representing the number of outputs.
+   * @param f                  A vector-valued function of x and u that returns
+   *                           the derivative of the state vector.
+   * @param h                  A vector-valued function of x and u that returns
+   *                           the measurement vector.
+   * @param stateStdDevs       Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param meanFuncX          A function that computes the mean of 2 * States + 1 state vectors
+   *                           using a given set of weights.
+   * @param meanFuncY          A function that computes the mean of 2 * States + 1 measurement
+   *                           vectors using a given set of weights.
+   * @param residualFuncX      A function that computes the residual of two state vectors (i.e. it
+   *                           subtracts them.)
+   * @param residualFuncY      A function that computes the residual of two measurement vectors
+   *                           (i.e. it subtracts them.)
+   * @param addFuncX           A function that adds two state vectors.
+   * @param nominalDtSeconds   Nominal discretization timestep.
+   */
+  @SuppressWarnings({"ParameterName", "PMD.ExcessiveParameterList"})
+  public UnscentedKalmanFilter(
+          Nat<States> states, Nat<Outputs> outputs,
+          BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+          BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
+          Matrix<States, N1> stateStdDevs,
+          Matrix<Outputs, N1> measurementStdDevs,
+          BiFunction<Matrix<States, ?>, Matrix<?, N1>, Matrix<States, N1>> meanFuncX,
+          BiFunction<Matrix<Outputs, ?>, Matrix<?, N1>, Matrix<Outputs, N1>> meanFuncY,
+          BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX,
+          BiFunction<Matrix<Outputs, N1>, Matrix<Outputs, N1>, Matrix<Outputs, N1>> residualFuncY,
+          BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX,
+          double nominalDtSeconds
+  ) {
     this.m_states = states;
     this.m_outputs = outputs;
 
     m_f = f;
     m_h = h;
 
+    m_meanFuncX = meanFuncX;
+    m_meanFuncY = meanFuncY;
+    m_residualFuncX = residualFuncX;
+    m_residualFuncY = residualFuncY;
+    m_addFuncX = addFuncX;
+
+    m_dtSeconds = nominalDtSeconds;
+
     m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
     m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
 
-    m_dtSeconds = dtSeconds;
-
     m_pts = new MerweScaledSigmaPoints<>(states);
 
     reset();
@@ -91,7 +154,9 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
   @SuppressWarnings({"ParameterName", "LocalVariableName", "PMD.CyclomaticComplexity"})
   static <S extends Num, C extends Num>
        Pair<Matrix<C, N1>, Matrix<C, C>> unscentedTransform(
-        Nat<S> s, Nat<C> dim, Matrix<C, ?> sigmas, Matrix<?, N1> Wm, Matrix<?, N1> Wc
+        Nat<S> s, Nat<C> dim, Matrix<C, ?> sigmas, Matrix<?, N1> Wm, Matrix<?, N1> Wc,
+        BiFunction<Matrix<C, ?>, Matrix<?, N1>, Matrix<C, N1>> meanFunc,
+        BiFunction<Matrix<C, N1>, Matrix<C, N1>, Matrix<C, N1>> residualFunc
   ) {
     if (sigmas.getNumRows() != dim.getNum() || sigmas.getNumCols() != 2 * s.getNum() + 1) {
       throw new IllegalArgumentException("Sigmas must be covDim by 2 * states + 1! Got "
@@ -108,15 +173,16 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
             + Wc.getNumRows() + " by " + Wc.getNumCols());
     }
 
-    // New mean is just the sum of the sigmas * weight
+    // New mean is usually just the sum of the sigmas * weight:
     // dot = \Sigma^n_1 (W[k]*Xi[k])
-    Matrix<C, N1> x = sigmas.times(Matrix.changeBoundsUnchecked(Wm));
+    Matrix<C, N1> x = meanFunc.apply(sigmas, Wm);
 
     // New covariance is the sum of the outer product of the residuals times the
     // weights
     Matrix<C, ?> y = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + 1));
     for (int i = 0; i < 2 * s.getNum() + 1; i++) {
-      y.setColumn(i, sigmas.extractColumnVector(i).minus(x));
+      // y[:, i] = sigmas[:, i] - x
+      y.setColumn(i, residualFunc.apply(sigmas.extractColumnVector(i), x));
     }
     Matrix<C, C> P = y.times(Matrix.changeBoundsUnchecked(Wc.diag()))
           .times(Matrix.changeBoundsUnchecked(y.transpose()));
@@ -234,7 +300,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
     }
 
     var ret = unscentedTransform(m_states, m_states,
-          m_sigmasF, m_pts.getWm(), m_pts.getWc());
+          m_sigmasF, m_pts.getWm(), m_pts.getWc(), m_meanFuncX, m_residualFuncX);
 
     m_xHat = ret.getFirst();
     m_P = ret.getSecond().plus(discQ);
@@ -250,7 +316,8 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
   @SuppressWarnings("ParameterName")
   @Override
   public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
-    correct(m_outputs, u, y, m_h, m_contR);
+    correct(m_outputs, u, y, m_h, m_contR,
+            m_meanFuncY, m_residualFuncY, m_residualFuncX, m_addFuncX);
   }
 
   /**
@@ -271,7 +338,11 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
         Nat<R> rows, Matrix<Inputs, N1> u,
         Matrix<R, N1> y,
         BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<R, N1>> h,
-        Matrix<R, R> R) {
+        Matrix<R, R> R,
+        BiFunction<Matrix<R, ?>, Matrix<?, N1>, Matrix<R, N1>> meanFuncY,
+        BiFunction<Matrix<R, N1>, Matrix<R, N1>, Matrix<R, N1>> residualFuncY,
+        BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX,
+        BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX) {
     final var discR = Discretization.discretizeR(R, m_dtSeconds);
 
     // Transform sigma points into measurement space
@@ -287,18 +358,19 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
     }
 
     // Mean and covariance of prediction passed through unscented transform
-    var transRet = unscentedTransform(m_states, rows, sigmasH, m_pts.getWm(), m_pts.getWc());
+    var transRet = unscentedTransform(m_states, rows,
+            sigmasH, m_pts.getWm(), m_pts.getWc(), meanFuncY, residualFuncY);
     var yHat = transRet.getFirst();
     var Py = transRet.getSecond().plus(discR);
 
     // Compute cross covariance of the state and the measurements
     Matrix<States, R> Pxy = new Matrix<>(m_states, rows);
     for (int i = 0; i < m_pts.getNumSigmas(); i++) {
-      var temp =
-            m_sigmasF.extractColumnVector(i).minus(m_xHat)
-                    .times(sigmasH.extractColumnVector(i).minus(yHat).transpose());
+      // Pxy += (sigmas_f[:, i] - xHat) * (sigmas_h[:, i] - yHat)^T * W_c[i]
+      var dx = residualFuncX.apply(m_sigmasF.extractColumnVector(i), m_xHat);
+      var dy = residualFuncY.apply(sigmasH.extractColumnVector(i), yHat).transpose();
 
-      Pxy = Pxy.plus(temp.times(m_pts.getWc(i)));
+      Pxy = Pxy.plus(dx.times(dy).times(m_pts.getWc(i)));
     }
 
     // K = P_{xy} Py^-1
@@ -310,7 +382,8 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
           Py.transpose().solve(Pxy.transpose()).transpose()
     );
 
-    m_xHat = m_xHat.plus(K.times(y.minus(yHat)));
+    // xHat + K * (y - yHat)
+    m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, yHat)));
     m_P = m_P.minus(K.times(Py).times(K.transpose()));
   }
 }

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

@@ -20,6 +20,7 @@ import edu.wpi.first.wpiutil.math.Num;
 import edu.wpi.first.wpiutil.math.VecBuilder;
 import edu.wpi.first.wpiutil.math.numbers.N1;
 import edu.wpi.first.wpiutil.math.numbers.N3;
+import edu.wpi.first.wpiutil.math.numbers.N4;
 
 @SuppressWarnings("ParameterName")
 public final class StateSpaceUtil {
@@ -177,4 +178,32 @@ public final class StateSpaceUtil {
     return u;
   }
 
+  /**
+   * Convert a {@link Pose2d} to a vector of [x, y, cos(theta), sin(theta)],
+   * where theta is in radians.
+   *
+   * @param pose A pose to convert to a vector.
+   */
+  public static Matrix<N4, N1> poseTo4dVector(Pose2d pose) {
+    return VecBuilder.fill(
+      pose.getTranslation().getX(),
+      pose.getTranslation().getY(),
+      pose.getRotation().getCos(),
+      pose.getRotation().getSin()
+    );
+  }
+
+  /**
+   * Convert a {@link Pose2d} to a vector of [x, y, theta], where theta is in radians.
+   *
+   * @param pose A pose to convert to a vector.
+   * @return The given pose in vector form, with the third element, theta, in radians.
+   */
+  public static Matrix<N3, N1> poseTo3dVector(Pose2d pose) {
+    return VecBuilder.fill(
+      pose.getTranslation().getX(),
+      pose.getTranslation().getY(),
+      pose.getRotation().getRadians()
+    );
+  }
 }

wpimath/src/main/native/cpp/StateSpaceUtil.cpp

@@ -9,6 +9,18 @@
 
 namespace frc {
 
+Eigen::Matrix<double, 3, 1> PoseTo3dVector(const Pose2d& pose) {
+  return frc::MakeMatrix<3, 1>(pose.Translation().X().to<double>(),
+                               pose.Translation().Y().to<double>(),
+                               pose.Rotation().Radians().to<double>());
+}
+
+Eigen::Matrix<double, 4, 1> PoseTo4dVector(const Pose2d& pose) {
+  return frc::MakeMatrix<4, 1>(pose.Translation().X().to<double>(),
+                               pose.Translation().Y().to<double>(),
+                               pose.Rotation().Cos(), pose.Rotation().Sin());
+}
+
 template <>
 bool IsStabilizable<1, 1>(const Eigen::Matrix<double, 1, 1>& A,
                           const Eigen::Matrix<double, 1, 1>& B) {

wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp

@@ -0,0 +1,143 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/estimator/DifferentialDrivePoseEstimator.h"
+
+#include <wpi/timestamp.h>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/AngleStatistics.h"
+
+using namespace frc;
+
+DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
+    const Rotation2d& gyroAngle, const Pose2d& initialPose,
+    const std::array<double, 5>& stateStdDevs,
+    const std::array<double, 3>& localMeasurementStdDevs,
+    const std::array<double, 3>& visionMeasurmentStdDevs,
+    units::second_t nominalDt)
+    : m_observer(
+          &DifferentialDrivePoseEstimator::F,
+          [](const Eigen::Matrix<double, 5, 1>& x,
+             const Eigen::Matrix<double, 3, 1>& u) {
+            return frc::MakeMatrix<3, 1>(x(3, 0), x(4, 0), x(2, 0));
+          },
+          stateStdDevs, localMeasurementStdDevs, frc::AngleMean<5, 5>(2),
+          frc::AngleMean<3, 5>(2), frc::AngleResidual<5>(2),
+          frc::AngleResidual<3>(2), frc::AngleAdd<5>(2), nominalDt),
+      m_nominalDt(nominalDt) {
+  // Create R (covariances) for vision measurements.
+  Eigen::Matrix<double, 3, 3> visionContR =
+      frc::MakeCovMatrix(visionMeasurmentStdDevs);
+  m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt);
+
+  // Create correction mechanism for vision measurements.
+  m_visionCorrect = [&](const Eigen::Matrix<double, 3, 1>& u,
+                        const Eigen::Matrix<double, 3, 1>& y) {
+    m_observer.Correct<3>(
+        u, y,
+        [](const Eigen::Matrix<double, 5, 1>& x,
+           const Eigen::Matrix<double, 3, 1>&) { return x.block<3, 1>(0, 0); },
+        m_visionDiscR, frc::AngleMean<3, 5>(2), frc::AngleResidual<3>(2),
+        frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
+  };
+
+  m_gyroOffset = initialPose.Rotation() - gyroAngle;
+  m_previousAngle = initialPose.Rotation();
+  m_observer.SetXhat(FillStateVector(initialPose, 0_m, 0_m));
+}
+
+void DifferentialDrivePoseEstimator::ResetPosition(
+    const Pose2d& pose, const Rotation2d& gyroAngle) {
+  m_previousAngle = pose.Rotation();
+  m_gyroOffset = GetEstimatedPosition().Rotation() - gyroAngle;
+  m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m));
+}
+
+Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
+  return Pose2d(units::meter_t(m_observer.Xhat(0)),
+                units::meter_t(m_observer.Xhat(1)),
+                Rotation2d(units::radian_t(m_observer.Xhat(2))));
+}
+
+void DifferentialDrivePoseEstimator::AddVisionMeasurement(
+    const Pose2d& visionRobotPose, units::second_t timestamp) {
+  m_latencyCompensator.ApplyPastMeasurement<3>(&m_observer, m_nominalDt,
+                                               PoseTo3dVector(visionRobotPose),
+                                               m_visionCorrect, timestamp);
+}
+
+Pose2d DifferentialDrivePoseEstimator::Update(
+    const Rotation2d& gyroAngle,
+    const DifferentialDriveWheelSpeeds& wheelSpeeds,
+    units::meter_t leftDistance, units::meter_t rightDistance) {
+  return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
+                        wheelSpeeds, leftDistance, rightDistance);
+}
+
+Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
+    units::second_t currentTime, const Rotation2d& gyroAngle,
+    const DifferentialDriveWheelSpeeds& wheelSpeeds,
+    units::meter_t leftDistance, units::meter_t rightDistance) {
+  auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
+  m_prevTime = currentTime;
+
+  auto angle = gyroAngle + m_gyroOffset;
+  auto omega = (gyroAngle - m_previousAngle).Radians() / dt;
+
+  auto u = frc::MakeMatrix<3, 1>(
+      (wheelSpeeds.left + wheelSpeeds.right).to<double>() / 2.0, 0.0,
+      omega.to<double>());
+
+  m_previousAngle = angle;
+
+  auto localY = frc::MakeMatrix<3, 1>(leftDistance.to<double>(),
+                                      rightDistance.to<double>(),
+                                      angle.Radians().to<double>());
+
+  m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
+  m_observer.Predict(u, dt);
+  m_observer.Correct(u, localY);
+
+  return GetEstimatedPosition();
+}
+
+Eigen::Matrix<double, 5, 1> DifferentialDrivePoseEstimator::F(
+    const Eigen::Matrix<double, 5, 1>& x,
+    const Eigen::Matrix<double, 3, 1>& u) {
+  // Apply a rotation matrix. Note that we do not add x because Runge-Kutta does
+  // that for us.
+  auto& theta = x(2, 0);
+  Eigen::Matrix<double, 5, 5> toFieldRotation = frc::MakeMatrix<5, 5>(
+      // clang-format off
+    std::cos(theta), -std::sin(theta), 0.0, 0.0, 0.0,
+    std::sin(theta), std::cos(theta), 0.0, 0.0, 0.0,
+    0.0, 0.0, 1.0, 0.0, 0.0,
+    0.0, 0.0, 0.0, 1.0, 0.0,
+    0.0, 0.0, 0.0, 0.0, 1.0);  // clang-format on
+  return toFieldRotation *
+         frc::MakeMatrix<5, 1>(u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0));
+}
+
+template <int Dim>
+std::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
+    const Eigen::Matrix<double, Dim, 1>& stdDevs) {
+  std::array<double, Dim> array;
+  for (size_t i = 0; i < Dim; ++i) {
+    array[i] = stdDevs(i);
+  }
+  return array;
+}
+
+Eigen::Matrix<double, 5, 1> DifferentialDrivePoseEstimator::FillStateVector(
+    const Pose2d& pose, units::meter_t leftDistance,
+    units::meter_t rightDistance) {
+  return frc::MakeMatrix<5, 1>(
+      pose.Translation().X().to<double>(), pose.Translation().Y().to<double>(),
+      pose.Rotation().Radians().to<double>(), leftDistance.to<double>(),
+      rightDistance.to<double>());
+}

wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp

@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/estimator/MecanumDrivePoseEstimator.h"
+
+#include <wpi/timestamp.h>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/AngleStatistics.h"
+
+using namespace frc;
+
+frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
+    const Rotation2d& gyroAngle, const Pose2d& initialPose,
+    MecanumDriveKinematics kinematics,
+    const std::array<double, 3>& stateStdDevs,
+    const std::array<double, 1>& localMeasurementStdDevs,
+    const std::array<double, 3>& visionMeasurementStdDevs,
+    units::second_t nominalDt)
+    : m_observer([](const Eigen::Matrix<double, 3, 1>& x,
+                    const Eigen::Matrix<double, 3, 1>& u) { return u; },
+                 [](const Eigen::Matrix<double, 3, 1>& x,
+                    const Eigen::Matrix<double, 3, 1>& u) {
+                   return x.block<1, 1>(2, 0);
+                 },
+                 stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2),
+                 frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2),
+                 frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt),
+      m_kinematics(kinematics),
+      m_nominalDt(nominalDt) {
+  // Construct R (covariances) matrix for vision measurements.
+  Eigen::Matrix3d visionContR = frc::MakeCovMatrix<3>(visionMeasurementStdDevs);
+
+  // Create and store discrete covariance matrix for vision measurements.
+  m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt);
+
+  // Create vision correction mechanism.
+  m_visionCorrect = [&](const Eigen::Matrix<double, 3, 1>& u,
+                        const Eigen::Matrix<double, 3, 1>& y) {
+    m_observer.Correct<3>(
+        u, y,
+        [](const Eigen::Matrix<double, 3, 1>& x,
+           const Eigen::Matrix<double, 3, 1>&) { return x; },
+        m_visionDiscR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
+        frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
+  };
+
+  // Set initial state.
+  m_observer.SetXhat(PoseTo3dVector(initialPose));
+
+  // Calculate offsets.
+  m_gyroOffset = initialPose.Rotation() - gyroAngle;
+  m_previousAngle = initialPose.Rotation();
+}
+
+void frc::MecanumDrivePoseEstimator::ResetPosition(
+    const Pose2d& pose, const Rotation2d& gyroAngle) {
+  // Set observer state.
+  m_observer.SetXhat(PoseTo3dVector(pose));
+
+  // Calculate offsets.
+  m_gyroOffset = pose.Rotation() - gyroAngle;
+  m_previousAngle = pose.Rotation();
+}
+
+Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
+  return Pose2d(m_observer.Xhat(0) * 1_m, m_observer.Xhat(1) * 1_m,
+                Rotation2d(units::radian_t{m_observer.Xhat(2)}));
+}
+
+void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
+    const Pose2d& visionRobotPose, units::second_t timestamp) {
+  m_latencyCompensator.ApplyPastMeasurement<3>(&m_observer, m_nominalDt,
+                                               PoseTo3dVector(visionRobotPose),
+                                               m_visionCorrect, timestamp);
+}
+
+Pose2d frc::MecanumDrivePoseEstimator::Update(
+    const Rotation2d& gyroAngle, const MecanumDriveWheelSpeeds& wheelSpeeds) {
+  return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
+                        wheelSpeeds);
+}
+
+Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
+    units::second_t currentTime, const Rotation2d& gyroAngle,
+    const MecanumDriveWheelSpeeds& wheelSpeeds) {
+  auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
+  m_prevTime = currentTime;
+
+  auto angle = gyroAngle + m_gyroOffset;
+  auto omega = (angle - m_previousAngle).Radians() / dt;
+
+  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+  auto fieldRelativeVelocities =
+      Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
+          .RotateBy(angle);
+
+  auto u = frc::MakeMatrix<3, 1>(fieldRelativeVelocities.X().to<double>(),
+                                 fieldRelativeVelocities.Y().to<double>(),
+                                 omega.to<double>());
+
+  auto localY = frc::MakeMatrix<1, 1>(angle.Radians().template to<double>());
+  m_previousAngle = angle;
+
+  m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
+
+  m_observer.Predict(u, dt);
+  m_observer.Correct(u, localY);
+
+  return GetEstimatedPosition();
+}

wpimath/src/main/native/include/frc/StateSpaceUtil.h

@@ -232,6 +232,24 @@ Eigen::Matrix<double, N, 1> MakeWhiteNoiseVector(
   return result;
 }
 
+/**
+ * Converts a Pose2d into a vector of [x, y, theta].
+ *
+ * @param pose The pose that is being represented.
+ *
+ * @return The vector.
+ */
+Eigen::Matrix<double, 3, 1> PoseTo3dVector(const Pose2d& pose);
+
+/**
+ * Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
+ *
+ * @param pose The pose that is being represented.
+ *
+ * @return The vector.
+ */
+Eigen::Matrix<double, 4, 1> PoseTo4dVector(const Pose2d& pose);
+
 /**
  * Returns true if (A, B) is a stabilizable pair.
  *

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

@@ -0,0 +1,128 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/math>
+
+#include "Eigen/Core"
+
+namespace frc {
+namespace {
+double NormalizeAngle(double angle) {
+  static constexpr double tau = 2 * wpi::math::pi;
+
+  angle -= std::floor(angle / tau) * tau;
+  if (angle > wpi::math::pi) angle -= tau;
+  return angle;
+}
+}  // namespace
+
+/**
+ * Subtracts a and b while normalizing the resulting value in the selected row
+ * as if it were an angle.
+ *
+ * @param a A vector to subtract from.
+ * @param b A vector to subtract with.
+ * @param angleStateIdx The row containing angles to be normalized.
+ */
+template <int States>
+Eigen::Matrix<double, States, 1> AngleResidual(
+    const Eigen::Matrix<double, States, 1>& a,
+    const Eigen::Matrix<double, States, 1>& b, int angleStateIdx) {
+  Eigen::Matrix<double, States, 1> ret = a - b;
+  ret[angleStateIdx] = NormalizeAngle(ret[angleStateIdx]);
+  return ret;
+}
+
+/**
+ * Returns a function that subtracts two vectors while normalizing the resulting
+ * value in the selected row as if it were an angle.
+ *
+ * @param angleStateIdx The row containing angles to be normalized.
+ */
+template <int States>
+std::function<
+    Eigen::Matrix<double, States, 1>(const Eigen::Matrix<double, States, 1>&,
+                                     const Eigen::Matrix<double, States, 1>&)>
+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.
+ *
+ * @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>
+Eigen::Matrix<double, States, 1> AngleAdd(
+    const Eigen::Matrix<double, States, 1>& a,
+    const Eigen::Matrix<double, States, 1>& b, int angleStateIdx) {
+  Eigen::Matrix<double, States, 1> ret = a + b;
+  ret[angleStateIdx] = NormalizeAngle(ret[angleStateIdx]);
+  return ret;
+}
+
+/**
+ * Returns a function that adds two vectors while normalizing the resulting
+ * value in the selected row as an angle.
+ *
+ * @param angleStateIdx The row containing angles to be normalized.
+ */
+template <int States>
+std::function<
+    Eigen::Matrix<double, States, 1>(const Eigen::Matrix<double, States, 1>&,
+                                     const Eigen::Matrix<double, States, 1>&)>
+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.
+ *
+ * @param sigmas Sigma points.
+ * @param Wm Weights for the mean.
+ * @param angleStateIdx The row containing the angles.
+ */
+template <int CovDim, int States>
+Eigen::Matrix<double, CovDim, 1> AngleMean(
+    const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
+    const Eigen::Matrix<double, 2 * States + 1, 1>& Wm, int angleStatesIdx) {
+  double sumSin = sigmas.row(angleStatesIdx)
+                      .unaryExpr([](auto it) { return std::sin(it); })
+                      .sum();
+  double sumCos = sigmas.row(angleStatesIdx)
+                      .unaryExpr([](auto it) { return std::cos(it); })
+                      .sum();
+
+  Eigen::Matrix<double, CovDim, 1> 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.
+ *
+ * @param angleStateIdx The row containing the angles.
+ */
+template <int CovDim, int States>
+std::function<Eigen::Matrix<double, CovDim, 1>(
+    const Eigen::Matrix<double, CovDim, 2 * States + 1>&,
+    const Eigen::Matrix<double, 2 * States + 1, 1>&)>
+AngleMean(int angleStateIdx) {
+  return [=](auto sigmas, auto Wm) {
+    return AngleMean<CovDim, States>(sigmas, Wm, angleStateIdx);
+  };
+}
+}  // namespace frc

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

@@ -0,0 +1,174 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+
+#include "Eigen/Core"
+#include "frc/estimator/KalmanFilterLatencyCompensator.h"
+#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+#include "units/time.h"
+
+namespace frc {
+/**
+ * This class wraps an Unscented Kalman Filter to fuse latency-compensated
+ * vision measurements with differential drive encoder measurements. It will
+ * correct for noisy vision measurements and encoder drift. It is intended to be
+ * an easy drop-in for 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, 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.
+ *
+ * Our state-space system is:
+ *
+ * <strong> x = [[x, y, theta, dist_l, dist_r]]^T </strong> in the field
+ * coordinate system.
+ *
+ * <strong> u = [[d_l, d_r, dtheta]]^T </strong> (robot-relative velocities) --
+ * NB: using velocities make things considerably easier, because it means that
+ * teams don't have to worry about getting an accurate model. Basically, we
+ * suspect that it's easier for teams to get good encoder data than it is for
+ * them to perform system identification well enough to get a good model
+ *
+ * <strong>y = [[x, y, theta]]^T </strong> from vision,
+ * or <strong>y = [[dist_l, dist_r, theta]] </strong> from encoders and gyro.
+ */
+class DifferentialDrivePoseEstimator {
+ public:
+  /**
+   * Constructs a DifferentialDrivePoseEstimator.
+   *
+   * @param gyroAngle                The gyro angle of the robot.
+   * @param initialPose              The estimated initial pose.
+   * @param stateStdDevs             Standard deviations of the model states.
+   *                                 Increase these to trust your wheel speeds
+   *                                 less.
+   * @param localMeasurementStdDevs  Standard deviations of the encoder
+   *                                 measurements. Increase these to trust
+   *                                 encoder distances less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision
+   *                                 measurements. Increase these to trust
+   *                                 vision less.
+   * @param nominalDt                The period of the loop calling Update().
+   */
+  DifferentialDrivePoseEstimator(
+      const Rotation2d& gyroAngle, const Pose2d& initialPose,
+      const std::array<double, 5>& stateStdDevs,
+      const std::array<double, 3>& localMeasurementStdDevs,
+      const std::array<double, 3>& visionMeasurementStdDevs,
+      units::second_t nominalDt = 0.02_s);
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * You NEED to reset your encoders to zero when calling this method. 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 estimated pose of the robot on the field.
+   *@param gyroAngle The current gyro angle.
+   */
+  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle);
+
+  /**
+   * Returns the pose of the robot at the current time as estimated by the
+   * Unscented Kalman Filter.
+   *
+   * @return The estimated robot pose.
+   */
+  Pose2d GetEstimatedPosition() const;
+
+  /**
+   * Adds a vision measurement to the Unscented 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.
+   *
+   * @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
+   *                        frc2::Timer::GetFPGATimestamp(). This means that
+   *                        you should use frc2::Timer::GetFPGATimestamp() as
+   *                        your time source in this case.
+   */
+  void AddVisionMeasurement(const Pose2d& visionRobotPose,
+                            units::second_t timestamp);
+
+  /**
+   * Updates the Unscented Kalman Filter using only wheel encoder information.
+   * Note that this should be called every loop iteration.
+   *
+   * @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,
+                const DifferentialDriveWheelSpeeds& wheelSpeeds,
+                units::meter_t leftDistance, units::meter_t rightDistance);
+
+  /**
+   * Updates the Unscented Kalman Filter using only wheel encoder information.
+   * Note that this should be called every loop iteration.
+   *
+   * @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,
+                        const DifferentialDriveWheelSpeeds& wheelSpeeds,
+                        units::meter_t leftDistance,
+                        units::meter_t rightDistance);
+
+ private:
+  UnscentedKalmanFilter<5, 3, 3> m_observer;
+  KalmanFilterLatencyCompensator<5, 3, 3, UnscentedKalmanFilter<5, 3, 3>>
+      m_latencyCompensator;
+  std::function<void(const Eigen::Matrix<double, 3, 1>& u,
+                     const Eigen::Matrix<double, 3, 1>& y)>
+      m_visionCorrect;
+
+  Eigen::Matrix<double, 3, 3> m_visionDiscR;
+
+  units::second_t m_nominalDt;
+  units::second_t m_prevTime = -1_s;
+
+  Rotation2d m_gyroOffset;
+  Rotation2d m_previousAngle;
+
+  template <int Dim>
+  static std::array<double, Dim> StdDevMatrixToArray(
+      const Eigen::Matrix<double, Dim, 1>& stdDevs);
+
+  static Eigen::Matrix<double, 5, 1> F(const Eigen::Matrix<double, 5, 1>& x,
+                                       const Eigen::Matrix<double, 3, 1>& u);
+  static Eigen::Matrix<double, 5, 1> FillStateVector(
+      const Pose2d& pose, units::meter_t leftDistance,
+      units::meter_t rightDistance);
+};
+
+}  // namespace frc

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

@@ -25,7 +25,7 @@ template <int States, int Inputs, int Outputs>
 class ExtendedKalmanFilter {
  public:
   /**
-   * Constructs an extended Kalman filter.
+   * Constructs an Extended Kalman filter.
    *
    * @param f                  A vector-valued function of x and u that returns
    *                           the derivative of the state vector.

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

@@ -0,0 +1,129 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <algorithm>
+#include <array>
+#include <functional>
+#include <utility>
+#include <vector>
+
+#include "Eigen/Core"
+#include "units/math.h"
+#include "units/time.h"
+
+namespace frc {
+
+template <int States, int Inputs, int Outputs, typename KalmanFilterType>
+class KalmanFilterLatencyCompensator {
+ public:
+  struct ObserverSnapshot {
+    Eigen::Matrix<double, States, 1> xHat;
+    Eigen::Matrix<double, States, States> errorCovariances;
+    Eigen::Matrix<double, Inputs, 1> inputs;
+    Eigen::Matrix<double, Outputs, 1> localMeasurements;
+
+    ObserverSnapshot(const KalmanFilterType& observer,
+                     const Eigen::Matrix<double, Inputs, 1>& u,
+                     const Eigen::Matrix<double, Outputs, 1>& localY)
+        : xHat(observer.Xhat()),
+          errorCovariances(observer.P()),
+          inputs(u),
+          localMeasurements(localY) {}
+  };
+
+  void AddObserverState(const KalmanFilterType& observer,
+                        Eigen::Matrix<double, Inputs, 1> u,
+                        Eigen::Matrix<double, Outputs, 1> 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());
+    }
+  }
+
+  template <int Rows>
+  void ApplyPastMeasurement(
+      KalmanFilterType* observer, units::second_t nominalDt,
+      Eigen::Matrix<double, Rows, 1> y,
+      std::function<void(const Eigen::Matrix<double, Inputs, 1>& u,
+                         const Eigen::Matrix<double, Rows, 1>& 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;
+    }
+
+    // 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.
+    auto lowerBoundIter = std::lower_bound(
+        m_pastObserverSnapshots.cbegin(), m_pastObserverSnapshots.cend(),
+        timestamp,
+        [](const auto& entry, const auto& ts) { return entry.first < ts; });
+    int index = std::distance(m_pastObserverSnapshots.cbegin(), lowerBoundIter);
+
+    // High and Low should be the same. The sampled timestamp is greater than or
+    // equal to the vision pose timestamp. We will now find the entry which is
+    // closest in time to the requested timestamp.
+
+    size_t indexOfClosestEntry =
+        units::math::abs(
+            timestamp - m_pastObserverSnapshots[std::max(index - 1, 0)].first) <
+                units::math::abs(timestamp -
+                                 m_pastObserverSnapshots[index].first)
+            ? index - 1
+            : index;
+
+    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->SetP(snapshot.errorCovariances);
+        observer->SetXhat(snapshot.xHat);
+      }
+
+      observer->Predict(snapshot.inputs, key - lastTimestamp);
+      observer->Correct(snapshot.inputs, snapshot.localMeasurements);
+
+      if (i == indexOfClosestEntry) {
+        // Note that the measurement is at a timestep close but probably not
+        // exactly equal to the timestep for which we called predict. This makes
+        // the assumption that the dt is small enough that the difference
+        // between the measurement time and the time that the inputs were
+        // captured at is very small.
+        globalMeasurementCorrect(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/frc/estimator/MecanumDrivePoseEstimator.h

@@ -0,0 +1,171 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+
+#include "Eigen/Core"
+#include "frc/estimator/KalmanFilterLatencyCompensator.h"
+#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "units/time.h"
+
+namespace frc {
+/**
+ * This class wraps an Unscented Kalman Filter to fuse latency-compensated
+ * vision measurements with mecanum drive encoder velocity measurements. It will
+ * correct for noisy measurements and encoder drift. It is intended to be an
+ * easy but more accurate drop-in for MecanumDriveOdometry.
+ *
+ * Update() should be called every robot loop. If your loops are faster or
+ * slower than the default of 0.02s, then you should change the nominal delta
+ * time 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.
+ *
+ * Our state-space system is:
+ *
+ * <strong> x = [[x, y, theta]]^T </strong> in the
+ * field-coordinate system.
+ *
+ * <strong> u = [[vx, vy, omega]]^T </strong> in the field-coordinate system.
+ *
+ * <strong> y = [[x, y, theta]]^T </strong> in field
+ * coords from vision, or <strong> y = [[theta]]^T
+ * </strong> from the gyro.
+ */
+class MecanumDrivePoseEstimator {
+ public:
+  /**
+   * Constructs a MecanumDrivePoseEstimator.
+   *
+   * @param gyroAngle                The current gyro angle.
+   * @param initialPoseMeters        The starting pose estimate.
+   * @param kinematics               A correctly-configured kinematics object
+   *                                 for your drivetrain.
+   * @param stateStdDevs             Standard deviations of model states.
+   *                                 Increase these numbers to trust your
+   *                                 wheel and gyro velocities less.
+   * @param localMeasurementStdDevs  Standard deviations of the gyro
+   *                                 measurement. Increase this number
+   *                                 to trust gyro angle measurements less.
+   * @param visionMeasurementStdDevs Standard deviations of the encoder
+   *                                 measurements. Increase these numbers
+   *                                 to trust vision less.
+   * @param nominalDt                The time in seconds between each robot
+   *                                 loop.
+   */
+  MecanumDrivePoseEstimator(
+      const Rotation2d& gyroAngle, const Pose2d& initialPose,
+      MecanumDriveKinematics kinematics,
+      const std::array<double, 3>& stateStdDevs,
+      const std::array<double, 1>& localMeasurementStdDevs,
+      const std::array<double, 3>& visionMeasurementStdDevs,
+      units::second_t nominalDt = 0.02_s);
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>You NEED to reset your encoders (to zero) when calling this method.
+   *
+   * <p>The gyroscope angle does not need to be reset in the user's robot code.
+   * The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param poseMeters The position on the field that your robot is at.
+   * @param gyroAngle  The angle reported by the gyroscope.
+   */
+  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle);
+
+  /**
+   * Gets the pose of the robot at the current time as estimated by the Extended
+   * Kalman Filter.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  Pose2d GetEstimatedPosition() const;
+
+  /**
+   * Add a vision measurement to the Unscented 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.
+   *
+   * @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 Timer#GetFPGATimestamp.) This means
+   *                        that you should use Timer#GetFPGATimestamp as your
+   *                        time source or sync the epochs.
+   */
+  void AddVisionMeasurement(const Pose2d& visionRobotPose,
+                            units::second_t timestamp);
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder
+   * information. This should be called every loop, and the correct loop period
+   * must be passed into the constructor of this class.
+   *
+   * @param gyroAngle   The current gyro angle.
+   * @param wheelSpeeds The current speeds of the mecanum drive wheels.
+   * @return The estimated pose of the robot in meters.
+   */
+  Pose2d Update(const Rotation2d& gyroAngle,
+                const MecanumDriveWheelSpeeds& wheelSpeeds);
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder
+   * information. This should be called every loop, and the correct loop period
+   * must be passed into the constructor of this class.
+   *
+   * @param currentTimeSeconds Time at which this method was called, in seconds.
+   * @param gyroAngle          The current gyroscope angle.
+   * @param wheelSpeeds        The current speeds of the mecanum drive wheels.
+   * @return The estimated pose of the robot in meters.
+   */
+  Pose2d UpdateWithTime(units::second_t currentTime,
+                        const Rotation2d& gyroAngle,
+                        const MecanumDriveWheelSpeeds& wheelSpeeds);
+
+ private:
+  UnscentedKalmanFilter<3, 3, 1> m_observer;
+  MecanumDriveKinematics m_kinematics;
+  KalmanFilterLatencyCompensator<3, 3, 1, UnscentedKalmanFilter<3, 3, 1>>
+      m_latencyCompensator;
+  std::function<void(const Eigen::Matrix<double, 3, 1>& u,
+                     const Eigen::Matrix<double, 3, 1>& y)>
+      m_visionCorrect;
+
+  Eigen::Matrix3d m_visionDiscR;
+
+  units::second_t m_nominalDt;
+  units::second_t m_prevTime = -1_s;
+
+  Rotation2d m_gyroOffset;
+  Rotation2d m_previousAngle;
+
+  template <int Dim>
+  static std::array<double, Dim> StdDevMatrixToArray(
+      const Eigen::Matrix<double, Dim, 1>& vector) {
+    std::array<double, Dim> array;
+    for (size_t i = 0; i < Dim; ++i) {
+      array[i] = vector(i);
+    }
+    return array;
+  }
+};
+
+}  // namespace frc

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

@@ -0,0 +1,260 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+#include <limits>
+
+#include <wpi/timestamp.h>
+
+#include "Eigen/Core"
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/AngleStatistics.h"
+#include "frc/estimator/KalmanFilterLatencyCompensator.h"
+#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "units/time.h"
+
+namespace frc {
+/**
+ * This class wraps an Unscented Kalman Filter to fuse latency-compensated
+ * vision measurements with swerve drive encoder velocity measurements. It will
+ * correct for noisy measurements and encoder drift. It is intended to be an
+ * easy but more accurate drop-in for SwerveDriveOdometry.
+ *
+ * Update() should be called every robot loop. If your loops are faster or
+ * slower than the default of 0.02s, then you should change the nominal delta
+ * time 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.
+ *
+ * Our state-space system is:
+ *
+ * <strong> x = [[x, y, theta]]^T </strong> in the
+ * field-coordinate system.
+ *
+ * <strong> u = [[vx, vy, omega]]^T </strong> in the field-coordinate system.
+ *
+ * <strong> y = [[x, y, std::theta]]^T </strong> in field
+ * coords from vision, or <strong> y = [[theta]]^T
+ * </strong> from the gyro.
+ */
+template <size_t NumModules>
+class SwerveDrivePoseEstimator {
+ public:
+  /**
+   * Constructs a SwerveDrivePoseEstimator.
+   *
+   * @param gyroAngle                The current gyro angle.
+   * @param initialPoseMeters        The starting pose estimate.
+   * @param kinematics               A correctly-configured kinematics object
+   *                                 for your drivetrain.
+   * @param stateStdDevs             Standard deviations of model states.
+   *                                 Increase these numbers to trust your
+   *                                 wheel and gyro velocities less.
+   * @param localMeasurementStdDevs  Standard deviations of the gyro
+   *                                 measurement. Increase this number to
+   *                                 trust gyro angle measurements less.
+   * @param visionMeasurementStdDevs Standard deviations of the encoder
+   *                                 measurements. Increase these numbers to
+   *                                 trust vision less.
+   * @param nominalDt                The time in seconds between each robot
+   *                                 loop.
+   */
+  SwerveDrivePoseEstimator(
+      const Rotation2d& gyroAngle, const Pose2d& initialPose,
+      SwerveDriveKinematics<NumModules>& kinematics,
+      const std::array<double, 3>& stateStdDevs,
+      const std::array<double, 1>& localMeasurementStdDevs,
+      const std::array<double, 3>& visionMeasurementStdDevs,
+      units::second_t nominalDt = 0.02_s)
+      : m_observer([](const Eigen::Matrix<double, 3, 1>& x,
+                      const Eigen::Matrix<double, 3, 1>& u) { return u; },
+                   [](const Eigen::Matrix<double, 3, 1>& x,
+                      const Eigen::Matrix<double, 3, 1>& u) {
+                     return x.block<1, 1>(2, 0);
+                   },
+                   stateStdDevs, localMeasurementStdDevs,
+                   frc::AngleMean<3, 3>(2), frc::AngleMean<1, 3>(0),
+                   frc::AngleResidual<3>(2), frc::AngleResidual<1>(0),
+                   frc::AngleAdd<3>(2), nominalDt),
+        m_kinematics(kinematics),
+        m_nominalDt(nominalDt) {
+    // Construct R (covariances) matrix for vision measurements.
+    Eigen::Matrix3d visionContR =
+        frc::MakeCovMatrix<3>(visionMeasurementStdDevs);
+
+    // Create and store discrete covariance matrix for vision measurements.
+    m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt);
+
+    // Create correction mechanism for vision measurements.
+    m_visionCorrect = [&](const Eigen::Matrix<double, 3, 1>& u,
+                          const Eigen::Matrix<double, 3, 1>& y) {
+      m_observer.Correct<3>(
+          u, y,
+          [](const Eigen::Matrix<double, 3, 1>& x,
+             const Eigen::Matrix<double, 3, 1>& u) { return x; },
+          m_visionDiscR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
+          frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
+    };
+
+    // Set initial state.
+    m_observer.SetXhat(PoseTo3dVector(initialPose));
+
+    // Calculate offsets.
+    m_gyroOffset = initialPose.Rotation() - gyroAngle;
+    m_previousAngle = initialPose.Rotation();
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * You NEED to reset your encoders (to zero) when calling this method.
+   *
+   * 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 pose      The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
+    // Set observer state.
+    m_observer.SetXhat(PoseTo3dVector(pose));
+
+    // Calculate offsets.
+    m_gyroOffset = pose.Rotation() - gyroAngle;
+    m_previousAngle = pose.Rotation();
+  }
+
+  /**
+   * Gets the pose of the robot at the current time as estimated by the Extended
+   * Kalman Filter.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  Pose2d GetEstimatedPosition() const {
+    return Pose2d(m_observer.Xhat(0) * 1_m, m_observer.Xhat(1) * 1_m,
+                  Rotation2d(units::radian_t{m_observer.Xhat(2)}));
+  }
+
+  /**
+   * Add a vision measurement to the Unscented 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.
+   *
+   * @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 Timer#GetFPGATimestamp.) This means
+   *                        that you should use Timer#GetFPGATimestamp as your
+   *                        time source or sync the epochs.
+   */
+  void AddVisionMeasurement(const Pose2d& visionRobotPose,
+                            units::second_t timestamp) {
+    m_latencyCompensator.ApplyPastMeasurement<3>(
+        &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
+        m_visionCorrect, timestamp);
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder
+   * information. This should be called every loop, and the correct loop period
+   * must be passed into the constructor of this class.
+   *
+   * @param gyroAngle    The current gyro angle.
+   * @param moduleStates The current velocities and rotations of the swerve
+   *                     modules.
+   * @return The estimated pose of the robot in meters.
+   */
+  template <typename... ModuleState>
+  Pose2d Update(const Rotation2d& gyroAngle, ModuleState&&... moduleStates) {
+    return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
+                          moduleStates...);
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder
+   * information. This should be called every loop, and the correct loop period
+   * must be passed into the constructor of this class.
+   *
+   * @param currentTime  Time at which this method was called, in seconds.
+   * @param gyroAngle    The current gyroscope angle.
+   * @param moduleStates The current velocities and rotations of the swerve
+   *                     modules.
+   * @return The estimated pose of the robot in meters.
+   */
+  template <typename... ModuleState>
+  Pose2d UpdateWithTime(units::second_t currentTime,
+                        const Rotation2d& gyroAngle,
+                        ModuleState&&... moduleStates) {
+    auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
+    m_prevTime = currentTime;
+
+    auto angle = gyroAngle + m_gyroOffset;
+    auto omega = (angle - m_previousAngle).Radians() / dt;
+
+    auto chassisSpeeds = m_kinematics.ToChassisSpeeds(moduleStates...);
+    auto fieldRelativeSpeeds =
+        Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
+            .RotateBy(angle);
+
+    auto u =
+        frc::MakeMatrix<3, 1>(fieldRelativeSpeeds.X().template to<double>(),
+                              fieldRelativeSpeeds.Y().template to<double>(),
+                              omega.template to<double>());
+
+    auto localY = frc::MakeMatrix<1, 1>(angle.Radians().template to<double>());
+    m_previousAngle = angle;
+
+    m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
+
+    m_observer.Predict(u, dt);
+    m_observer.Correct(u, localY);
+
+    return GetEstimatedPosition();
+  }
+
+ private:
+  UnscentedKalmanFilter<3, 3, 1> m_observer;
+  SwerveDriveKinematics<NumModules>& m_kinematics;
+  KalmanFilterLatencyCompensator<3, 3, 1, UnscentedKalmanFilter<3, 3, 1>>
+      m_latencyCompensator;
+  std::function<void(const Eigen::Matrix<double, 3, 1>& u,
+                     const Eigen::Matrix<double, 3, 1>& y)>
+      m_visionCorrect;
+
+  Eigen::Matrix3d m_visionDiscR;
+
+  units::second_t m_nominalDt;
+  units::second_t m_prevTime = -1_s;
+
+  Rotation2d m_gyroOffset;
+  Rotation2d m_previousAngle;
+
+  template <int Dim>
+  static std::array<double, Dim> StdDevMatrixToArray(
+      const Eigen::Matrix<double, Dim, 1>& vector) {
+    std::array<double, Dim> array;
+    for (size_t i = 0; i < Dim; ++i) {
+      array[i] = vector(i);
+    }
+    return array;
+  }
+};
+
+}  // namespace frc

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

@@ -50,6 +50,91 @@ class UnscentedKalmanFilter {
       : m_f(f), m_h(h) {
     m_contQ = MakeCovMatrix(stateStdDevs);
     m_contR = MakeCovMatrix(measurementStdDevs);
+    m_meanFuncX = [](auto sigmas, auto Wm) -> Eigen::Matrix<double, States, 1> {
+      return sigmas * Wm;
+    };
+    m_meanFuncY = [](auto sigmas,
+                     auto Wc) -> Eigen::Matrix<double, Outputs, 1> {
+      return sigmas * Wc;
+    };
+    m_residualFuncX = [](auto a, auto b) -> Eigen::Matrix<double, States, 1> {
+      return a - b;
+    };
+    m_residualFuncY = [](auto a, auto b) -> Eigen::Matrix<double, Outputs, 1> {
+      return a - b;
+    };
+    m_addFuncX = [](auto a, auto b) -> Eigen::Matrix<double, States, 1> {
+      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.
+   *
+   * @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<Eigen::Matrix<double, States, 1>(
+          const Eigen::Matrix<double, States, 1>&,
+          const Eigen::Matrix<double, Inputs, 1>&)>
+          f,
+      std::function<Eigen::Matrix<double, Outputs, 1>(
+          const Eigen::Matrix<double, States, 1>&,
+          const Eigen::Matrix<double, Inputs, 1>&)>
+          h,
+      const std::array<double, States>& stateStdDevs,
+      const std::array<double, Outputs>& measurementStdDevs,
+      std::function<Eigen::Matrix<double, States, 1>(
+          const Eigen::Matrix<double, States, 2 * States + 1>&,
+          const Eigen::Matrix<double, 2 * States + 1, 1>&)>
+          meanFuncX,
+      std::function<Eigen::Matrix<double, Outputs, 1>(
+          const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
+          const Eigen::Matrix<double, 2 * States + 1, 1>&)>
+          meanFuncY,
+      std::function<Eigen::Matrix<double, States, 1>(
+          const Eigen::Matrix<double, States, 1>&,
+          const Eigen::Matrix<double, States, 1>&)>
+          residualFuncX,
+      std::function<Eigen::Matrix<double, Outputs, 1>(
+          const Eigen::Matrix<double, Outputs, 1>&,
+          const Eigen::Matrix<double, Outputs, 1>&)>
+          residualFuncY,
+      std::function<Eigen::Matrix<double, States, 1>(
+          const Eigen::Matrix<double, States, 1>&,
+          const Eigen::Matrix<double, States, 1>&)>
+          addFuncX,
+      units::second_t dt)
+      : m_f(f),
+        m_h(h),
+        m_meanFuncX(meanFuncX),
+        m_meanFuncY(meanFuncY),
+        m_residualFuncX(residualFuncX),
+        m_residualFuncY(residualFuncY),
+        m_addFuncX(addFuncX) {
+    m_contQ = MakeCovMatrix(stateStdDevs);
+    m_contR = MakeCovMatrix(measurementStdDevs);
     m_dt = dt;
 
     Reset();
@@ -136,8 +221,8 @@ class UnscentedKalmanFilter {
       m_sigmasF.template block<States, 1>(0, i) = RungeKutta(m_f, x, u, dt);
     }
 
-    auto ret =
-        UnscentedTransform<States, States>(m_sigmasF, m_pts.Wm(), m_pts.Wc());
+    auto ret = UnscentedTransform<States, States>(
+        m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX);
     m_xHat = std::get<0>(ret);
     m_P = std::get<1>(ret);
 
@@ -152,7 +237,8 @@ class UnscentedKalmanFilter {
    */
   void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
                const Eigen::Matrix<double, Outputs, 1>& y) {
-    Correct<Outputs>(u, y, m_h, m_contR);
+    Correct<Outputs>(u, y, m_h, m_contR, m_meanFuncY, m_residualFuncY,
+                     m_residualFuncX, m_addFuncX);
   }
 
   /**
@@ -175,7 +261,23 @@ class UnscentedKalmanFilter {
                    const Eigen::Matrix<double, States, 1>&,
                    const Eigen::Matrix<double, Inputs, 1>&)>
                    h,
-               const Eigen::Matrix<double, Rows, Rows>& R) {
+               const Eigen::Matrix<double, Rows, Rows>& R,
+               std::function<Eigen::Matrix<double, Rows, 1>(
+                   const Eigen::Matrix<double, Rows, 2 * States + 1>&,
+                   const Eigen::Matrix<double, 2 * States + 1, 1>&)>
+                   meanFuncY,
+               std::function<Eigen::Matrix<double, Rows, 1>(
+                   const Eigen::Matrix<double, Rows, 1>&,
+                   const Eigen::Matrix<double, Rows, 1>&)>
+                   residualFuncY,
+               std::function<Eigen::Matrix<double, States, 1>(
+                   const Eigen::Matrix<double, States, 1>&,
+                   const Eigen::Matrix<double, States, 1>&)>
+                   residualFuncX,
+               std::function<Eigen::Matrix<double, States, 1>(
+                   const Eigen::Matrix<double, States, 1>&,
+                   const Eigen::Matrix<double, States, 1>)>
+                   addFuncX) {
     const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
 
     // Transform sigma points into measurement space
@@ -188,17 +290,19 @@ class UnscentedKalmanFilter {
     }
 
     // Mean and covariance of prediction passed through UT
-    auto [yHat, Py] =
-        UnscentedTransform<States, Rows>(sigmasH, m_pts.Wm(), m_pts.Wc());
+    auto [yHat, Py] = UnscentedTransform<States, Rows>(
+        sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY);
     Py += discR;
 
     // Compute cross covariance of the state and the measurements
     Eigen::Matrix<double, States, Rows> Pxy;
     Pxy.setZero();
     for (int i = 0; i < m_pts.NumSigmas(); ++i) {
-      Pxy += m_pts.Wc(i) *
-             (m_sigmasF.template block<States, 1>(0, i) - m_xHat) *
-             (sigmasH.template block<Rows, 1>(0, i) - yHat).transpose();
+      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();
     }
 
     // K = P_{xy} Py^-1
@@ -209,7 +313,7 @@ class UnscentedKalmanFilter {
     Eigen::Matrix<double, States, Rows> K =
         Py.transpose().ldlt().solve(Pxy.transpose()).transpose();
 
-    m_xHat += K * (y - yHat);
+    m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
     m_P -= K * Py * K.transpose();
   }
 
@@ -222,6 +326,26 @@ class UnscentedKalmanFilter {
       const Eigen::Matrix<double, States, 1>&,
       const Eigen::Matrix<double, Inputs, 1>&)>
       m_h;
+  std::function<Eigen::Matrix<double, States, 1>(
+      const Eigen::Matrix<double, States, 2 * States + 1>&,
+      const Eigen::Matrix<double, 2 * States + 1, 1>&)>
+      m_meanFuncX;
+  std::function<Eigen::Matrix<double, Outputs, 1>(
+      const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
+      const Eigen::Matrix<double, 2 * States + 1, 1>&)>
+      m_meanFuncY;
+  std::function<Eigen::Matrix<double, States, 1>(
+      const Eigen::Matrix<double, States, 1>&,
+      const Eigen::Matrix<double, States, 1>&)>
+      m_residualFuncX;
+  std::function<Eigen::Matrix<double, Outputs, 1>(
+      const Eigen::Matrix<double, Outputs, 1>&,
+      const Eigen::Matrix<double, Outputs, 1>)>
+      m_residualFuncY;
+  std::function<Eigen::Matrix<double, States, 1>(
+      const Eigen::Matrix<double, States, 1>&,
+      const Eigen::Matrix<double, States, 1>)>
+      m_addFuncX;
   Eigen::Matrix<double, States, 1> m_xHat;
   Eigen::Matrix<double, States, States> m_P;
   Eigen::Matrix<double, States, States> m_contQ;

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

@@ -14,8 +14,8 @@
 namespace frc {
 
 /**
- * Computes unscented transform of a set of sigma points and weights. CovDimurns
- * the mean and covariance in a tuple.
+ * Computes unscented transform of a set of sigma points and weights. CovDim
+ * returns the mean and covariance in a tuple.
  *
  * This works in conjunction with the UnscentedKalmanFilter class.
  *
@@ -34,17 +34,26 @@ std::tuple<Eigen::Matrix<double, CovDim, 1>,
            Eigen::Matrix<double, CovDim, CovDim>>
 UnscentedTransform(const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
                    const Eigen::Matrix<double, 2 * States + 1, 1>& Wm,
-                   const Eigen::Matrix<double, 2 * States + 1, 1>& Wc) {
-  // New mean is just the sum of the sigmas * weight
+                   const Eigen::Matrix<double, 2 * States + 1, 1>& Wc,
+                   std::function<Eigen::Matrix<double, CovDim, 1>(
+                       const Eigen::Matrix<double, CovDim, 2 * States + 1>&,
+                       const Eigen::Matrix<double, 2 * States + 1, 1>&)>
+                       meanFunc,
+                   std::function<Eigen::Matrix<double, CovDim, 1>(
+                       const Eigen::Matrix<double, CovDim, 1>&,
+                       const Eigen::Matrix<double, CovDim, 1>&)>
+                       residualFunc) {
+  // New mean is usually just the sum of the sigmas * weight:
   // dot = \Sigma^n_1 (W[k]*Xi[k])
-  Eigen::Matrix<double, CovDim, 1> x = sigmas * Wm;
+  Eigen::Matrix<double, CovDim, 1> x = meanFunc(sigmas, Wm);
 
   // New covariance is the sum of the outer product of the residuals times the
   // weights
   Eigen::Matrix<double, CovDim, 2 * States + 1> y;
   for (int i = 0; i < 2 * States + 1; ++i) {
+    // y[:, i] = sigmas[:, i] - x;
     y.template block<CovDim, 1>(0, i) =
-        sigmas.template block<CovDim, 1>(0, i) - x;
+        residualFunc(sigmas.template block<CovDim, 1>(0, i), x);
   }
   Eigen::Matrix<double, CovDim, CovDim> P =
       y * Eigen::DiagonalMatrix<double, 2 * States + 1>(Wc) * y.transpose();

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

@@ -76,6 +76,23 @@ class SwerveDriveKinematics {
         wpi::math::MathUsageId::kKinematics_SwerveDrive, 1);
   }
 
+  explicit SwerveDriveKinematics(
+      const std::array<Translation2d, NumModules>& wheels)
+      : m_modules{wheels} {
+    for (size_t i = 0; i < NumModules; i++) {
+      // clang-format off
+      m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
+        1, 0, (-m_modules[i].Y()).template to<double>(),
+        0, 1, (+m_modules[i].X()).template to<double>();
+      // clang-format on
+    }
+
+    m_forwardKinematics = m_inverseKinematics.householderQr();
+
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kKinematics_SwerveDrive, 1);
+  }
+
   SwerveDriveKinematics(const SwerveDriveKinematics&) = default;
 
   /**

wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/AngleStatisticsTest.java

@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+public class AngleStatisticsTest {
+  @Test
+  public void testMean() {
+    // 3 states, 2 sigmas
+    var sigmas = Matrix.mat(Nat.N3(), Nat.N2()).fill(
+            1, 1.2,
+            Math.toRadians(359), Math.toRadians(3),
+            1, 2
+    );
+    // Weights need to produce the mean of the sigmas
+    var weights = new Matrix<>(Nat.N2(), Nat.N1());
+    weights.fill(1.0 / sigmas.getNumCols());
+
+    assertTrue(AngleStatistics.angleMean(sigmas, weights, 1).isEqual(
+            VecBuilder.fill(1.1, Math.toRadians(1), 1.5), 1e-6));
+  }
+
+  @Test
+  public void testResidual() {
+    var first = VecBuilder.fill(1, Math.toRadians(1), 2);
+    var second = VecBuilder.fill(1, Math.toRadians(359), 1);
+    assertTrue(AngleStatistics.angleResidual(first, second, 1).isEqual(
+            VecBuilder.fill(0, Math.toRadians(2), 1), 1e-6));
+  }
+
+  @Test
+  public void testAdd() {
+    var first = VecBuilder.fill(1, Math.toRadians(1), 2);
+    var second = VecBuilder.fill(1, Math.toRadians(359), 1);
+    assertTrue(AngleStatistics.angleAdd(first, second, 1).isEqual(VecBuilder.fill(2, 0, 3), 1e-6));
+  }
+
+  @Test
+  public void testNormalize() {
+    assertEquals(AngleStatistics.normalizeAngle(Math.toRadians(-2000)), Math.toRadians(160), 1e-6);
+    assertEquals(AngleStatistics.normalizeAngle(Math.toRadians(358)), Math.toRadians(-2), 1e-6);
+    assertEquals(AngleStatistics.normalizeAngle(Math.toRadians(360)), 0, 1e-6);
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java

@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import java.util.List;
+import java.util.Random;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpiutil.math.MatBuilder;
+import edu.wpi.first.wpiutil.math.Nat;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class DifferentialDrivePoseEstimatorTest {
+  @SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength",
+        "PMD.AvoidInstantiatingObjectsInLoops"})
+  @Test
+  public void testAccuracy() {
+    var estimator = new DifferentialDrivePoseEstimator(new Rotation2d(), new Pose2d(),
+            new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02),
+            new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.01, 0.01, 0.001),
+            new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.1, 0.1, 0.01));
+
+    var traj = TrajectoryGenerator.generateTrajectory(
+            List.of(
+                    new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                    new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+                    new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                    new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+                    new Pose2d(0, 0, Rotation2d.fromDegrees(45))
+            ),
+            new TrajectoryConfig(10, 5));
+
+
+    var kinematics = new DifferentialDriveKinematics(1);
+    var rand = new Random(4915);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    final double visionUpdateRate = 0.1;
+    Pose2d lastVisionPose = null;
+    double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+
+    double distanceLeft = 0.0;
+    double distanceRight = 0.0;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    Trajectory.State groundtruthState;
+    DifferentialDriveWheelSpeeds input;
+    while (t <= traj.getTotalTimeSeconds()) {
+      groundtruthState = traj.sample(t);
+      input = kinematics.toWheelSpeeds(new ChassisSpeeds(
+              groundtruthState.velocityMetersPerSecond, 0.0,
+              // ds/dt * dtheta/ds = dtheta/dt
+              groundtruthState.velocityMetersPerSecond * groundtruthState.curvatureRadPerMeter
+      ));
+
+      if (lastVisionUpdateTime + visionUpdateRate + rand.nextGaussian() * 0.4 < t) {
+        if (lastVisionPose != null) {
+          estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+        }
+        var groundPose = groundtruthState.poseMeters;
+        lastVisionPose = new Pose2d(
+                new Translation2d(
+                        groundPose.getTranslation().getX() + rand.nextGaussian() * 0.1,
+                        groundPose.getTranslation().getY() + rand.nextGaussian() * 0.1
+                ),
+                new Rotation2d(rand.nextGaussian() * 0.01).plus(groundPose.getRotation())
+        );
+        lastVisionUpdateTime = t;
+      }
+
+      input.leftMetersPerSecond += rand.nextGaussian() * 0.01;
+      input.rightMetersPerSecond += rand.nextGaussian() * 0.01;
+
+      distanceLeft += input.leftMetersPerSecond * dt;
+      distanceRight += input.rightMetersPerSecond * dt;
+
+      var rotNoise = new Rotation2d(rand.nextGaussian() * 0.001);
+      var xHat = estimator.updateWithTime(
+              t,
+              groundtruthState.poseMeters.getRotation().plus(rotNoise),
+              input,
+              distanceLeft, distanceRight
+      );
+
+      double error =
+              groundtruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+            0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.035,
+            "Incorrect mean error"
+    );
+    assertEquals(
+            0.0, maxError, 0.055,
+            "Incorrect max error"
+    );
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimatorTest.java

@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import java.util.List;
+import java.util.Random;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class MecanumDrivePoseEstimatorTest {
+  @Test
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops",
+          "PMD.ExcessiveMethodLength"})
+  public void testAccuracy() {
+    var kinematics = new MecanumDriveKinematics(
+            new Translation2d(1, 1), new Translation2d(1, -1),
+            new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+    var estimator = new MecanumDrivePoseEstimator(
+            new Rotation2d(), new Pose2d(), kinematics,
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.05),
+            VecBuilder.fill(0.1, 0.1, 0.1)
+    );
+
+    var trajectory = TrajectoryGenerator.generateTrajectory(
+            List.of(new Pose2d(),
+                    new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
+                    new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
+                    new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
+                    new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
+                    new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
+            new TrajectoryConfig(0.5, 2)
+    );
+
+    var rand = new Random(5190);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    final double visionUpdateRate = 0.1;
+    Pose2d lastVisionPose = null;
+    double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    while (t <= trajectory.getTotalTimeSeconds()) {
+      var groundTruthState = trajectory.sample(t);
+
+      if (lastVisionUpdateTime + visionUpdateRate < t) {
+        if (lastVisionPose != null) {
+          estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+        }
+
+        lastVisionPose = new Pose2d(
+                new Translation2d(
+                        groundTruthState.poseMeters.getTranslation().getX()
+                                + rand.nextGaussian() * 0.1,
+                        groundTruthState.poseMeters.getTranslation().getY()
+                                + rand.nextGaussian() * 0.1
+                ),
+                new Rotation2d(
+                        rand.nextGaussian() * 0.1).plus(groundTruthState.poseMeters.getRotation())
+        );
+
+        lastVisionUpdateTime = t;
+      }
+
+      var wheelSpeeds = kinematics.toWheelSpeeds(new ChassisSpeeds(
+              groundTruthState.velocityMetersPerSecond, 0,
+              groundTruthState.velocityMetersPerSecond * groundTruthState.curvatureRadPerMeter));
+
+      wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
+
+      var xHat = estimator.updateWithTime(t,
+              groundTruthState.poseMeters.getRotation()
+                      .plus(new Rotation2d(rand.nextGaussian() * 0.05)), wheelSpeeds);
+
+      double error =
+              groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+            0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25,
+            "Incorrect mean error"
+    );
+    assertEquals(
+            0.0, maxError, 0.42,
+            "Incorrect max error"
+    );
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java

@@ -0,0 +1,125 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import java.util.List;
+import java.util.Random;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class SwerveDrivePoseEstimatorTest {
+  @Test
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops",
+        "PMD.ExcessiveMethodLength"})
+  public void testAccuracy() {
+    var kinematics = new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1),
+            new Translation2d(-1, 1)
+    );
+    var estimator = new SwerveDrivePoseEstimator(
+            new Rotation2d(), new Pose2d(), kinematics,
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.005),
+            VecBuilder.fill(0.1, 0.1, 0.1)
+    );
+
+    var trajectory = TrajectoryGenerator.generateTrajectory(
+            List.of(
+                    new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                    new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+                    new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                    new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+                    new Pose2d(0, 0, Rotation2d.fromDegrees(45))
+            ),
+            new TrajectoryConfig(0.5, 2)
+    );
+
+    var rand = new Random(4915);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    final double visionUpdateRate = 0.1;
+    Pose2d lastVisionPose = null;
+    double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    while (t <= trajectory.getTotalTimeSeconds()) {
+      var groundTruthState = trajectory.sample(t);
+
+      if (lastVisionUpdateTime + visionUpdateRate < t) {
+        if (lastVisionPose != null) {
+          estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+        }
+
+        lastVisionPose = new Pose2d(
+                new Translation2d(
+                        groundTruthState.poseMeters.getTranslation().getX()
+                                + rand.nextGaussian() * 0.1,
+                        groundTruthState.poseMeters.getTranslation().getY()
+                                + rand.nextGaussian() * 0.1
+                ),
+                new Rotation2d(
+                        rand.nextGaussian() * 0.1).plus(groundTruthState.poseMeters.getRotation())
+        );
+
+        lastVisionUpdateTime = t;
+
+      }
+
+      var moduleStates = kinematics.toSwerveModuleStates(new ChassisSpeeds(
+              groundTruthState.velocityMetersPerSecond,
+              0.0,
+              groundTruthState.velocityMetersPerSecond * groundTruthState.curvatureRadPerMeter)
+      );
+      for (var moduleState : moduleStates) {
+        moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
+        moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1;
+      }
+
+      var xHat = estimator.updateWithTime(
+              t,
+              groundTruthState.poseMeters.getRotation()
+                      .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
+              moduleStates);
+
+      double error =
+              groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+            0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25,
+            "Incorrect mean error"
+    );
+    assertEquals(
+            0.0, maxError, 0.42,
+            "Incorrect max error"
+    );
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java

@@ -222,7 +222,9 @@ public class UnscentedKalmanFilterTest {
     var R = StateSpaceUtil.makeCostMatrix(
           VecBuilder.fill(0.01, 0.01, 0.0001, 0.0001, 0.5, 0.5));
     observer.correct(Nat.N6(), u, globalY,
-          UnscentedKalmanFilterTest::getGlobalMeasurementModel, R);
+        UnscentedKalmanFilterTest::getGlobalMeasurementModel, R,
+        (sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)),
+        Matrix::minus, Matrix::minus, Matrix::plus);
 
     final var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());
 
@@ -371,7 +373,7 @@ public class UnscentedKalmanFilterTest {
               16.66666667,
               16.66666667,
               16.66666667
-          )
+          ), (sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)), Matrix::minus
     );
 
     assertTrue(

wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp

@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <gtest/gtest.h>
+
+#include "Eigen/Core"
+#include "frc/estimator/AngleStatistics.h"
+
+TEST(AngleStatisticsTest, TestMean) {
+  Eigen::Matrix<double, 3, 3> sigmas;
+  sigmas << 1, 1.2, 0, 359 * wpi::math::pi / 180, 3 * wpi::math::pi / 180, 0, 1,
+      2, 0;
+  // Weights need to produce the mean of the sigmas
+  Eigen::Vector3d weights{};
+  weights.fill(1.0 / sigmas.cols());
+
+  EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1)
+                  .isApprox(frc::AngleMean<3, 1>(sigmas, weights, 1), 1e-3));
+}
+
+TEST(AngleStatisticsTest, TestResidual) {
+  Eigen::Vector3d a(1, 1 * wpi::math::pi / 180, 2);
+  Eigen::Vector3d b(1, 359 * wpi::math::pi / 180, 1);
+
+  EXPECT_TRUE(frc::AngleResidual<3>(a, b, 1).isApprox(
+      Eigen::Vector3d(0, 2 * wpi::math::pi / 180, 1)));
+}
+
+TEST(AngleStatisticsTest, TestAdd) {
+  Eigen::Vector3d a(1, 1 * wpi::math::pi / 180, 2);
+  Eigen::Vector3d b(1, 359 * wpi::math::pi / 180, 1);
+
+  EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d(2, 0, 3)));
+}
+
+TEST(AngleStatisticsTest, TestNormalize) {
+  EXPECT_NEAR(frc::NormalizeAngle(-2000 * wpi::math::pi / 180),
+              160 * wpi::math::pi / 180, 1e-6);
+  EXPECT_NEAR(frc::NormalizeAngle(358 * wpi::math::pi / 180),
+              -2 * wpi::math::pi / 180, 1e-6);
+  EXPECT_NEAR(frc::NormalizeAngle(360 * wpi::math::pi / 180), 0, 1e-6);
+}

wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp

@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <limits>
+#include <random>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/DifferentialDrivePoseEstimator.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/DifferentialDriveOdometry.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+#include "units/angle.h"
+#include "units/length.h"
+#include "units/time.h"
+
+TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) {
+  frc::DifferentialDrivePoseEstimator estimator{frc::Rotation2d(),
+                                                frc::Pose2d(),
+                                                {0.02, 0.02, 0.01, 0.02, 0.02},
+                                                {0.01, 0.01, 0.001},
+                                                {0.1, 0.1, 0.01}};
+
+  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
+                  frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
+                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
+                  frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
+                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
+      frc::TrajectoryConfig(10_mps, 5.0_mps_sq));
+
+  frc::DifferentialDriveKinematics kinematics{1.0_m};
+  frc::DifferentialDriveOdometry odometry{frc::Rotation2d()};
+
+  std::default_random_engine generator;
+  std::normal_distribution<double> distribution(0.0, 1.0);
+
+  units::second_t dt = 0.02_s;
+  units::second_t t = 0.0_s;
+
+  units::meter_t leftDistance = 0_m;
+  units::meter_t rightDistance = 0_m;
+
+  units::second_t kVisionUpdateRate = 0.1_s;
+  frc::Pose2d lastVisionPose;
+  units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
+
+  double maxError = -std::numeric_limits<double>::max();
+  double errorSum = 0;
+
+  while (t <= trajectory.TotalTime()) {
+    auto groundTruthState = trajectory.Sample(t);
+    auto input = kinematics.ToWheelSpeeds(
+        {groundTruthState.velocity, 0_mps,
+         groundTruthState.velocity * groundTruthState.curvature});
+
+    if (lastVisionUpdateTime + kVisionUpdateRate < t) {
+      if (lastVisionPose != frc::Pose2d()) {
+        estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+      }
+      lastVisionPose =
+          groundTruthState.pose +
+          frc::Transform2d(
+              frc::Translation2d(distribution(generator) * 0.1 * 1_m,
+                                 distribution(generator) * 0.1 * 1_m),
+              frc::Rotation2d(distribution(generator) * 0.01 * 1_rad));
+
+      lastVisionUpdateTime = t;
+    }
+
+    leftDistance += input.left * distribution(generator) * 0.01 * dt;
+    rightDistance += input.right * distribution(generator) * 0.01 * dt;
+
+    auto xhat = estimator.UpdateWithTime(
+        t,
+        groundTruthState.pose.Rotation() +
+            frc::Rotation2d(units::radian_t(distribution(generator) * 0.001)),
+        input, leftDistance, rightDistance);
+
+    double error = groundTruthState.pose.Translation()
+                       .Distance(xhat.Translation())
+                       .to<double>();
+
+    if (error > maxError) {
+      maxError = error;
+    }
+    errorSum += error;
+
+    t += dt;
+  }
+
+  EXPECT_NEAR(
+      0.0, errorSum / (trajectory.TotalTime().to<double>() / dt.to<double>()),
+      0.2);
+  EXPECT_NEAR(0.0, maxError, 0.4);
+}

wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp

@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <limits>
+#include <random>
+
+#include "frc/estimator/MecanumDrivePoseEstimator.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveOdometry.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+
+TEST(MecanumDrivePoseEstimatorTest, TestAccuracy) {
+  frc::MecanumDriveKinematics kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+  frc::MecanumDrivePoseEstimator estimator{
+      frc::Rotation2d(), frc::Pose2d(), kinematics,
+      {0.1, 0.1, 0.1},   {0.05},        {0.1, 0.1, 0.1}};
+
+  frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d()};
+
+  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
+                  frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
+                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
+                  frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
+                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
+      frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+
+  std::default_random_engine generator;
+  std::normal_distribution<double> distribution(0.0, 1.0);
+
+  units::second_t dt = 0.02_s;
+  units::second_t t = 0_s;
+
+  units::second_t kVisionUpdateRate = 0.1_s;
+  frc::Pose2d lastVisionPose;
+  units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
+
+  std::vector<frc::Pose2d> visionPoses;
+
+  double maxError = -std::numeric_limits<double>::max();
+  double errorSum = 0;
+
+  while (t < trajectory.TotalTime()) {
+    frc::Trajectory::State groundTruthState = trajectory.Sample(t);
+
+    if (lastVisionUpdateTime + kVisionUpdateRate < t) {
+      if (lastVisionPose != frc::Pose2d()) {
+        estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+      }
+      lastVisionPose =
+          groundTruthState.pose +
+          frc::Transform2d(
+              frc::Translation2d(distribution(generator) * 0.1_m,
+                                 distribution(generator) * 0.1_m),
+              frc::Rotation2d(distribution(generator) * 0.1 * 1_rad));
+      visionPoses.push_back(lastVisionPose);
+      lastVisionUpdateTime = t;
+    }
+
+    auto wheelSpeeds = kinematics.ToWheelSpeeds(
+        {groundTruthState.velocity, 0_mps,
+         groundTruthState.velocity * groundTruthState.curvature});
+
+    auto xhat = estimator.UpdateWithTime(
+        t,
+        groundTruthState.pose.Rotation() +
+            frc::Rotation2d(distribution(generator) * 0.05_rad),
+        wheelSpeeds);
+    double error = groundTruthState.pose.Translation()
+                       .Distance(xhat.Translation())
+                       .to<double>();
+
+    if (error > maxError) {
+      maxError = error;
+    }
+    errorSum += error;
+
+    t += dt;
+  }
+
+  EXPECT_LT(errorSum / (trajectory.TotalTime().to<double>() / dt.to<double>()),
+            0.2);
+  EXPECT_LT(maxError, 0.4);
+}

wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp

@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <limits>
+#include <random>
+
+#include "frc/estimator/SwerveDrivePoseEstimator.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "frc/kinematics/SwerveDriveOdometry.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+
+TEST(SwerveDrivePoseEstimatorTest, TestAccuracy) {
+  frc::SwerveDriveKinematics<4> kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+  frc::SwerveDrivePoseEstimator<4> estimator{
+      frc::Rotation2d(), frc::Pose2d(), kinematics,
+      {0.1, 0.1, 0.1},   {0.05},        {0.1, 0.1, 0.1}};
+
+  frc::SwerveDriveOdometry<4> odometry{kinematics, frc::Rotation2d()};
+
+  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
+                  frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
+                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
+                  frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
+                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
+      frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+
+  std::default_random_engine generator;
+  std::normal_distribution<double> distribution(0.0, 1.0);
+
+  units::second_t dt = 0.02_s;
+  units::second_t t = 0_s;
+
+  units::second_t kVisionUpdateRate = 0.1_s;
+  frc::Pose2d lastVisionPose;
+  units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
+
+  std::vector<frc::Pose2d> visionPoses;
+
+  double maxError = -std::numeric_limits<double>::max();
+  double errorSum = 0;
+
+  while (t < trajectory.TotalTime()) {
+    frc::Trajectory::State groundTruthState = trajectory.Sample(t);
+
+    if (lastVisionUpdateTime + kVisionUpdateRate < t) {
+      if (lastVisionPose != frc::Pose2d()) {
+        estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+      }
+      lastVisionPose =
+          groundTruthState.pose +
+          frc::Transform2d(
+              frc::Translation2d(distribution(generator) * 0.1_m,
+                                 distribution(generator) * 0.1_m),
+              frc::Rotation2d(distribution(generator) * 0.1 * 1_rad));
+      visionPoses.push_back(lastVisionPose);
+      lastVisionUpdateTime = t;
+    }
+
+    auto moduleStates = kinematics.ToSwerveModuleStates(
+        {groundTruthState.velocity, 0_mps,
+         groundTruthState.velocity * groundTruthState.curvature});
+
+    auto xhat = estimator.UpdateWithTime(
+        t,
+        groundTruthState.pose.Rotation() +
+            frc::Rotation2d(distribution(generator) * 0.05_rad),
+        moduleStates[0], moduleStates[1], moduleStates[2], moduleStates[3]);
+    double error = groundTruthState.pose.Translation()
+                       .Distance(xhat.Translation())
+                       .to<double>();
+
+    if (error > maxError) {
+      maxError = error;
+    }
+    errorSum += error;
+
+    t += dt;
+  }
+
+  EXPECT_LT(errorSum / (trajectory.TotalTime().to<double>() / dt.to<double>()),
+            0.2);
+  EXPECT_LT(maxError, 0.4);
+}

wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp

@@ -13,6 +13,7 @@
 #include "Eigen/Core"
 #include "Eigen/QR"
 #include "frc/StateSpaceUtil.h"
+#include "frc/estimator/AngleStatistics.h"
 #include "frc/estimator/UnscentedKalmanFilter.h"
 #include "frc/system/NumericalJacobian.h"
 #include "frc/system/RungeKutta.h"
@@ -94,7 +95,9 @@ TEST(UnscentedKalmanFilterTest, Init) {
 
   auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
   auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
-  observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
+  observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
+                      frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
+                      frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
 }
 
 TEST(UnscentedKalmanFilterTest, Convergence) {
@@ -162,7 +165,11 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
 
   auto globalY = GlobalMeasurementModel(trueXhat, u);
   auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
-  observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
+  observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
+                      frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
+                      frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)
+
+  );
 
   auto finalPosition = trajectory.Sample(trajectory.TotalTime());
   ASSERT_NEAR(finalPosition.pose.Translation().X().template to<double>(),