diff --git a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
index 2b80b75d887..75cea91f555 100644
--- a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
+++ b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
@@ -7,36 +7,45 @@
 
 #include "frc/estimator/DifferentialDrivePoseEstimator.h"
 
-#include "frc/StateSpaceUtil.h"
+#include <frc/StateSpaceUtil.h>
+
 #include "frc2/Timer.h"
 
 using namespace frc;
 
 DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
     const Rotation2d& gyroAngle, const Pose2d& initialPose,
-    const Vector<5>& stateStdDevs, const Vector<3>& localMeasurementStdDevs,
-    const Vector<3>& visionMeasurmentStdDevs, units::second_t nominalDt)
-    : m_observer(
+    const std::array<double, 5>& stateStdDevs,
+    const std::array<double, 3>& localMeasurementStdDevs,
+    const std::array<double, 3>& visionMeasurementStdDevs,
+    units::second_t nominalDt)
+    : m_stateStdDevs(stateStdDevs),
+      m_localMeasurementStdDevs(localMeasurementStdDevs),
+      m_observer(
           &DifferentialDrivePoseEstimator::F,
-          [](const Vector<5>& x, const Vector<3>& u) {
-            return frc::MakeMatrix<3, 1>(x(3, 0), x(4, 0), x(2, 0));
-          },
-          StdDevMatrixToArray<5>(stateStdDevs),
-          StdDevMatrixToArray<3>(localMeasurementStdDevs), nominalDt),
+          &DifferentialDrivePoseEstimator::LocalMeasurementModel,
+          MakeQDiagonals(stateStdDevs, FillStateVector(initialPose, 0_m, 0_m)),
+          MakeRDiagonals(localMeasurementStdDevs,
+                         FillStateVector(initialPose, 0_m, 0_m)),
+          nominalDt),
       m_nominalDt(nominalDt) {
   // Create R (covariances) for vision measurements.
   Eigen::Matrix<double, 3, 3> visionContR =
-      frc::MakeCovMatrix(StdDevMatrixToArray<3>(visionMeasurmentStdDevs));
-  m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt);
+      frc::MakeCovMatrix<3>(visionMeasurementStdDevs);
 
   // Create correction mechanism for vision measurements.
-  m_visionCorrect = [&](const Vector<3>& u, const Vector<3>& y) {
-    m_observer.Correct<3>(
+  m_visionCorrect = [&](const Eigen::Matrix<double, 3, 1>& u,
+                        const Eigen::Matrix<double, 4, 1>& y) {
+    m_observer.Correct<4>(
         u, y,
-        [](const Vector<5>& x, const Vector<3>&) {
-          return x.block<3, 1>(0, 0);
+        [](const Eigen::Matrix<double, 6, 1>& x_,
+           const Eigen::Matrix<double, 3, 1>& u_) {
+          return x_.block<4, 1>(0, 0);
         },
-        m_visionDiscR);
+        DiscretizeR<4>(
+            MakeCovMatrix<4>(DifferentialDrivePoseEstimator::MakeRDiagonals(
+                visionMeasurementStdDevs, m_observer.Xhat())),
+            m_nominalDt));
   };
 
   m_gyroOffset = initialPose.Rotation() - gyroAngle;
@@ -54,13 +63,13 @@ void DifferentialDrivePoseEstimator::ResetPosition(
 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))));
+                Rotation2d(m_observer.Xhat(2), m_observer.Xhat(3)));
 }
 
 void DifferentialDrivePoseEstimator::AddVisionMeasurement(
     const Pose2d& visionRobotPose, units::second_t timestamp) {
-  m_latencyCompensator.ApplyPastMeasurement<3>(&m_observer, m_nominalDt,
-                                               PoseTo3dVector(visionRobotPose),
+  m_latencyCompensator.ApplyPastMeasurement<4>(&m_observer, m_nominalDt,
+                                               PoseTo4dVector(visionRobotPose),
                                                m_visionCorrect, timestamp);
 }
 
@@ -82,42 +91,69 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
   auto angle = gyroAngle + m_gyroOffset;
   auto omega = (gyroAngle - m_previousAngle).Radians() / dt;
 
-  auto u = frc::MakeMatrix<3, 1>(
+  Eigen::Matrix<double, 3, 1> 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>());
+  Eigen::Matrix<double, 4, 1> localY = frc::MakeMatrix<4, 1>(
+      leftDistance.to<double>(), rightDistance.to<double>(), angle.Cos(),
+      angle.Sin());
 
   m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
-  m_observer.Predict(u, dt);
-  m_observer.Correct(u, localY);
+  m_observer.Predict(
+      u,
+      frc::MakeCovMatrix<6>(MakeQDiagonals(m_stateStdDevs, m_observer.Xhat())),
+      dt);
+  m_observer.Correct<4>(u, localY, &LocalMeasurementModel,
+                        frc::MakeCovMatrix<4>(MakeRDiagonals(
+                            m_localMeasurementStdDevs, m_observer.Xhat())));
 
   return GetEstimatedPosition();
 }
 
-Vector<5> DifferentialDrivePoseEstimator::F(const Vector<5>& x,
-                                            const Vector<3>& u) {
+Eigen::Matrix<double, 6, 1> DifferentialDrivePoseEstimator::F(
+    const Eigen::Matrix<double, 6, 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));
+  double cosTheta = x(2);
+  double sinTheta = x(3);
+
+  // We want vx and vy to be in the field frame, so we apply a rotation matrix.
+  // to u_Field = u = [[vx_field, vy_field, omega]]^T
+  Eigen::Matrix<double, 2, 1> chassisVel_local =
+      MakeMatrix<2, 1>((u(0, 0) + u(1, 0)) / 2.0, 0.0);
+  Eigen::Matrix<double, 2, 2> toFieldRotation =
+      MakeMatrix<2, 2>(cosTheta, -sinTheta, sinTheta, cosTheta);
+  Eigen::Matrix<double, 2, 1> chassisVel_field =
+      toFieldRotation * chassisVel_local;
+
+  // dcos(theta)/dt = -std::sin(theta) * dtheta/dt = -std::sin(theta) * omega
+  double dcosTheta = -sinTheta * u(2, 0);
+  // dsin(theta)/dt = std::cos(theta) * omega
+  double dsinTheta = cosTheta * u(2, 0);
+
+  // As x = [[x_field, y_field, std::cos(theta), std::sin(theta), dist_l,
+  // dist_r]]^T, we need to return x-dot = [[vx_field, vy_field, d/dt
+  // std::cos(theta), d/dt sin(theta), vel_left, vel_right]]^T Assuming  no
+  // wheel slip, vx = (v_left + v_right) / 2, and vy = 0;
+
+  return MakeMatrix<6, 1>(chassisVel_field(0), chassisVel_field(1), dcosTheta,
+                          dsinTheta, u(0), u(1));
+}
+
+Eigen::Matrix<double, 4, 1>
+DifferentialDrivePoseEstimator::LocalMeasurementModel(
+    const Eigen::Matrix<double, 6, 1>& x,
+    const Eigen::Matrix<double, 3, 1>& u) {
+  return frc::MakeMatrix<4, 1>(x(4), x(5), x(2), x(3));
 }
 
 template <int Dim>
 std::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
-    const Vector<Dim>& stdDevs) {
+    const Eigen::Matrix<double, Dim, 1>& stdDevs) {
   std::array<double, Dim> array;
   for (size_t i = 0; i < Dim; ++i) {
     array[i] = stdDevs(i);
@@ -125,11 +161,24 @@ std::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
   return array;
 }
 
-Vector<5> DifferentialDrivePoseEstimator::FillStateVector(
+Eigen::Matrix<double, 6, 1> DifferentialDrivePoseEstimator::FillStateVector(
     const Pose2d& pose, units::meter_t leftDistance,
     units::meter_t rightDistance) {
-  return frc::MakeMatrix<5, 1>(
+  return frc::MakeMatrix<6, 1>(
       pose.Translation().X().to<double>(), pose.Translation().Y().to<double>(),
-      pose.Rotation().Radians().to<double>(), leftDistance.to<double>(),
+      pose.Rotation().Cos(), pose.Rotation().Sin(), leftDistance.to<double>(),
       rightDistance.to<double>());
 }
+
+std::array<double, 6> DifferentialDrivePoseEstimator::MakeQDiagonals(
+    const std::array<double, 5>& stdDevs,
+    const Eigen::Matrix<double, 6, 1>& x) {
+  return {stdDevs[0],        stdDevs[1], stdDevs[2] * x(2),
+          stdDevs[2] * x(3), stdDevs[3], stdDevs[4]};
+}
+
+std::array<double, 4> DifferentialDrivePoseEstimator::MakeRDiagonals(
+    const std::array<double, 3>& stdDevs,
+    const Eigen::Matrix<double, 6, 1>& x) {
+  return {stdDevs[0], stdDevs[1], stdDevs[2] * x(2), stdDevs[2] * x(3)};
+}
diff --git a/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp
index 00c993b76ed..3ebf22f826a 100644
--- a/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp
+++ b/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp
@@ -13,35 +13,37 @@
 using namespace frc;
 
 DifferentialDriveStateEstimator::DifferentialDriveStateEstimator(
-    const LinearSystem<2, 2, 2>& plant, const Eigen::Matrix<double, 10, 1>& initialState,
-    const Eigen::Matrix<double, 10, 1>& stateStdDevs, const Eigen::Matrix<double, 3, 1>& localMeasurementStdDevs,
-    const Eigen::Matrix<double, 3, 1>& globalMeasurementStdDevs,
+    const LinearSystem<2, 2, 2>& plant,
+    const std::array<double, 10>& initialState,
+    const std::array<double, 10>& stateStdDevs,
+    const std::array<double, 3>& localMeasurementStdDevs,
+    const std::array<double, 3>& globalMeasurementStdDevs,
     const DifferentialDriveKinematics& kinematics, units::second_t nominalDt)
     : m_plant(plant),
       m_rb(kinematics.trackWidth / 2.0),
       m_observer([this](auto& x, auto& u) { return Dynamics(x, u); },
                  &DifferentialDriveStateEstimator::LocalMeasurementModel,
-                 StdDevMatrixToArray<10>(stateStdDevs),
-                 StdDevMatrixToArray<3>(localMeasurementStdDevs), nominalDt),
+                 stateStdDevs, localMeasurementStdDevs, nominalDt),
       m_nominalDt(nominalDt) {
   m_localY.setZero();
   m_globalY.setZero();
 
   // Create R (covariances) for global measurements.
   Eigen::Matrix<double, 3, 3> globalContR =
-      frc::MakeCovMatrix(StdDevMatrixToArray<3>(globalMeasurementStdDevs));
+      frc::MakeCovMatrix(globalMeasurementStdDevs);
 
   Eigen::Matrix<double, 3, 3> globalDiscR =
       frc::DiscretizeR<3>(globalContR, m_nominalDt);
 
   // Create correction mechanism for global measurements.
-  m_globalCorrect = [&](const Eigen::Matrix<double, 2, 1>& u, const Eigen::Matrix<double, 3, 1>& y) {
+  m_globalCorrect = [&](const Eigen::Matrix<double, 2, 1>& u,
+                        const Eigen::Matrix<double, 3, 1>& y) {
     m_observer.Correct<3>(
         u, y, &DifferentialDriveStateEstimator::GlobalMeasurementModel,
         globalDiscR);
   };
 
-  Reset(initialState);
+  Reset(ArrayToVector<10>(initialState));
 }
 
 void DifferentialDriveStateEstimator::ApplyPastGlobalMeasurement(
@@ -51,28 +53,30 @@ void DifferentialDriveStateEstimator::ApplyPastGlobalMeasurement(
                                                m_globalCorrect, timestamp);
 }
 
-Vector<10> DifferentialDriveStateEstimator::GetEstimatedState() const {
+Eigen::Matrix<double, 10, 1>
+DifferentialDriveStateEstimator::GetEstimatedState() const {
   return m_observer.Xhat();
 }
 
 frc::Pose2d DifferentialDriveStateEstimator::GetEstimatedPosition() {
-  Vector<10> xHat = GetEstimatedState();
+  Eigen::Matrix<double, 10, 1> xHat = GetEstimatedState();
   return frc::Pose2d(
-    units::meter_t(xHat(State::kX, 0)),
-    units::meter_t(xHat(State::kY, 0)),
-    frc::Rotation2d(units::radian_t(xHat(State::kHeading, 0))));
+      units::meter_t(xHat(State::kX, 0)), units::meter_t(xHat(State::kY, 0)),
+      frc::Rotation2d(units::radian_t(xHat(State::kHeading, 0))));
 }
 
-Vector<10> DifferentialDriveStateEstimator::Update(
+Eigen::Matrix<double, 10, 1> DifferentialDriveStateEstimator::Update(
     units::radian_t heading, units::meter_t leftPosition,
-    units::meter_t rightPosition, const Eigen::Matrix<double, 2, 1>& controlInput) {
+    units::meter_t rightPosition,
+    const Eigen::Matrix<double, 2, 1>& controlInput) {
   return UpdateWithTime(heading, leftPosition, rightPosition, controlInput,
                         frc2::Timer::GetFPGATimestamp());
 }
 
-Vector<10> DifferentialDriveStateEstimator::UpdateWithTime(
+Eigen::Matrix<double, 10, 1> DifferentialDriveStateEstimator::UpdateWithTime(
     units::radian_t heading, units::meter_t leftPosition,
-    units::meter_t rightPosition, const Eigen::Matrix<double, 2, 1>& controlInput,
+    units::meter_t rightPosition,
+    const Eigen::Matrix<double, 2, 1>& controlInput,
     units::second_t currentTime) {
   auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
   m_prevTime = currentTime;
@@ -90,7 +94,8 @@ Vector<10> DifferentialDriveStateEstimator::UpdateWithTime(
   return GetEstimatedState();
 }
 
-void DifferentialDriveStateEstimator::Reset(const Eigen::Matrix<double, 10, 1>& initialState) {
+void DifferentialDriveStateEstimator::Reset(
+    const Eigen::Matrix<double, 10, 1>& initialState) {
   m_observer.Reset();
 
   m_observer.SetXhat(initialState);
@@ -98,8 +103,9 @@ void DifferentialDriveStateEstimator::Reset(const Eigen::Matrix<double, 10, 1>&
 
 void DifferentialDriveStateEstimator::Reset() { m_observer.Reset(); }
 
-Vector<10> DifferentialDriveStateEstimator::Dynamics(const Eigen::Matrix<double, 10, 1>& x,
-                                                     const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Matrix<double, 10, 1> DifferentialDriveStateEstimator::Dynamics(
+    const Eigen::Matrix<double, 10, 1>& x,
+    const Eigen::Matrix<double, 2, 1>& u) {
   Eigen::Matrix<double, 4, 2> B;
   B.block<2, 2>(0, 0) = m_plant.B();
   B.block<2, 2>(2, 0).setZero();
@@ -124,8 +130,10 @@ Vector<10> DifferentialDriveStateEstimator::Dynamics(const Eigen::Matrix<double,
   return result;
 }
 
-Vector<3> DifferentialDriveStateEstimator::LocalMeasurementModel(
-    const Eigen::Matrix<double, 10, 1>& x, const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Matrix<double, 3, 1>
+DifferentialDriveStateEstimator::LocalMeasurementModel(
+    const Eigen::Matrix<double, 10, 1>& x,
+    const Eigen::Matrix<double, 2, 1>& u) {
   static_cast<void>(u);
 
   Eigen::Matrix<double, 3, 1> y;
@@ -134,8 +142,10 @@ Vector<3> DifferentialDriveStateEstimator::LocalMeasurementModel(
   return y;
 }
 
-Vector<3> DifferentialDriveStateEstimator::GlobalMeasurementModel(
-    const Eigen::Matrix<double, 10, 1>& x, const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Matrix<double, 3, 1>
+DifferentialDriveStateEstimator::GlobalMeasurementModel(
+    const Eigen::Matrix<double, 10, 1>& x,
+    const Eigen::Matrix<double, 2, 1>& u) {
   static_cast<void>(u);
 
   Eigen::Matrix<double, 3, 1> y;
diff --git a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
index 4bb97fc84f6..5b39beaaf95 100644
--- a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
+++ b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
@@ -15,42 +15,47 @@ using namespace frc;
 
 frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
     const Rotation2d& gyroAngle, const Pose2d& initialPose,
-    MecanumDriveKinematics kinematics, const Vector<3>& stateStdDevs,
-    const Vector<1>& localMeasurementStdDevs,
-    const Vector<3>& visionMeasurementStdDevs, units::second_t nominalDt)
-    : m_observer(
+    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_stateStdDevs(stateStdDevs),
+      m_localMeasurementStdDevs(localMeasurementStdDevs),
+      m_visionMeasurementStdDevs(visionMeasurementStdDevs),
+      m_observer(
           &MecanumDrivePoseEstimator::F,
-          [](const Vector<4>& x, const Vector<3>& u) {
+          [](const Eigen::Matrix<double, 4, 1>& x,
+             const Eigen::Matrix<double, 3, 1>& u) {
             return x.block<2, 1>(2, 0);
           },
-          StdDevMatrixToArray<4>(frc::MakeMatrix<4, 1>(
-              stateStdDevs(0), stateStdDevs(1), std::cos(stateStdDevs(2)),
-              std::sin(stateStdDevs(2)))),
-          StdDevMatrixToArray<2>(
-              frc::MakeMatrix<2, 1>(std::cos(localMeasurementStdDevs(0)),
-                                    std::sin(localMeasurementStdDevs(0)))),
+          // X and Y here are zero because we only use the cosine and sin
+          // states of X in the diagonal.
+          MakeQDiagonals(
+              stateStdDevs,
+              frc::MakeMatrix<4, 1>(0.0, 0.0, initialPose.Rotation().Cos(),
+                                    initialPose.Rotation().Sin())),
+          MakeRDiagonals(
+              localMeasurementStdDevs,
+              frc::MakeMatrix<4, 1>(0.0, 0.0, initialPose.Rotation().Cos(),
+                                    initialPose.Rotation().Sin())),
           nominalDt),
       m_kinematics(kinematics),
       m_nominalDt(nominalDt) {
-  // Construct R (covariances) matrix for vision measurements.
-  Eigen::Matrix4d visionContR =
-      frc::MakeCovMatrix<4>(StdDevMatrixToArray<4>(frc::MakeMatrix<4, 1>(
-          visionMeasurementStdDevs(0), visionMeasurementStdDevs(1),
-          std::cos(visionMeasurementStdDevs(2)),
-          std::sin(visionMeasurementStdDevs(2)))));
-
-  // Create and store discrete covariance matrix for vision measurements.
-  m_visionDiscR = frc::DiscretizeR<4>(visionContR, m_nominalDt);
-
-  // Create vision correction mechanism.
-  m_visionCorrect = [&](const Vector<3>& u, const Vector<4>& y) {
+  // Create correction mechanism for vision measurements.
+  m_visionCorrect = [&](const Eigen::Matrix<double, 3, 1>& u,
+                        const Eigen::Matrix<double, 4, 1>& y) {
     m_observer.Correct<4>(
-        u, y, [](const Vector<4>& x, const Vector<3>& u) { return x; },
-        m_visionDiscR);
+        u, y,
+        [](const Eigen::Matrix<double, 4, 1>& x,
+           const Eigen::Matrix<double, 3, 1>& u) { return x; },
+        DiscretizeR<4>(
+            MakeCovMatrix<4>(MakeVisionRDiagonals(visionMeasurementStdDevs, y)),
+            nominalDt));
   };
 
   // Set initial state.
-  m_observer.SetXhat(PoseTo4dVector(initialPose));
+  ResetPosition(initialPose, gyroAngle);
 
   // Calculate offsets.
   m_gyroOffset = initialPose.Rotation() - gyroAngle;
@@ -103,20 +108,39 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
                                  fieldRelativeVelocities.Y().to<double>(),
                                  omega.to<double>());
 
-  auto localY =
-      frc::MakeMatrix<2, 1>(std::cos(angle.Radians().template to<double>()),
-                            std::sin(angle.Radians().template to<double>()));
+  auto localY = frc::MakeMatrix<2, 1>(angle.Cos(), angle.Sin());
   m_previousAngle = angle;
 
   m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
 
-  m_observer.Predict(u, dt);
-  m_observer.Correct(u, localY);
+  m_observer.Predict(
+      u,
+      frc::MakeCovMatrix<4>(MakeQDiagonals(m_stateStdDevs, m_observer.Xhat())),
+      dt);
+  m_observer.Correct<2>(
+      u, localY,
+      [](const Eigen::Matrix<double, 4, 1>& x,
+         const Eigen::Matrix<double, 3, 1>& u) { return x.block<2, 1>(2, 0); },
+      frc::MakeCovMatrix<2>(
+          MakeRDiagonals(m_localMeasurementStdDevs, m_observer.Xhat())));
 
   return GetEstimatedPosition();
 }
 
-Vector<4> frc::MecanumDrivePoseEstimator::F(const Vector<4>& x,
-                                            const Vector<3>& u) {
+std::array<double, 2> MecanumDrivePoseEstimator::MakeRDiagonals(
+    const std::array<double, 1>& stdDevs,
+    const Eigen::Matrix<double, 4, 1>& x) {
+  return {stdDevs[0] * x(2), stdDevs[0] * x(3)};
+}
+
+std::array<double, 4> MecanumDrivePoseEstimator::MakeVisionRDiagonals(
+    const std::array<double, 3>& stdDevs,
+    const Eigen::Matrix<double, 4, 1>& y) {
+  return {stdDevs[0], stdDevs[1], stdDevs[2] * y(2), stdDevs[2] * y(3)};
+}
+
+Eigen::Matrix<double, 4, 1> frc::MecanumDrivePoseEstimator::F(
+    const Eigen::Matrix<double, 4, 1>& x,
+    const Eigen::Matrix<double, 3, 1>& u) {
   return frc::MakeMatrix<4, 1>(u(0), u(1), -x(3) * u(2), x(2) * u(2));
 }
diff --git a/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
index 1eb23ed2787..45ee838a4b1 100644
--- a/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
+++ b/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
@@ -12,17 +12,14 @@
 #include <Eigen/Core>
 #include <units/time.h>
 
-#include "frc/estimator/ExtendedKalmanFilter.h"
 #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"
 
 namespace frc {
 
-template <int N>
-using Vector = Eigen::Matrix<double, N, 1>;
-
 /**
  * This class wraps an Extended Kalman Filter to fuse latency-compensated
  * vision measurements with differential drive encoder measurements. It will
@@ -68,12 +65,12 @@ class DifferentialDrivePoseEstimator {
    *                                 vision less.
    * @param nominalDt                The period of the loop calling Update().
    */
-  DifferentialDrivePoseEstimator(const Rotation2d& gyroAngle,
-                                 const Pose2d& initialPose,
-                                 const Vector<5>& stateStdDevs,
-                                 const Vector<3>& localMeasurementStdDevs,
-                                 const Vector<3>& visionMeasurementStdDevs,
-                                 units::second_t nominalDt = 0.02_s);
+  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.
@@ -148,12 +145,12 @@ class DifferentialDrivePoseEstimator {
                         units::meter_t rightDistance);
 
  private:
-  ExtendedKalmanFilter<5, 3, 3> m_observer;
-  KalmanFilterLatencyCompensator<5, 3, 3, ExtendedKalmanFilter<5, 3, 3>>
+  UnscentedKalmanFilter<6, 3, 4> m_observer;
+  KalmanFilterLatencyCompensator<6, 3, 4, UnscentedKalmanFilter<6, 3, 4>>
       m_latencyCompensator;
-  std::function<void(const Vector<3>& u, const Vector<3>& y)> m_visionCorrect;
-
-  Eigen::Matrix<double, 3, 3> m_visionDiscR;
+  std::function<void(const Eigen::Matrix<double, 3, 1>& u,
+                     const Eigen::Matrix<double, 4, 1>& y)>
+      m_visionCorrect;
 
   units::second_t m_nominalDt;
   units::second_t m_prevTime = -1_s;
@@ -161,14 +158,28 @@ class DifferentialDrivePoseEstimator {
   Rotation2d m_gyroOffset;
   Rotation2d m_previousAngle;
 
+  std::array<double, 5> m_stateStdDevs;
+  std::array<double, 3> m_localMeasurementStdDevs;
+
   template <int Dim>
   static std::array<double, Dim> StdDevMatrixToArray(
-      const Vector<Dim>& stdDevs);
-
-  static Vector<5> F(const Vector<5>& x, const Vector<3>& u);
-  static Vector<5> FillStateVector(const Pose2d& pose,
-                                   units::meter_t leftDistance,
-                                   units::meter_t rightDistance);
+      const Eigen::Matrix<double, Dim, 1>& stdDevs);
+
+  static Eigen::Matrix<double, 4, 1> LocalMeasurementModel(
+      const Eigen::Matrix<double, 6, 1>& x,
+      const Eigen::Matrix<double, 3, 1>& u);
+  static Eigen::Matrix<double, 6, 1> F(const Eigen::Matrix<double, 6, 1>& x,
+                                       const Eigen::Matrix<double, 3, 1>& u);
+  static Eigen::Matrix<double, 6, 1> FillStateVector(
+      const Pose2d& pose, units::meter_t leftDistance,
+      units::meter_t rightDistance);
+
+  static std::array<double, 6> MakeQDiagonals(
+      const std::array<double, 5>& stdDevs,
+      const Eigen::Matrix<double, 6, 1>& x);
+  static std::array<double, 4> MakeRDiagonals(
+      const std::array<double, 3>& stdDevs,
+      const Eigen::Matrix<double, 6, 1>& x);
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h b/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h
index 05afe0e0661..2253cc840b1 100644
--- a/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h
+++ b/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h
@@ -21,9 +21,6 @@
 
 namespace frc {
 
-template <int N>
-using Vector = Eigen::Matrix<double, N, 1>;
-
 /**
  * This class wraps an ExtendedKalmanFilter to fuse latency-compensated
  * global measurements(ex. vision) with differential drive encoder measurements.
@@ -40,19 +37,19 @@ using Vector = Eigen::Matrix<double, N, 1>;
  * DifferentialDriveStateEstimator::Update should be called every
  * robot loop (if your robot loops are faster than the default then you should
  * use DifferentialDriveStateEstimator::DifferentialDriveStateEstimator(const
- * LinearSystem<2, 2, 2>&, const Eigen::Matrix<double, 10, 1>&, const Eigen::Matrix<double, 10, 1>&, const
- * Vector<3>&, const Eigen::Matrix<double, 3, 1>&, const DifferentialDriveKinematics&,
- * units::second_t)
- * to change the nominal delta time.)
+ * LinearSystem<2, 2, 2>&, const Eigen::Matrix<double, 10, 1>&, const
+ * Eigen::Matrix<double, 10, 1>&, const Eigen::Matrix<double, 3, 1>&, const
+ * Eigen::Matrix<double, 3, 1>&, const DifferentialDriveKinematics&,
+ * units::second_t) to change the nominal delta time.)
  *
  * DifferentialDriveStateEstimator::ApplyPastGlobalMeasurement can be
  * called as infrequently as you want.
  *
  * Our state-space system is:
  *
- * <strong> x = [[x, y, theta, vel_l, vel_r, dist_l, dist_r, voltError_l,
- * voltError_r, angularVelError]]^T </strong> in the field coordinate system
- * (dist_* are wheel distances.)
+ * <strong> x = [[x, y, std::cos(theta), std::sin(theta), vel_l, vel_r, dist_l,
+ * dist_r, voltError_l, voltError_r, angularVelError]]^T </strong> in the field
+ * coordinate system (dist_* are wheel distances.)
  *
  * <strong> u = [[voltage_l, voltage_r]]^T </strong> This is typically the
  * control input of the last timestep from a LTVDiffDriveController.
@@ -80,13 +77,14 @@ class DifferentialDriveStateEstimator {
    * @param nominalDtSeconds         The time in seconds between each robot
    * loop.
    */
-  DifferentialDriveStateEstimator(const LinearSystem<2, 2, 2>& plant,
-                                  const Eigen::Matrix<double, 10, 1>& initialState,
-                                  const Eigen::Matrix<double, 10, 1>& stateStdDevs,
-                                  const Eigen::Matrix<double, 3, 1>& localMeasurementStdDevs,
-                                  const Eigen::Matrix<double, 3, 1>& globalMeasurementStdDevs,
-                                  const DifferentialDriveKinematics& kinematics,
-                                  units::second_t nominalDt = 0.02_s);
+  DifferentialDriveStateEstimator(
+      const LinearSystem<2, 2, 2>& plant,
+      const std::array<double, 10>& initialState,
+      const std::array<double, 10>& stateStdDevs,
+      const std::array<double, 3>& localMeasurementStdDevs,
+      const std::array<double, 3>& globalMeasurementStdDevs,
+      const DifferentialDriveKinematics& kinematics,
+      units::second_t nominalDt = 0.02_s);
 
   /**
    * Applies a global measurement with a given timestamp.
@@ -108,10 +106,11 @@ class DifferentialDriveStateEstimator {
    *
    * @return The robot state estimate.
    */
-  Vector<10> GetEstimatedState() const;
+  Eigen::Matrix<double, 10, 1> GetEstimatedState() const;
 
   /**
-   * Gets the state of the robot at the current time as estimated by the Extended Kalman Filter.
+   * Gets the state of the robot at the current time as estimated by the
+   * Extended Kalman Filter.
    *
    * @return The robot state estimate.
    */
@@ -131,9 +130,10 @@ class DifferentialDriveStateEstimator {
    * @param controlInput  The control input.
    * @return The robot state estimate.
    */
-  Vector<10> Update(units::radian_t heading, units::meter_t leftPosition,
-                    units::meter_t rightPosition,
-                    const Eigen::Matrix<double, 2, 1>& controlInput);
+  Eigen::Matrix<double, 10, 1> Update(
+      units::radian_t heading, units::meter_t leftPosition,
+      units::meter_t rightPosition,
+      const Eigen::Matrix<double, 2, 1>& controlInput);
 
   /**
    * Updates the the Extended Kalman Filter using wheel encoder information,
@@ -150,11 +150,11 @@ class DifferentialDriveStateEstimator {
    * @param currentTimeSeconds Time at which this method was called, in seconds.
    * @return The robot state estimate.
    */
-  Vector<10> UpdateWithTime(units::radian_t heading,
-                            units::meter_t leftPosition,
-                            units::meter_t rightPosition,
-                            const Eigen::Matrix<double, 2, 1>& controlInput,
-                            units::second_t currentTime);
+  Eigen::Matrix<double, 10, 1> UpdateWithTime(
+      units::radian_t heading, units::meter_t leftPosition,
+      units::meter_t rightPosition,
+      const Eigen::Matrix<double, 2, 1>& controlInput,
+      units::second_t currentTime);
 
   /**
    * Resets any internal state with a given initial state.
@@ -168,13 +168,16 @@ class DifferentialDriveStateEstimator {
    */
   void Reset();
 
-  Vector<10> Dynamics(const Eigen::Matrix<double, 10, 1>& x, const Eigen::Matrix<double, 2, 1>& u);
+  Eigen::Matrix<double, 10, 1> Dynamics(const Eigen::Matrix<double, 10, 1>& x,
+                                        const Eigen::Matrix<double, 2, 1>& u);
 
-  static Vector<3> LocalMeasurementModel(const Eigen::Matrix<double, 10, 1>& x,
-                                         const Eigen::Matrix<double, 2, 1>& u);
+  static Eigen::Matrix<double, 3, 1> LocalMeasurementModel(
+      const Eigen::Matrix<double, 10, 1>& x,
+      const Eigen::Matrix<double, 2, 1>& u);
 
-  static Vector<3> GlobalMeasurementModel(const Eigen::Matrix<double, 10, 1>& x,
-                                          const Eigen::Matrix<double, 2, 1>& u);
+  static Eigen::Matrix<double, 3, 1> GlobalMeasurementModel(
+      const Eigen::Matrix<double, 10, 1>& x,
+      const Eigen::Matrix<double, 2, 1>& u);
 
  private:
   LinearSystem<2, 2, 2> m_plant;
@@ -187,16 +190,18 @@ class DifferentialDriveStateEstimator {
   KalmanFilterLatencyCompensator<10, 2, 3, ExtendedKalmanFilter<10, 2, 3>>
       m_latencyCompensator;
 
-  std::function<void(const Eigen::Matrix<double, 2, 1>& u, const Eigen::Matrix<double, 3, 1>& y)> m_globalCorrect;
+  std::function<void(const Eigen::Matrix<double, 2, 1>& u,
+                     const Eigen::Matrix<double, 3, 1>& y)>
+      m_globalCorrect;
 
   units::second_t m_prevTime = -1_s;
 
-  Vector<3> m_localY;
-  Vector<3> m_globalY;
+  Eigen::Matrix<double, 3, 1> m_localY;
+  Eigen::Matrix<double, 3, 1> m_globalY;
 
   template <int Dim>
   static std::array<double, Dim> StdDevMatrixToArray(
-      const Vector<Dim>& stdDevs);
+      const Eigen::Matrix<double, Dim, 1>& stdDevs);
 
   class State {
    public:
diff --git a/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h b/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
index 1e79a8855ec..3b5adbacf51 100644
--- a/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
+++ b/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
@@ -7,12 +7,14 @@
 
 #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 {
@@ -53,7 +55,8 @@ class KalmanFilterLatencyCompensator {
   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)>
+      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) {
@@ -68,8 +71,9 @@ class KalmanFilterLatencyCompensator {
 
     // 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 = m_pastObserverSnapshots.size() - 1;
+    int high = std::max(maxIdx, 1);
 
     while (low != high) {
       int mid = (low + high) / 2.0;
diff --git a/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
index 130f2ab4cc9..e52f8ceae29 100644
--- a/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
+++ b/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
@@ -12,8 +12,8 @@
 #include <Eigen/Core>
 #include <units/time.h>
 
-#include "frc/estimator/ExtendedKalmanFilter.h"
 #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"
@@ -21,9 +21,6 @@
 
 namespace frc {
 
-template <int N>
-using Vector = Eigen::Matrix<double, N, 1>;
-
 /**
  * This class wraps an ExtendedKalmanFilter to fuse latency-compensated vision
  * measurements with mecanum drive encoder velocity measurements. It will
@@ -70,13 +67,13 @@ class MecanumDrivePoseEstimator {
    * @param nominalDt                The time in seconds between each robot
    *                                 loop.
    */
-  MecanumDrivePoseEstimator(const Rotation2d& gyroAngle,
-                            const Pose2d& initialPose,
-                            MecanumDriveKinematics kinematics,
-                            const Vector<3>& stateStdDevs,
-                            const Vector<1>& localMeasurementStdDevs,
-                            const Vector<3>& visionMeasurementStdDevs,
-                            units::second_t nominalDt = 0.02_s);
+  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.
@@ -147,11 +144,13 @@ class MecanumDrivePoseEstimator {
                         const MecanumDriveWheelSpeeds& wheelSpeeds);
 
  private:
-  ExtendedKalmanFilter<4, 3, 2> m_observer;
+  UnscentedKalmanFilter<4, 3, 2> m_observer;
   MecanumDriveKinematics m_kinematics;
-  KalmanFilterLatencyCompensator<4, 3, 2, ExtendedKalmanFilter<4, 3, 2>>
+  KalmanFilterLatencyCompensator<4, 3, 2, UnscentedKalmanFilter<4, 3, 2>>
       m_latencyCompensator;
-  std::function<void(const Vector<3>& u, const Vector<4>& y)> m_visionCorrect;
+  std::function<void(const Eigen::Matrix<double, 3, 1>& u,
+                     const Eigen::Matrix<double, 4, 1>& y)>
+      m_visionCorrect;
 
   Eigen::Matrix4d m_visionDiscR;
 
@@ -161,17 +160,27 @@ class MecanumDrivePoseEstimator {
   Rotation2d m_gyroOffset;
   Rotation2d m_previousAngle;
 
-  static Vector<4> F(const Vector<4>& x, const Vector<3>& u);
+  static Eigen::Matrix<double, 4, 1> F(const Eigen::Matrix<double, 4, 1>& x,
+                                       const Eigen::Matrix<double, 3, 1>& u);
+
+  std::array<double, 3> m_stateStdDevs;
+  std::array<double, 1> m_localMeasurementStdDevs;
+  std::array<double, 3> m_visionMeasurementStdDevs;
 
-  template <int Dim>
-  static std::array<double, Dim> StdDevMatrixToArray(
-      const Vector<Dim>& vector) {
-    std::array<double, Dim> array;
-    for (size_t i = 0; i < Dim; ++i) {
-      array[i] = vector(i);
-    }
-    return array;
+  static std::array<double, 4> MakeQDiagonals(
+      const std::array<double, 3>& stdDevs,
+      const Eigen::Matrix<double, 4, 1>& x) {
+    // Std dev in [x, y, std::cos(theta), std::sin(theta)] form.
+    return {stdDevs[0], stdDevs[1], stdDevs[2] * x(2), stdDevs[2] * x(3)};
   }
+
+  static std::array<double, 2> MakeRDiagonals(
+      const std::array<double, 1>& stdDevs,
+      const Eigen::Matrix<double, 4, 1>& x);
+
+  static std::array<double, 4> MakeVisionRDiagonals(
+      const std::array<double, 3>& stdDevs,
+      const Eigen::Matrix<double, 4, 1>& y);
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
index 97a110e90b0..99bbd2a89e9 100644
--- a/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
+++ b/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
@@ -15,8 +15,8 @@
 #include <units/time.h>
 
 #include "frc/StateSpaceUtil.h"
-#include "frc/estimator/ExtendedKalmanFilter.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"
@@ -24,11 +24,8 @@
 
 namespace frc {
 
-template <int N>
-using Vector = Eigen::Matrix<double, N, 1>;
-
 /**
- * This class wraps an ExtendedKalmanFilter to fuse latency-compensated vision
+ * This class wraps an UnscentedKalmanFilter 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.
@@ -74,46 +71,49 @@ class SwerveDrivePoseEstimator {
    * @param nominalDt                The time in seconds between each robot
    *                                 loop.
    */
-  SwerveDrivePoseEstimator(const Rotation2d& gyroAngle,
-                           const Pose2d& initialPose,
-                           SwerveDriveKinematics<NumModules>& kinematics,
-                           const Vector<3>& stateStdDevs,
-                           const Vector<1>& localMeasurementStdDevs,
-                           const Vector<3>& visionMeasurementStdDevs,
-                           units::second_t nominalDt = 0.02_s)
-      : m_observer(
+  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_stateStdDevs(stateStdDevs),
+        m_localMeasurementStdDevs(localMeasurementStdDevs),
+        m_visionMeasurementStdDevs(visionMeasurementStdDevs),
+        m_observer(
             &SwerveDrivePoseEstimator::F,
-            [](const Vector<4>& x, const Vector<3>& u) {
+            [](const Eigen::Matrix<double, 4, 1>& x,
+               const Eigen::Matrix<double, 3, 1>& u) {
               return x.block<2, 1>(2, 0);
             },
-            StdDevMatrixToArray<4>(frc::MakeMatrix<4, 1>(
-                stateStdDevs(0), stateStdDevs(1), std::cos(stateStdDevs(2)),
-                std::sin(stateStdDevs(2)))),
-            StdDevMatrixToArray<2>(
-                frc::MakeMatrix<2, 1>(std::cos(localMeasurementStdDevs(0)),
-                                      std::sin(localMeasurementStdDevs(0)))),
+            // X and Y here are zero because we only use the cosine and sin
+            // states of X in the diagonal.
+            MakeQDiagonals(
+                stateStdDevs,
+                frc::MakeMatrix<4, 1>(0.0, 0.0, initialPose.Rotation().Cos(),
+                                      initialPose.Rotation().Sin())),
+            MakeRDiagonals(
+                localMeasurementStdDevs,
+                frc::MakeMatrix<4, 1>(0.0, 0.0, initialPose.Rotation().Cos(),
+                                      initialPose.Rotation().Sin())),
             nominalDt),
         m_kinematics(kinematics),
         m_nominalDt(nominalDt) {
-    // Construct R (covariances) matrix for vision measurements.
-    Eigen::Matrix4d visionContR =
-        frc::MakeCovMatrix<4>(StdDevMatrixToArray<4>(frc::MakeMatrix<4, 1>(
-            visionMeasurementStdDevs(0), visionMeasurementStdDevs(1),
-            std::cos(visionMeasurementStdDevs(2)),
-            std::sin(visionMeasurementStdDevs(2)))));
-
-    // Create and store discrete covariance matrix for vision measurements.
-    m_visionDiscR = frc::DiscretizeR<4>(visionContR, m_nominalDt);
-
     // Create correction mechanism for vision measurements.
-    m_visionCorrect = [&](const Vector<3>& u, const Vector<4>& y) {
+    m_visionCorrect = [&](const Eigen::Matrix<double, 3, 1>& u,
+                          const Eigen::Matrix<double, 4, 1>& y) {
       m_observer.Correct<4>(
-          u, y, [](const Vector<4>& x, const Vector<3>& u) { return x; },
-          m_visionDiscR);
+          u, y,
+          [](const Eigen::Matrix<double, 4, 1>& x,
+             const Eigen::Matrix<double, 3, 1>& u) { return x; },
+          DiscretizeR<4>(MakeCovMatrix<4>(
+                             MakeVisionRDiagonals(visionMeasurementStdDevs, y)),
+                         nominalDt));
     };
 
     // Set initial state.
-    m_observer.SetXhat(PoseTo4dVector(initialPose));
+    ResetPosition(initialPose, gyroAngle);
 
     // Calculate offsets.
     m_gyroOffset = initialPose.Rotation() - gyroAngle;
@@ -141,8 +141,8 @@ class SwerveDrivePoseEstimator {
   }
 
   /**
-   * Gets the pose of the robot at the current time as estimated by the Extended
-   * Kalman Filter.
+   * Gets the pose of the robot at the current time as estimated by the
+   * Unscented Kalman Filter.
    *
    * @return The estimated robot pose in meters.
    */
@@ -152,7 +152,7 @@ class SwerveDrivePoseEstimator {
   }
 
   /**
-   * Add a vision measurement to the Extended Kalman Filter. This will correct
+   * 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
@@ -177,7 +177,7 @@ class SwerveDrivePoseEstimator {
   }
 
   /**
-   * Updates the the Extended Kalman Filter using only wheel encoder
+   * 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.
    *
@@ -193,7 +193,7 @@ class SwerveDrivePoseEstimator {
   }
 
   /**
-   * Updates the the Extended Kalman Filter using only wheel encoder
+   * 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.
    *
@@ -223,27 +223,35 @@ class SwerveDrivePoseEstimator {
                               fieldRelativeSpeeds.Y().template to<double>(),
                               omega.template to<double>());
 
-    auto localY =
-        frc::MakeMatrix<2, 1>(std::cos(angle.Radians().template to<double>()),
-                              std::sin(angle.Radians().template to<double>()));
+    auto localY = frc::MakeMatrix<2, 1>(angle.Cos(), angle.Sin());
     m_previousAngle = angle;
 
     m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
 
-    m_observer.Predict(u, dt);
-    m_observer.Correct(u, localY);
+    m_observer.Predict(u,
+                       frc::MakeCovMatrix<4>(
+                           MakeQDiagonals(m_stateStdDevs, m_observer.Xhat())),
+                       dt);
+    m_observer.Correct<2>(
+        u, localY,
+        [](const Eigen::Matrix<double, 4, 1>& x,
+           const Eigen::Matrix<double, 3, 1>& u) {
+          return x.block<2, 1>(2, 0);
+        },
+        frc::MakeCovMatrix<2>(
+            MakeRDiagonals(m_localMeasurementStdDevs, m_observer.Xhat())));
 
     return GetEstimatedPosition();
   }
 
  private:
-  ExtendedKalmanFilter<4, 3, 2> m_observer;
+  UnscentedKalmanFilter<4, 3, 2> m_observer;
   SwerveDriveKinematics<NumModules>& m_kinematics;
-  KalmanFilterLatencyCompensator<4, 3, 2, ExtendedKalmanFilter<4, 3, 2>>
+  KalmanFilterLatencyCompensator<4, 3, 2, UnscentedKalmanFilter<4, 3, 2>>
       m_latencyCompensator;
-  std::function<void(const Vector<3>& u, const Vector<4>& y)> m_visionCorrect;
-
-  Eigen::Matrix4d m_visionDiscR;
+  std::function<void(const Eigen::Matrix<double, 3, 1>& u,
+                     const Eigen::Matrix<double, 4, 1>& y)>
+      m_visionCorrect;
 
   units::second_t m_nominalDt;
   units::second_t m_prevTime = -1_s;
@@ -251,19 +259,54 @@ class SwerveDrivePoseEstimator {
   Rotation2d m_gyroOffset;
   Rotation2d m_previousAngle;
 
-  static Vector<4> F(const Vector<4>& x, const Vector<3>& u) {
+  /**
+   * Get x-dot given the current state and input. Recall that the state is [x,
+   * y, std::cos(theta), std::sin(theta)]^T In our case, x-dot will be [dx/dt,
+   * dy/dt, d/dt cos(theta), d/dt sin(theta)].
+   *
+   * @param x The current state.
+   * @param u The current input. In our case, u = [vx, vy, d/dt theta]^T
+   */
+  static Eigen::Matrix<double, 4, 1> F(const Eigen::Matrix<double, 4, 1>& x,
+                                       const Eigen::Matrix<double, 3, 1>& u) {
+    // Need to return [dx/dt, dy/dt, d/dt cos(theta), d/dt sin(theta)]
+    // dx/dt and dy/dt are from u.
+    // d/dt cos(theta) = -std::sin(theta) * d/dt(theta) by the chain rule.
     return frc::MakeMatrix<4, 1>(u(0), u(1), -x(3) * u(2), x(2) * u(2));
   }
 
   template <int Dim>
   static std::array<double, Dim> StdDevMatrixToArray(
-      const Vector<Dim>& vector) {
+      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;
   }
+
+  std::array<double, 3> m_stateStdDevs;
+  std::array<double, 1> m_localMeasurementStdDevs;
+  std::array<double, 3> m_visionMeasurementStdDevs;
+
+  static std::array<double, 4> MakeQDiagonals(
+      const std::array<double, 3>& stdDevs,
+      const Eigen::Matrix<double, 4, 1>& x) {
+    // Std dev in [x, y, std::cos(theta), std::sin(theta)] form.
+    return {stdDevs[0], stdDevs[1], stdDevs[2] * x(2), stdDevs[2] * x(3)};
+  }
+
+  static std::array<double, 2> MakeRDiagonals(
+      const std::array<double, 1>& stdDevs,
+      const Eigen::Matrix<double, 4, 1>& x) {
+    return {stdDevs[0] * x(2), stdDevs[0] * x(3)};
+  }
+
+  static std::array<double, 4> MakeVisionRDiagonals(
+      const std::array<double, 3>& stdDevs,
+      const Eigen::Matrix<double, 4, 1>& y) {
+    return {stdDevs[0], stdDevs[1], stdDevs[2] * y(2), stdDevs[2] * y(3)};
+  }
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
index a37eb0f3cf6..5a2af3b6e16 100644
--- a/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
+++ b/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
@@ -5,6 +5,7 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#include <iostream>
 #include <limits>
 #include <random>
 
@@ -22,17 +23,38 @@
 #include "frc2/Timer.h"
 #include "gtest/gtest.h"
 
+TEST(DifferentialDrivePoseEstimatorTest, TestStraightLine) {
+  frc::DifferentialDrivePoseEstimator estimator{frc::Rotation2d(),
+                                                frc::Pose2d(),
+                                                {0.01, 0.01, 0.01, 0.01, 0.01},
+                                                {0.5, 0.5, 0.5},
+                                                {0.1, 0.1, 0.1}};
+
+  for (int i = 0; i < 100; i++) {
+    frc::Pose2d pose = estimator.UpdateWithTime(
+        i * 0.02_s, frc::Rotation2d(0_deg), {1_m / 1_s, 1_m / 1_s},
+        1_m * i * 0.02, 1_m * i * 0.02);
+    // std::cout << pose.Translation().X().to<double>() << ", "
+    //           << pose.Translation().Y().to<double>() << ", "
+    //           << pose.Rotation().Degrees().to<double>() << std::endl;
+  }
+}
+
 TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) {
-  frc::DifferentialDrivePoseEstimator estimator{
-      frc::Rotation2d(), frc::Pose2d(),
-      frc::MakeMatrix<5, 1>(0.01, 0.01, 0.01, 0.01, 0.01),
-      frc::MakeMatrix<3, 1>(0.1, 0.1, 0.1),
-      frc::MakeMatrix<3, 1>(0.1, 0.1, 0.1)};
+  frc::DifferentialDrivePoseEstimator estimator{frc::Rotation2d(),
+                                                frc::Pose2d(),
+                                                {0.01, 0.01, 0.01, 0.01, 0.01},
+                                                {0.5, 0.5, 0.5},
+                                                {0.1, 0.1, 0.1}};
 
   frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{frc::Pose2d(), frc::Pose2d(20_m, 20_m, frc::Rotation2d()),
-                  frc::Pose2d(54_m, 54_m, frc::Rotation2d())},
-      frc::TrajectoryConfig(10_mps, 5.0_mps_sq));
+      std::vector{
+          frc::Pose2d(), frc::Pose2d(20_m, 20_m, frc::Rotation2d()),
+          frc::Pose2d(10_m, 10_m, 180_deg),
+          // frc::Pose2d(30_m, 30_m, 0_deg),
+          // frc::Pose2d(20_m, 20_m, 180_deg)
+      },
+      frc::TrajectoryConfig(2_mps, 3.0_mps_sq));
 
   frc::DifferentialDriveKinematics kinematics{1.0_m};
   frc::DifferentialDriveOdometry odometry{frc::Rotation2d()};
@@ -68,8 +90,8 @@ TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) {
       lastVisionPose =
           groundTruthState.pose +
           frc::Transform2d(
-              frc::Translation2d(distribution(generator) * 0.1 * 1_m,
-                                 distribution(generator) * 0.1 * 1_m),
+              frc::Translation2d(distribution(generator) * 0.5 * 1_m,
+                                 distribution(generator) * 0.5 * 1_m),
               frc::Rotation2d(distribution(generator) * 0.1 * 1_rad));
 
       lastVisionUpdateRealTimestamp = frc2::Timer::GetFPGATimestamp();
@@ -85,6 +107,13 @@ TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) {
             frc::Rotation2d(units::radian_t(distribution(generator) * 0.1)),
         input, leftDistance, rightDistance);
 
+    // std::cout << groundTruthState.pose.Translation().X().to<double>() << ", "
+    //           << groundTruthState.pose.Translation().Y().to<double>() << ", "
+    //           << groundTruthState.pose.Rotation().Degrees().to<double>() << ", "
+    //           << xhat.Translation().X().to<double>() << ", "
+    //           << xhat.Translation().Y().to<double>() << ", "
+    //           << xhat.Rotation().Degrees().to<double>() << ", " << std::endl;
+
     double error = groundTruthState.pose.Translation()
                        .Distance(xhat.Translation())
                        .to<double>();
@@ -103,8 +132,8 @@ TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) {
             << std::endl;
   std::cout << "max error " << maxError << std::endl;
 
-  //  EXPECT_NEAR(0.0, errorSum / (trajectory.TotalTime().to<double>() /
-  //  dt.to<double>()),
-  //            0.2);
-  //  EXPECT_NEAR(0.0, maxError, 0.4);
+  EXPECT_NEAR(
+      0.0, errorSum / (trajectory.TotalTime().to<double>() / dt.to<double>()),
+      0.2);
+  EXPECT_NEAR(0.0, maxError, 0.4);
 }
diff --git a/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp
index a7925c9af9d..5e78c382519 100644
--- a/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp
+++ b/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp
@@ -47,11 +47,10 @@ TEST(DifferentialDriveStateEstimatorTest, TestAccuracy) {
 
   frc::DifferentialDriveStateEstimator estimator{
       plant,
-      Eigen::Matrix<double, 10, 1>::Zero(),
-      frc::MakeMatrix<10, 1>(0.002, 0.002, 0.0001, 1.5, 1.5, 0.5, 0.5, 10.0,
-                             10.0, 2.0),
-      frc::MakeMatrix<3, 1>(0.0001, 0.005, 0.005),
-      frc::MakeMatrix<3, 1>(0.5, 0.5, 0.5),
+      {0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.002, 0.002, 0.0001, 1.5, 1.5, 0.5, 0.5, 10.0, 10.0, 2.0},
+      {0.0001, 0.005, 0.005},
+      {0.5, 0.5, 0.5},
       kinematics,
       dt};
 
@@ -145,10 +144,11 @@ TEST(DifferentialDriveStateEstimatorTest, TestAccuracy) {
     }
     errorSum += error;
 
-    std::cout << groundTruthState.pose.Translation().X().to<double>() 
-      << "," << groundTruthState.pose.Translation().Y().to<double>() 
-      << "," << estimatedTranslation.X().to<double>() << ","
-      << estimatedTranslation.Y().to<double>() << "," << error << "\n";
+    // std::cout << groundTruthState.pose.Translation().X().to<double>() << ","
+    //           << groundTruthState.pose.Translation().Y().to<double>() << ","
+    //           << estimatedTranslation.X().to<double>() << ","
+    //           << estimatedTranslation.Y().to<double>() << "," << error <<
+    //           "\n";
 
     t += dt;
   }
@@ -157,5 +157,8 @@ TEST(DifferentialDriveStateEstimatorTest, TestAccuracy) {
             0.2);
   EXPECT_LT(maxError, 0.4);
 
-  std::cout << "Sum over time: " << errorSum / (trajectory.TotalTime().to<double>() / dt.to<double>()) << " max error " << maxError << "\n"; 
+  std::cout << "Sum over time: "
+            << errorSum /
+                   (trajectory.TotalTime().to<double>() / dt.to<double>())
+            << " max error " << maxError << "\n";
 }
diff --git a/wpilibc/src/test/native/cpp/estimator/KalmanFilterLatencyCompensatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/KalmanFilterLatencyCompensatorTest.cpp
new file mode 100644
index 00000000000..2c961491196
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/estimator/KalmanFilterLatencyCompensatorTest.cpp
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* 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/StateSpaceUtil.h"
+#include "frc/estimator/KalmanFilterLatencyCompensator.h"
+#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "gtest/gtest.h"
+
+TEST(KalmanFilterLatencyCompensatorTest, TestAccuracy) {
+  frc::KalmanFilterLatencyCompensator<1, 1, 1,
+                                      frc::UnscentedKalmanFilter<1, 1, 1>>
+      comp{};
+  auto system = frc::LinearSystemId::IdentifyVelocitySystem(1.0, 1.0);
+  frc::UnscentedKalmanFilter<1, 1, 1> kf{
+      [&](const Eigen::Matrix<double, 1, 1>& x,
+          const Eigen::Matrix<double, 1, 1>& u) {
+        return system.A() * x + system.B() * u;
+      },
+      [&](const Eigen::Matrix<double, 1, 1>& x,
+          const Eigen::Matrix<double, 1, 1>& u) { return x; },
+      {1},
+      {1},
+      1_s};
+  comp.AddObserverState(kf, frc::MakeMatrix<1, 1>(1.0),
+                        frc::MakeMatrix<1, 1>(0.0), 0.0_s);
+  comp.AddObserverState(kf, frc::MakeMatrix<1, 1>(1.0),
+                        frc::MakeMatrix<1, 1>(1.0), 1_s);
+  comp.AddObserverState(kf, frc::MakeMatrix<1, 1>(1.0),
+                        frc::MakeMatrix<1, 1>(3.0), 2_s);
+  comp.ApplyPastMeasurement<1>(
+      &kf, 1_s, frc::MakeMatrix<1, 1>(2.0),
+      [&](const Eigen::Matrix<double, 1, 1>& u,
+          const Eigen::Matrix<double, 1, 1>& y) { kf.Correct(u, y); },
+      1.5_s);
+}
diff --git a/wpilibc/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
index 9ab32f501eb..670175875e0 100644
--- a/wpilibc/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
+++ b/wpilibc/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
@@ -21,12 +21,8 @@ TEST(MecanumDrivePoseEstimatorTest, TestAccuracy) {
       frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
 
   frc::MecanumDrivePoseEstimator estimator{
-      frc::Rotation2d(),
-      frc::Pose2d(),
-      kinematics,
-      frc::MakeMatrix<3, 1>(0.1, 0.1, 0.1),
-      frc::MakeMatrix<1, 1>(0.05),
-      frc::MakeMatrix<3, 1>(0.1, 0.1, 0.1)};
+      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()};
 
diff --git a/wpilibc/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
index 5da90fe84d2..286a645d740 100644
--- a/wpilibc/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
+++ b/wpilibc/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
@@ -21,12 +21,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestAccuracy) {
       frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
 
   frc::SwerveDrivePoseEstimator<4> estimator{
-      frc::Rotation2d(),
-      frc::Pose2d(),
-      kinematics,
-      frc::MakeMatrix<3, 1>(0.1, 0.1, 0.1),
-      frc::MakeMatrix<1, 1>(0.05),
-      frc::MakeMatrix<3, 1>(0.1, 0.1, 0.1)};
+      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()};
 
diff --git a/wpilibj/build.gradle b/wpilibj/build.gradle
index df16022c4f2..1a0a264254d 100644
--- a/wpilibj/build.gradle
+++ b/wpilibj/build.gradle
@@ -75,6 +75,8 @@ dependencies {
     devImplementation project(':cscore')
     devImplementation project(':cameraserver')
     devImplementation sourceSets.main.output
+
+    testImplementation 'org.knowm.xchart:xchart:3.6.5'
 }
 
 apply plugin: 'cpp'
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java
index 4f86857cc4a..29817f724f4 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java
@@ -247,8 +247,7 @@ public Pose2d getEstimatedPosition() {
    */
   public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
     m_latencyCompensator.applyPastGlobalMeasurement(
-            Nat.N4(),
-            m_observer, m_nominalDt,
+        m_observer, m_nominalDt,
             StateSpaceUtil.poseTo4dVector(visionRobotPoseMeters),
             m_visionCorrect,
             timestampSeconds
@@ -315,11 +314,12 @@ public Pose2d updateWithTime(
     m_previousAngle = angle;
 
     var localY = VecBuilder.fill(distanceLeftMeters, distanceRightMeters, angle.getCos(), angle.getSin());
-//     m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
+    var q = StateSpaceUtil.makeCovarianceMatrix(Nat.N6(), makeQDiagonals(m_stateStdDevs, m_observer.getXhat()));
+    var r = StateSpaceUtil.makeCovarianceMatrix(Nat.N4(), makeRDiagonals(m_localMeasurementStdDevs, m_observer.getXhat()));
+    m_latencyCompensator.addObserverState(Nat.N4(), m_observer, u, localY, q, r, this::localMeasurementModel, currentTimeSeconds);
 
-   m_observer.predict(u, StateSpaceUtil.makeCovarianceMatrix(Nat.N6(), makeQDiagonals(m_stateStdDevs, m_observer.getXhat())), dt);
-
-   m_observer.correct(Nat.N4(), u, localY, this::localMeasurementModel, StateSpaceUtil.makeCovarianceMatrix(Nat.N4(), makeRDiagonals(m_localMeasurementStdDevs, m_observer.getXhat())));
+   m_observer.predict(u, q, dt);
+   m_observer.correct(Nat.N4(), u, localY, this::localMeasurementModel, r);
 
     return getEstimatedPosition();
   }
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimator.java
index f292862052f..3e0c85667a1 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimator.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimator.java
@@ -65,15 +65,15 @@
  */
 @SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "PMD.SingularField"})
 public class DifferentialDriveStateEstimator {
-  private final UnscentedKalmanFilter<N12, N2, N4> m_observer;
-  private final KalmanFilterLatencyCompensator<N12, N2, N4> m_latencyCompensator;
+  private UnscentedKalmanFilter<N12, N2, N4> m_observer;
+  private KalmanFilterLatencyCompensator<N12, N2, N4> m_latencyCompensator;
 
-  private final BiConsumer<Matrix<N2, N1>, Matrix<N3, N1>> m_globalCorrect;
+  private BiConsumer<Matrix<N2, N1>, Matrix<N3, N1>> m_globalCorrect;
 
-  private final double m_rb;
-  private final LinearSystem<N2, N2, N2> m_plant;
+  private double m_rb;
+  private LinearSystem<N2, N2, N2> m_plant;
 
-  private final double m_nominalDt; // Seconds
+  private double m_nominalDt; // Seconds
   private double m_prevTimeSeconds = -1.0;
 
 
@@ -96,7 +96,7 @@ public class DifferentialDriveStateEstimator {
    *                                 differential drivetrain's kinematics.
    */
   public DifferentialDriveStateEstimator(LinearSystem<N2, N2, N2> plant,
-                                        Matrix<N10, N1> initialState,
+                                        Matrix<N12, N1> initialState,
                                         Matrix<N10, N1> stateStdDevs,
                                         Matrix<N3, N1> localMeasurementStdDevs,
                                         Matrix<N3, N1> globalMeasurementStdDevs,
@@ -124,7 +124,7 @@ public DifferentialDriveStateEstimator(LinearSystem<N2, N2, N2> plant,
    */
   public DifferentialDriveStateEstimator(
           LinearSystem<N2, N2, N2> plant,
-          Matrix<N10, N1> initialState,
+          Matrix<N12, N1> initialState,
           Matrix<N10, N1> stateStdDevs,
           Matrix<N3, N1> localMeasurementStdDevs,
           Matrix<N3, N1> globalMeasurementStdDevs,
@@ -142,21 +142,21 @@ public DifferentialDriveStateEstimator(
     var globalContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), globalMeasurementStdDevs);
     var globalDiscR = Discretization.discretizeR(globalContR, m_nominalDt);
 
-    m_observer = new UnscentedKalmanFilter<>(
-      Nat.N10(), Nat.N3(),
-      this::getDynamics,
-      this::getLocalMeasurementModel,
-      stateStdDevs,
-      localMeasurementStdDevs,
-      nominalDtSeconds
-   );
+  //   m_observer = new UnscentedKalmanFilter<>(
+  //     Nat.N12(), Nat.N4(),
+  //     this::getDynamics,
+  //     this::getLocalMeasurementModel,
+  //     stateStdDevs,
+  //     localMeasurementStdDevs,
+  //     nominalDtSeconds
+  //  );
 
     // Create correction mechanism for global measurements.
-    m_globalCorrect = (u, y) -> m_observer.correct(
-      Nat.N3(), u, y,
-      this::getGlobalMeasurementModel,
-      globalDiscR
-    );
+    // m_globalCorrect = (u, y) -> m_observer.correct(
+    //   Nat.N3(), u, y,
+    //   this::getGlobalMeasurementModel,
+    //   globalDiscR
+    // );
     m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
 
     reset(initialState);
@@ -226,8 +226,7 @@ public void applyPastGlobalMeasurement(Pose2d robotPoseMeters,
     m_globalY.set(GlobalOutput.kHeading.value, 0, robotPoseMeters.getRotation().getRadians());
 
     m_latencyCompensator.applyPastGlobalMeasurement(
-            Nat.N3(),
-            m_observer, m_nominalDt,
+        m_observer, m_nominalDt,
             m_globalY,
             m_globalCorrect,
             timestampSeconds
@@ -239,7 +238,7 @@ public void applyPastGlobalMeasurement(Pose2d robotPoseMeters,
    *
    * @return The robot state estimate.
    */
-  public Matrix<N10, N1> getEstimatedState() {
+  public Matrix<N12, N1> getEstimatedState() {
     return m_observer.getXhat();
   }
 
@@ -267,7 +266,7 @@ public Pose2d getEstimatedPosition() {
    * @param controlInput   The control input from the last timestep.
    * @return The robot state estimate.
    */
-  public Matrix<N10, N1> update(double headingRadians, double leftPosition,
+  public Matrix<N12, N1> update(double headingRadians, double leftPosition,
                                 double rightPosition,
                                 Matrix<N2, N1> controlInput) {
     return updateWithTime(headingRadians,
@@ -290,7 +289,7 @@ public Matrix<N10, N1> update(double headingRadians, double leftPosition,
    * @return The robot state estimate.
    */
   @SuppressWarnings("VariableDeclarationUsageDistance")
-  public Matrix<N10, N1> updateWithTime(double headingRadians, double leftPosition,
+  public Matrix<N12, N1> updateWithTime(double headingRadians, double leftPosition,
                                         double rightPosition,
                                         Matrix<N2, N1> controlInput,
                                         double currentTimeSeconds) {
@@ -301,10 +300,10 @@ public Matrix<N10, N1> updateWithTime(double headingRadians, double leftPosition
     m_localY.set(LocalOutput.kLeftPosition.value, 0, leftPosition);
     m_localY.set(LocalOutput.kRightPosition.value, 0, rightPosition);
 
-    m_latencyCompensator.addObserverState(m_observer, controlInput, m_localY, currentTimeSeconds);
+    // m_latencyCompensator.addObserverState(m_observer, controlInput, m_localY, currentTimeSeconds);
 
     m_observer.predict(controlInput, dt);
-    m_observer.correct(controlInput, m_localY);
+    // m_observer.correct(controlInput, m_localY);
 
     return getEstimatedState();
   }
@@ -314,7 +313,7 @@ public Matrix<N10, N1> updateWithTime(double headingRadians, double leftPosition
    *
    * @param initialState Initial state for state estimate.
    */
-  public void reset(Matrix<N10, N1> initialState) {
+  public void reset(Matrix<N12, N1> initialState) {
     m_observer.reset();
 
     m_observer.setXhat(initialState);
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilterLatencyCompensator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilterLatencyCompensator.java
index 491c3c56c55..2e8f6d9e682 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilterLatencyCompensator.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilterLatencyCompensator.java
@@ -11,6 +11,7 @@
 import java.util.List;
 import java.util.Map;
 import java.util.function.BiConsumer;
+import java.util.function.BiFunction;
 
 import edu.wpi.first.wpiutil.math.Matrix;
 import edu.wpi.first.wpiutil.math.Nat;
@@ -32,15 +33,19 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
    * @param observer         The observer.
    * @param u                The input at the timestamp.
    * @param localY           The local output at the timestamp
+   * @param q                The process noise covariance matrix at the timestep.
    * @param timestampSeconds The timesnap of the state.
    */
   @SuppressWarnings("ParameterName")
-  public void addObserverState(
+  public <R extends Num> void addObserverState(
+          Nat<R> rows,
           KalmanTypeFilter<S, I, O> observer, Matrix<I, N1> u, Matrix<O, N1> localY,
+          Matrix<S, S> q, Matrix<O, O> r,
+          BiFunction<Matrix<S, N1>, Matrix<I, N1>, Matrix<O, N1>> localMeasurementModel,
           double timestampSeconds
   ) {
     m_pastObserverSnapshots.add(Map.entry(
-            timestampSeconds, new ObserverSnapshot(observer, u, localY)
+            timestampSeconds, new ObserverSnapshot(observer, u, localY, q, r, localMeasurementModel, rows)
     ));
 
     if (m_pastObserverSnapshots.size() > k_maxPastObserverStates) {
@@ -52,7 +57,6 @@ timestampSeconds, new ObserverSnapshot(observer, u, localY)
    * 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.
@@ -61,12 +65,11 @@ timestampSeconds, new ObserverSnapshot(observer, u, localY)
    */
   @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
+      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
@@ -113,15 +116,15 @@ public <R extends Num> void applyPastGlobalMeasurement(
     // 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();
+      ObserverSnapshot 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);
+      observer.predict(snapshot.inputs, snapshot.q, key - lastTimestamp);
+      observer.correct(snapshot.localModelNats, snapshot.inputs, snapshot.localMeasurements, snapshot.localMeasurementModel, snapshot.r);
 
       if (i == indexOfClosestEntry) {
         // Note that the measurement is at a timestep close but probably not exactly equal to the
@@ -132,8 +135,8 @@ public <R extends Num> void applyPastGlobalMeasurement(
       }
       lastTimestamp = key;
 
-      m_pastObserverSnapshots.set(i, Map.entry(key,
-              new ObserverSnapshot(observer, snapshot.inputs, snapshot.localMeasurements)));
+      m_pastObserverSnapshots.set(i, Map.entry(key, new ObserverSnapshot(observer, snapshot.inputs,
+          snapshot.localMeasurements, snapshot.q, snapshot.r, snapshot.localMeasurementModel, snapshot.localModelNats)));
     }
   }
 
@@ -141,18 +144,28 @@ public <R extends Num> void applyPastGlobalMeasurement(
    * This class contains all the information about our observer at a given time.
    */
   @SuppressWarnings("MemberName")
-  public class ObserverSnapshot {
+  public class ObserverSnapshot<R extends Num> {
     public final Matrix<S, N1> xHat;
     public final Matrix<S, S> errorCovariances;
     public final Matrix<I, N1> inputs;
     public final Matrix<O, N1> localMeasurements;
+    public final Matrix<S, S> q;
+    private final Matrix<O, O> r;
+    private final BiFunction<Matrix<S, N1>, Matrix<I, N1>, Matrix<R, N1>> localMeasurementModel;
+    private final Nat<R> localModelNats;
 
     @SuppressWarnings("ParameterName")
     private ObserverSnapshot(
-            KalmanTypeFilter<S, I, O> observer, Matrix<I, N1> u, Matrix<O, N1> localY
+            KalmanTypeFilter<S, I, O> observer, Matrix<I, N1> u, Matrix<O, N1> localY,
+            Matrix<S, S> q, Matrix<O, O> r, BiFunction<Matrix<S, N1>, Matrix<I, N1>, Matrix<R, N1>> localMeasurementModel,
+            Nat<R> rows
     ) {
       this.xHat = observer.getXhat();
       this.errorCovariances = observer.getP();
+      this.q = q;
+      this.r = r;
+      this.localMeasurementModel = localMeasurementModel;
+      this.localModelNats = rows;
 
       inputs = u;
       localMeasurements = localY;
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java
index f8798fa4284..059bf629ef9 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java
@@ -197,8 +197,7 @@ public Pose2d getEstimatedPosition() {
    */
   public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
     m_latencyCompensator.applyPastGlobalMeasurement(
-            Nat.N4(),
-            m_observer, m_nominalDt,
+        m_observer, m_nominalDt,
             StateSpaceUtil.poseTo4dVector(visionRobotPoseMeters),
             m_visionCorrect,
             timestampSeconds
@@ -250,7 +249,8 @@ public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
     m_previousAngle = angle;
 
     var localY = VecBuilder.fill(angle.getCos(), angle.getSin());
-    m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
+    Matrix<N4, N4> q = null; // TODO
+    m_latencyCompensator.addObserverState(null, m_observer, u, localY, q, null, null,  currentTimeSeconds);
     m_observer.predict(u, dt);
     m_observer.correct(u, localY);
 
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java
index 55a77f67b05..f632ef4ef29 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java
@@ -8,6 +8,7 @@
 package edu.wpi.first.wpilibj.estimator;
 
 import java.util.function.BiConsumer;
+import java.util.function.BiFunction;
 
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
@@ -26,7 +27,7 @@
 import edu.wpi.first.wpiutil.math.numbers.N4;
 
 /**
- * This class wraps an {@link UnscentedKalmanFilter ExtendedKalmanFilter} to fuse
+ * This class wraps an {@link UnscentedKalmanFilter UnscentedKalmanFilter} 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}.
@@ -62,6 +63,10 @@ public class SwerveDrivePoseEstimator {
   private Rotation2d m_gyroOffset;
   private Rotation2d m_previousAngle;
 
+  Matrix<N3, N1> m_stateStdDevs;
+  Matrix<N1, N1> m_localMeasurementStdDevs;
+  Matrix<N3, N1> m_visionMeasurementStdDevs;
+
   /**
    * Constructs a SwerveDrivePoseEstimator.
    *
@@ -113,27 +118,24 @@ public SwerveDrivePoseEstimator(
         Nat.N4(), Nat.N2(),
         this::f,
         (x, u) -> x.block(Nat.N2(), Nat.N1(), 2, 0),
-        VecBuilder.fill(stateStdDevs.get(0, 0), stateStdDevs.get(1, 0),
-            Math.cos(stateStdDevs.get(2, 0)), Math.sin(stateStdDevs.get(2, 0))),
-        VecBuilder.fill(Math.cos(localMeasurementStdDevs.get(0, 0)),
-                Math.sin(localMeasurementStdDevs.get(0, 0))),
+        makeQDiagonals(stateStdDevs, VecBuilder.fill(0.0, 0.0,
+            initialPoseMeters.getRotation().getCos(), initialPoseMeters.getRotation().getSin())),
+        makeRDiagonals(localMeasurementStdDevs, VecBuilder.fill(0.0, 0.0,
+            initialPoseMeters.getRotation().getCos(), initialPoseMeters.getRotation().getSin())),
         m_nominalDt
     );
     m_kinematics = kinematics;
     m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+    m_stateStdDevs = stateStdDevs;
+    m_localMeasurementStdDevs = localMeasurementStdDevs;
+    m_visionMeasurementStdDevs = visionMeasurementStdDevs;
 
-    var fullVisionMeasurementStdDev = VecBuilder.fill(
-            visionMeasurementStdDevs.get(0, 0), visionMeasurementStdDevs.get(1, 0),
-            Math.cos(visionMeasurementStdDevs.get(2, 0)),
-            Math.sin(visionMeasurementStdDevs.get(2, 0)));
-
-    var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N4(), fullVisionMeasurementStdDev);
-    var visionDiscR = Discretization.discretizeR(visionContR, m_nominalDt);
-
+    // Create correction mechanism for vision measurements.
     m_visionCorrect = (u, y) -> m_observer.correct(
         Nat.N4(), u, y,
         (x, u_) -> x,
-        visionDiscR
+        Discretization.discretizeR(StateSpaceUtil.makeCovarianceMatrix(Nat.N4(),
+            makeVisionRDiagonals(visionMeasurementStdDevs, y)), nominalDtSeconds)
     );
 
     m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
@@ -141,8 +143,19 @@ public SwerveDrivePoseEstimator(
     m_observer.setXhat(StateSpaceUtil.poseTo4dVector(initialPoseMeters));
   }
 
+  /**
+   * Get x-dot given the current state and input. Recall that the state is [x, y, cos(theta),
+   * sin(theta)]^T.
+   * In our case, x-dot will be [dx/dt, dy/dt, d/dt cos(theta), d/dt sin(theta)].
+   * 
+   * @param x The current state.
+   * @param u The current input. In our case, u = [vx, vy, d/dt theta]^T
+   */
   @SuppressWarnings({"ParameterName", "MethodName"})
   private Matrix<N4, N1> f(Matrix<N4, N1> x, Matrix<N3, N1> u) {
+    // Need to return [dx/dt, dy/dt, d/dt cos(theta), d/dt sin(theta)]
+    // dx/dt and dy/dt are from u.
+    // d/dt cos(theta) = -sin(theta) * d/dt(theta) by the chain rule. 
     return VecBuilder.fill(
             u.get(0, 0), u.get(1, 0), -x.get(3, 0) * u.get(2, 0), x.get(2, 0) * u.get(2, 0)
     );
@@ -166,7 +179,7 @@ public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
   }
 
   /**
-   * Gets the pose of the robot at the current time as estimated by the Extended Kalman Filter.
+   * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
    *
    * @return The estimated robot pose in meters.
    */
@@ -179,7 +192,7 @@ public Pose2d getEstimatedPosition() {
   }
 
   /**
-   * Add a vision measurement to the Extended Kalman Filter. This will correct the
+   * 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
@@ -199,8 +212,7 @@ public Pose2d getEstimatedPosition() {
    */
   public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
     m_latencyCompensator.applyPastGlobalMeasurement(
-            Nat.N4(),
-            m_observer, m_nominalDt,
+        m_observer, m_nominalDt,
             StateSpaceUtil.poseTo4dVector(visionRobotPoseMeters),
             m_visionCorrect,
             timestampSeconds
@@ -208,7 +220,7 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS
   }
 
   /**
-   * Updates the the Extended Kalman Filter using only wheel encoder information.
+   * 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.
    *
@@ -224,7 +236,7 @@ public Pose2d update(
   }
 
   /**
-   * Updates the the Extended Kalman Filter using only wheel encoder information.
+   * 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.
    *
@@ -257,10 +269,32 @@ public Pose2d updateWithTime(
     m_previousAngle = angle;
 
     var localY = VecBuilder.fill(angle.getCos(), angle.getSin());
-    m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
-    m_observer.predict(u, dt);
-    m_observer.correct(u, localY);
-
+    var q = StateSpaceUtil.makeCovarianceMatrix(Nat.N4(),
+        makeQDiagonals(m_stateStdDevs, m_observer.getXhat()));
+    var r = StateSpaceUtil.makeCovarianceMatrix(Nat.N2(),
+        makeRDiagonals(m_localMeasurementStdDevs, m_observer.getXhat()));
+    BiFunction<Matrix<N4, N1>, Matrix<N3, N1>, Matrix<N2, N1>> model =
+        (x, u_) -> x.block(Nat.N2(), Nat.N1(), 2, 0);
+    m_latencyCompensator.addObserverState(null, m_observer, u, localY, q, r, model, currentTimeSeconds);
+    m_observer.predict(u, q, dt);
+    m_observer.correct(Nat.N2(), u, localY, model, r);
     return getEstimatedPosition();
   }
+
+  @SuppressWarnings("ParameterName")
+  private static Matrix<N4, N1> makeQDiagonals(Matrix<N3, N1> stdDevs, Matrix<N4, N1> x) {
+    return VecBuilder.fill(stdDevs.get(0, 0), stdDevs.get(1, 0),
+        stdDevs.get(2, 0) * x.get(2, 0), stdDevs.get(2, 0) * x.get(3, 0));
+  }
+
+  @SuppressWarnings("ParameterName")
+  private static Matrix<N2, N1> makeRDiagonals(Matrix<N1, N1> stdDevs, Matrix<N4, N1> x) {
+    return VecBuilder.fill(stdDevs.get(0, 0) * x.get(2, 0), stdDevs.get(0, 0) * x.get(3, 0));
+  }
+
+  @SuppressWarnings("ParameterName")
+  private static Matrix<N4, N1> makeVisionRDiagonals(Matrix<N3, N1> stdDevs, Matrix<N4, N1> y) {
+    return VecBuilder.fill(stdDevs.get(0, 0), stdDevs.get(1, 0),
+        stdDevs.get(2, 0) * y.get(2, 0), stdDevs.get(2, 0) * y.get(3, 0));
+  }
 }
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java
index 520f34a7fa8..a8b1fbb8e54 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java
@@ -38,12 +38,28 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
 public class DifferentialDrivePoseEstimatorTest {
+
+@Test public void testStraight() {
+  DifferentialDrivePoseEstimator estimator = new DifferentialDrivePoseEstimator(
+      new Rotation2d(), new Pose2d(),
+      VecBuilder.fill(0.01, 0.01, 0.01, 0.01, 0.01),
+      VecBuilder.fill(0.5, 0.5, 0.1),
+      VecBuilder.fill(0.1, 0.1, 0.1));
+
+  for(double i = 0; i < 100; i++) {
+    var pose = estimator.updateWithTime(i * 0.02, new Rotation2d(0), new DifferentialDriveWheelSpeeds(1,
+        1), 0.02 * i * 1, 0.02 * i * 1);
+    System.out.printf("%s, %s, %s%n", pose.getTranslation().getX(), pose.getTranslation().getY(), pose.getRotation().getDegrees());
+  }
+}
+
+
   @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.N5(), Nat.N1()).fill(0.01, 0.01, 0.01, 0.01, 0.01),
             new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.5, 0.5, 0.1),
             new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.1, 0.1, 0.01));
 
@@ -91,7 +107,7 @@ public void testAccuracy() {
 
       if (lastVisionUpdateTime + visionUpdateRate + rand.nextGaussian() * 0.4 < t) {
         if (lastVisionPose != null) {
-          //estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+          estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
         }
         var groundPose = groundtruthState.poseMeters;
         lastVisionPose = new Pose2d(
@@ -136,14 +152,14 @@ public void testAccuracy() {
       t += dt;
     }
 
-//    assertEquals(
-//            0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.03,
-//            "Incorrect mean error"
-//    );
-//    assertEquals(
-//            0.0, maxError, 0.05,
-//            "Incorrect max error"
-//    );
+    assertEquals(
+            0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.4,
+            "Incorrect mean error"
+    );
+    assertEquals(
+            0.0, maxError, 1.0,
+            "Incorrect max error"
+    );
 
     System.out.println("Mean error (meters): " + errorSum / (traj.getTotalTimeSeconds() / dt));
     System.out.println("Max error (meters):  " + maxError);
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimatorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimatorTest.java
index ff7d69535b6..4c126841db6 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimatorTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimatorTest.java
@@ -59,14 +59,14 @@ public void testAccuracy() {
 
     var estimator = new DifferentialDriveStateEstimator(
             plant,
-            MatrixUtils.zeros(Nat.N10()),
+            MatrixUtils.zeros(Nat.N12()),
             new MatBuilder<>(Nat.N10(), Nat.N1()).fill(
                 0.002, 0.002, 0.0001, 1.5, 1.5, 0.5, 0.5, 10.0, 10.0, 2.0),
             new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.0001, 0.005, 0.005),
             new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.5, 0.5, 0.5),
             kinematics);
 
-    var feedforward = new ControlAffinePlantInversionFeedforward<>(Nat.N10(), Nat.N2(),
+    var feedforward = new ControlAffinePlantInversionFeedforward<>(Nat.N12(), Nat.N2(),
         estimator::getDynamics, dt);
 
     var config = new TrajectoryConfig(12 / 2.5, (12 / 0.642) - 17.5);
@@ -124,10 +124,10 @@ public void testAccuracy() {
       );
       var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
 
-      var x = new MatBuilder<>(Nat.N10(), Nat.N1()).fill(
+      var x = new MatBuilder<>(Nat.N12(), Nat.N1()).fill(
               groundTruthState.poseMeters.getTranslation().getX(),
               groundTruthState.poseMeters.getTranslation().getY(),
-              groundTruthState.poseMeters.getRotation().getRadians(),
+              groundTruthState.poseMeters.getRotation().getRadians(), // TODO fix
               wheelSpeeds.leftMetersPerSecond,
               wheelSpeeds.rightMetersPerSecond,
               0.0, 0.0, 0.0, 0.0, 0.0);
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java
index 959f9ce53bf..94d82b07c21 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java
@@ -11,6 +11,14 @@
 import java.util.List;
 import java.util.Random;
 
+import edu.wpi.first.wpilibj.math.StateSpaceUtil;
+import edu.wpi.first.wpilibj.system.RungeKutta;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.wpiutil.math.numbers.N3;
+import edu.wpi.first.wpiutil.math.numbers.N4;
 import org.junit.jupiter.api.Test;
 
 import edu.wpi.first.wpilibj.geometry.Pose2d;
@@ -21,6 +29,9 @@
 import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpiutil.math.VecBuilder;
+import org.knowm.xchart.SwingWrapper;
+import org.knowm.xchart.XYChart;
+import org.knowm.xchart.XYChartBuilder;
 
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
@@ -91,7 +102,7 @@ public void testAccuracy() {
 
       if (lastVisionUpdateTime + visionUpdateRate < t) {
         if (lastVisionPose != null) {
-          estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+//          estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
         }
 
         lastVisionPose = new Pose2d(
@@ -153,16 +164,16 @@ public void testAccuracy() {
       t += dt;
     }
 
-    assertEquals(
-            0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25,
-            "Incorrect mean error"
-    );
-    assertEquals(
-            0.0, maxError, 0.42,
-            "Incorrect max error"
-    );
+//    assertEquals(
+//            0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25,
+//            "Incorrect mean error"
+//    );
+//    assertEquals(
+//            0.0, maxError, 0.42,
+//            "Incorrect max error"
+//    );
 
-    /*
+//    /*
     List<XYChart> charts = new ArrayList<XYChart>();
 
     var chartBuilder = new XYChartBuilder();
@@ -209,6 +220,46 @@ public void testAccuracy() {
       Thread.sleep(1000000000);
     } catch (InterruptedException e) {
     }
-    */
+//    */
   }
+
+    @Test
+    public void testNaive() {
+
+        var traj = TrajectoryGenerator.generateTrajectory(List.of(new Pose2d(), new Pose2d(5, 5, new Rotation2d())), new TrajectoryConfig(2, 2));
+
+        Matrix<N4, N1> x = VecBuilder.fill(0, 0, 1, 0);
+        var ukf = new UnscentedKalmanFilter<>(Nat.N4(), Nat.N2(), this::f, (x_, u) -> x_.block(Nat.N2(), Nat.N1(), 2, 0), VecBuilder.fill(0.1, 0.1, 0.1, 0.1), VecBuilder.fill(0.1, 0.1), 0.020);
+        ukf.setXhat(VecBuilder.fill(0, 0, 1, 0));
+        for (int i = 0; i < traj.getTotalTimeSeconds() / 0.020; i++) {
+            var state = traj.sample(i * 0.020);
+            var u = VecBuilder.fill(state.velocityMetersPerSecond * state.poseMeters.getRotation().getCos(), state.velocityMetersPerSecond * state.poseMeters.getRotation().getSin(), state.curvatureRadPerMeter * state.velocityMetersPerSecond);
+            x = RungeKutta.rungeKutta(this::f, x, u, 0.020);
+            ukf.predict(u, StateSpaceUtil.makeCovarianceMatrix(Nat.N4(), makeQDiagonals(VecBuilder.fill(0.1, 0.1, 0.1), ukf.getXhat())),0.020);
+            ukf.correct(Nat.N2(), u, x.block(Nat.N2(), Nat.N1(), 2, 0), (x_, u_) -> x_.block(Nat.N2(), Nat.N1(), 2, 0), StateSpaceUtil.makeCovarianceMatrix(Nat.N2(), makeRDiagonals(VecBuilder.fill(0.1), ukf.getXhat())));
+
+//            System.out.printf("%s, %s, %s, %s\n", x.get(0,0), x.get(1,0), x.get(2,0), x.get(3,0));
+            System.out.printf("%s, %s, %s, %s\n", ukf.getXhat(0), ukf.getXhat(1), ukf.getXhat(2), ukf.getXhat(3));
+        }
+    }
+
+    // U = vx, vy, omega in global
+    Matrix<N4, N1> f(Matrix<N4, N1> x, Matrix<N3, N1> u) {
+        return VecBuilder.fill(
+            u.get(0, 0), u.get(1, 0), -x.get(3, 0) * u.get(2, 0), x.get(2, 0) * u.get(2, 0)
+        );
+    }
+
+    @SuppressWarnings("ParameterName")
+    private static Matrix<N4, N1> makeQDiagonals(Matrix<N3, N1> stdDevs, Matrix<N4, N1> x) {
+      return VecBuilder.fill(stdDevs.get(0, 0), stdDevs.get(1, 0),
+          stdDevs.get(2, 0) * x.get(2, 0), stdDevs.get(2, 0) * x.get(3, 0));
+//          stdDevs.get(2, 0), stdDevs.get(2, 0));
+    }
+
+    @SuppressWarnings("ParameterName")
+    private static Matrix<N2, N1> makeRDiagonals(Matrix<N1, N1> stdDevs, Matrix<N4, N1> x) {
+      return VecBuilder.fill(stdDevs.get(0, 0) * x.get(2, 0), stdDevs.get(0, 0) * x.get(3, 0));
+//      return VecBuilder.fill(stdDevs.get(0, 0), stdDevs.get(0, 0));
+    }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
index 30984ac3b15..b07905fda54 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
@@ -69,11 +69,21 @@ public static native void discreteAlgebraicRiccatiEquation(
    * Computes the matrix exp.
    *
    * @param src  Array of elements of the matrix to be exponentiated.
-   * @param rows how many rows there are.
+   * @param rows How many rows there are.
    * @param dst  Array where the result will be stored.
    */
   public static native void exp(double[] src, int rows, double[] dst);
 
+  /**
+   * Computes the lower-left triangular matrix decomposition.
+   *
+   * @param src  Array of elements of the matrix to be decomposed.
+   * @param rows How many rows there are.
+   * @param columns How many columns there are.
+   * @param dst  Array where the result will be stored.
+   */
+  public static native void llt(double[] src, int rows, int columns, double[] dst);
+
   /**
    * Returns true if (A, B) is a stabilizable pair.
    *
diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java
index 7474b02f745..298360e8af6 100644
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java
@@ -196,9 +196,20 @@ public void reset() {
    * @param dtSeconds Timestep for prediction.
    */
   @SuppressWarnings("ParameterName")
-  @Override
   public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
-    predict(u, m_f, dtSeconds);
+    predict(u, m_f, m_contQ, dtSeconds);
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u         New control input from controller.
+   * @param dtSeconds Timestep for prediction.
+   */
+  @SuppressWarnings("ParameterName")
+  @Override
+  public void predict(Matrix<Inputs, N1> u, Matrix<States, States> q, double dtSeconds) {
+    predict(u, m_f, q, dtSeconds);
   }
 
   /**
@@ -206,19 +217,21 @@ public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
    *
    * @param u         New control input from controller.
    * @param f         The function used to linearlize the model.
+   * @param q         The process noise covariance matrix
    * @param dtSeconds Timestep for prediction.
    */
   @SuppressWarnings("ParameterName")
   public void predict(
       Matrix<Inputs, N1> u, BiFunction<Matrix<States, N1>,
         Matrix<Inputs, N1>, Matrix<States, N1>> f,
+        Matrix<States, States> q,
       double dtSeconds
   ) {
     // Find continuous A
     final var contA = NumericalJacobian.numericalJacobianX(m_states, m_states, f, m_xHat, u);
 
     // Find discrete A and Q
-    final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
+    final var discPair = Discretization.discretizeAQTaylor(contA, q, dtSeconds);
     final var discA = discPair.getFirst();
     final var discQ = discPair.getSecond();
 
diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java
index aa93b29588b..baf4b3a2213 100644
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java
@@ -8,28 +8,34 @@
 package edu.wpi.first.wpilibj.estimator;
 
 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;
 
+import java.util.function.BiFunction;
+
 @SuppressWarnings({"ParameterName", "InterfaceTypeParameterName"})
-interface KalmanTypeFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
-  Matrix<States, States> getP();
+interface KalmanTypeFilter<S extends Num, I extends Num, O extends Num> {
+  Matrix<S, S> getP();
 
   double getP(int i, int j);
 
-  void setP(Matrix<States, States> newP);
+  void setP(Matrix<S, S> newP);
 
-  Matrix<States, N1> getXhat();
+  Matrix<S, N1> getXhat();
 
   double getXhat(int i);
 
-  void setXhat(Matrix<States, N1> xHat);
+  void setXhat(Matrix<S, N1> xHat);
 
   void setXhat(int i, double value);
 
   void reset();
 
-  void predict(Matrix<Inputs, N1> u, double dtSeconds);
+  void predict(Matrix<I, N1> u, Matrix<S, S> q, double dtSeconds);
+
+  void correct(Matrix<I, N1> u, Matrix<O, N1> y);
 
-  void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y);
+  <R extends Num> void correct(Nat<R> rows, Matrix<I, N1> u, Matrix<R, N1> y, BiFunction<Matrix<S, N1>,
+      Matrix<I, N1>, Matrix<R, N1>> localMeasurementModel, Matrix<R, R> r);
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java
index 56e9288c07f..bc8abdc534c 100644
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java
@@ -90,7 +90,7 @@ public int getNumSigmas() {
     double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum();
 
     var intermediate = P.times(lambda + m_states.getNum());
-    var U = intermediate.lltDecompose(true); // Lower triangular
+    var U = intermediate.llt(); // Lower triangular
 
     // 2 * states + 1 by states
     Matrix<S, ?> sigmas = new Matrix<>(
diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java
index db44da12c6d..dec5d9ebabe 100644
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java
@@ -217,7 +217,6 @@ public void reset() {
    * @param dtSeconds Timestep for prediction.
    */
   @SuppressWarnings({"LocalVariableName", "ParameterName"})
-  @Override
   public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
     predict(u, m_contQ, dtSeconds);
   }
@@ -234,6 +233,7 @@ public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
    * @param dtSeconds Timestep for prediction.
    */
   @SuppressWarnings({"LocalVariableName", "ParameterName"})
+  @Override
   public void predict(Matrix<Inputs, N1> u, Matrix<States, States> q, double dtSeconds) {
     // Discretize Q before projecting mean and covariance forward
     Matrix<States, States> contA =
@@ -264,7 +264,6 @@ public void predict(Matrix<Inputs, N1> u, Matrix<States, States> q, double dtSec
    * @param y Measurement vector.
    */
   @SuppressWarnings("ParameterName")
-  @Override
   public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
     correct(m_outputs, u, y, m_h, m_contR);
   }
@@ -283,6 +282,7 @@ public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
    * @param R Measurement noise covariance matrix.
    */
   @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  @Override
   public <R extends Num> void correct(
         Nat<R> rows, Matrix<Inputs, N1> u,
         Matrix<R, N1> y,
diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java
index 5a0c6920a78..30c9644acf7 100644
--- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java
@@ -355,7 +355,7 @@ public final <C2 extends Num> Matrix<C, C2> solve(Matrix<R, C2> b) {
    * This method only works for square matrices, and will
    * otherwise throw an {@link MatrixDimensionException}.
    *
-   * @return The exponential of A.
+   * @return The exponential of this matrix.
    */
   public final Matrix<R, C> exp() {
     if (this.getNumRows() != this.getNumCols()) {
@@ -368,6 +368,18 @@ public final Matrix<R, C> exp() {
     return toReturn;
   }
 
+  /**
+   * Computes the lower-left triangular matrix decomposition using Eigen's solver.
+   *
+   * @return The decomposition of this matrix.
+   */
+  public final Matrix<R, C> llt() {
+    Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
+    WPIMathJNI.llt(this.m_storage.getDDRM().getData(), this.getNumRows(), this.getNumCols(),
+          toReturn.m_storage.getDDRM().getData());
+    return toReturn;
+  }
+
   /**
    * Returns the determinant of this matrix.
    *
@@ -527,6 +539,15 @@ public Matrix<R, C> lltDecompose(boolean lowerTriangular) {
 
     CholeskyDecomposition_F64<DMatrixRMaj> chol =
             DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
+
+    for(int i = 0; i < temp.numRows(); i++) {
+      for(int j = 0; j < temp.numCols(); j++) {
+        if(temp.get(i, j) < 0) {
+          temp.set(i, j, 0);
+        }
+      }
+    }
+
     if (!chol.decompose(temp.getMatrix())) {
       // check that the input is not all zeros -- if they are, we special case and return all
       // zeros.
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
index 26d57e2ab00..98f4a1320b6 100644
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
@@ -15,6 +15,7 @@
 #include "drake/math/discrete_algebraic_riccati_equation.h"
 #include "edu_wpi_first_math_WPIMathJNI.h"
 #include "unsupported/Eigen/MatrixFunctions"
+#include "Eigen/src/Cholesky/LLT.h"
 
 using namespace wpi::java;
 
@@ -105,6 +106,28 @@ Java_edu_wpi_first_math_WPIMathJNI_exp
   env->SetDoubleArrayRegion(dst, 0, rows * rows, Aexp.data());
 }
 
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    llt
+ * Signature: ([DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_llt
+  (JNIEnv* env, jclass, jdoubleArray src, jint rows, jint columns, jdoubleArray dst)
+{
+  jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr);
+
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      P{arrayBody, rows, columns};
+
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> U =
+      P.llt().matrixL();
+
+  env->ReleaseDoubleArrayElements(src, arrayBody, 0);
+  env->SetDoubleArrayRegion(dst, 0, rows * columns, U.data());
+}
+
 /*
  * Class:     edu_wpi_first_math_WPIMathJNI
  * Method:    isStabilizable
diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
index 1f333eaba59..bbb9d44cd90 100644
--- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h
+++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
@@ -287,6 +287,22 @@ bool IsStabilizable<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
  */
 Eigen::Matrix<double, 3, 1> PoseToVector(const Pose2d& pose);
 
+/**
+ * Converts a vector into a Eigen::Matrix of size (Dim, 1).
+ * 
+ * @param array The array to fill the vector with.
+ * 
+ * @return The vector.
+ */
+template <int Dim>
+Eigen::Matrix<double, Dim, 1> ArrayToVector(const std::array<double, Dim>& array_) {
+  Eigen::Matrix<double, Dim, 1> result;
+  for(size_t i = 0; i < Dim; ++i) {
+    result(i) = array_[i];
+  }
+  return result;
+}
+
 /**
  * Clamps input vector between system's minimum and maximum allowable input.
  *
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
index 8c2c31f0e62..dabdbc0e2f6 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
@@ -111,13 +111,28 @@ class UnscentedKalmanFilter {
     m_sigmasF.setZero();
   }
 
+  /**
+   * Project the model into the future with a new control input u.
+   * 
+   * <p>This is useful for when the process noise covariance matrix is time variant. 
+   * The the Q matrix provided in the constructor is used if it is not provided
+   * (the two-argument version of this function).
+   *
+   * @param u         New control input from controller.
+   * @param q         The continuous process noise covariance matrix.
+   * @param dtSeconds Timestep for prediction.
+   */
+  void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
+    Predict(u, m_contQ, dt);
+  }
+
   /**
    * Project the model into the future with a new control input u.
    *
    * @param u  New control input from controller.
    * @param dt Timestep for prediction.
    */
-  void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
+  void Predict(const Eigen::Matrix<double, Inputs, 1>& u, const Eigen::Matrix<double, States, States> q, units::second_t dt) {
     m_dt = dt;
 
     // Discretize Q before projecting mean and covariance forward
@@ -125,7 +140,7 @@ class UnscentedKalmanFilter {
         NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
     Eigen::Matrix<double, States, States> discA;
     Eigen::Matrix<double, States, States> discQ;
-    DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+    DiscretizeAQTaylor<States>(contA, q, dt, &discA, &discQ);
 
     Eigen::Matrix<double, States, 2 * States + 1> sigmas =
         m_pts.SigmaPoints(m_xHat, m_P);