diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index a4d2ca6..d3ed728 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -46,6 +46,7 @@
     deps = [
         ":drivetrain_config",
         "//aos/containers:priority_queue",
+        "//aos/util:math",
         "//frc971/control_loops:c2d",
         "//frc971/control_loops:runge_kutta",
         "//third_party/eigen",
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 82f409c..119386a 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -4,6 +4,7 @@
 #include <chrono>
 
 #include "aos/containers/priority_queue.h"
+#include "aos/util/math.h"
 #include "frc971/control_loops/c2d.h"
 #include "frc971/control_loops/runge_kutta.h"
 #include "Eigen/Dense"
@@ -48,11 +49,8 @@
     kLeftVelocity = 4,
     kRightEncoder = 5,
     kRightVelocity = 6,
-    kLeftVoltageError = 7,
-    kRightVoltageError = 8 ,
-    kAngularError = 9,
   };
-  static constexpr int kNStates = 10;
+  static constexpr int kNStates = 7;
   static constexpr int kNInputs = 2;
   // Number of previous samples to save.
   static constexpr int kSaveSamples = 50;
@@ -70,19 +68,11 @@
   // variable-size measurement updates.
   typedef Eigen::Matrix<Scalar, kNOutputs, 1> Output;
   typedef Eigen::Matrix<Scalar, kNStates, kNStates> StateSquare;
-  // State is [x_position, y_position, theta, Kalman States], where
-  // Kalman States are the states from the standard drivetrain Kalman Filter,
-  // which is: [left encoder, left ground vel, right encoder, right ground vel,
-  // left voltage error, right voltage error, angular_error], where:
-  // left/right encoder should correspond directly to encoder readings
-  // left/right velocities are the velocity of the left/right sides over the
+  // State is [x_position, y_position, theta, left encoder, left ground vel,
+  // right encoder, right ground vel]. left/right encoder should correspond
+  // directly to encoder readings left/right velocities are the velocity of the
+  // left/right sides over the
   //   ground (i.e., corrected for angular_error).
-  // voltage errors are the difference between commanded and effective voltage,
-  //   used to estimate consistent modelling errors (e.g., friction).
-  // angular error is the difference between the angular velocity as estimated
-  //   by the encoders vs. estimated by the gyro, such as might be caused by
-  //   wheels on one side of the drivetrain being too small or one side's
-  //   wheels slipping more than the other.
   typedef Eigen::Matrix<Scalar, kNStates, 1> State;
 
   // Constructs a HybridEkf for a particular drivetrain.
@@ -406,9 +396,7 @@
 
   // Encoder derivatives
   A_continuous_(kLeftEncoder, kLeftVelocity) = 1.0;
-  A_continuous_(kLeftEncoder, kAngularError) = 1.0;
   A_continuous_(kRightEncoder, kRightVelocity) = 1.0;
-  A_continuous_(kRightEncoder, kAngularError) = -1.0;
 
   // Pull velocity derivatives from velocity matrices.
   // Note that this looks really awkward (doesn't use
@@ -425,21 +413,22 @@
   B_continuous_.setZero();
   B_continuous_.row(kLeftVelocity) = vel_coefs.B_continuous.row(0);
   B_continuous_.row(kRightVelocity) = vel_coefs.B_continuous.row(1);
-  A_continuous_.template block<kNStates, kNInputs>(0, 7) = B_continuous_;
 
   Q_continuous_.setZero();
   // TODO(james): Improve estimates of process noise--e.g., X/Y noise can
   // probably be reduced when we are stopped because you rarely jump randomly.
   // Or maybe it's more appropriate to scale wheelspeed noise with wheelspeed,
   // since the wheels aren't likely to slip much stopped.
-  Q_continuous_(kX, kX) = 0.005;
-  Q_continuous_(kY, kY) = 0.005;
-  Q_continuous_(kTheta, kTheta) = 0.001;
-  Q_continuous_.template block<7, 7>(3, 3) =
-      dt_config_.make_kf_drivetrain_loop().observer().coefficients().Q;
+  Q_continuous_(kX, kX) = 0.01;
+  Q_continuous_(kY, kY) = 0.01;
+  Q_continuous_(kTheta, kTheta) = 0.0002;
+  Q_continuous_(kLeftEncoder, kLeftEncoder) = ::std::pow(0.03, 2.0);
+  Q_continuous_(kRightEncoder, kRightEncoder) = ::std::pow(0.03, 2.0);
+  Q_continuous_(kLeftVelocity, kLeftVelocity) = ::std::pow(0.1, 2.0);
+  Q_continuous_(kRightVelocity, kRightVelocity) = ::std::pow(0.1, 2.0);
 
   P_.setZero();
-  P_.diagonal() << 0.1, 0.1, 0.01, 0.02, 0.01, 0.02, 0.01, 1, 1, 0.03;
+  P_.diagonal() << 0.1, 0.1, 0.01, 0.02, 0.01, 0.02, 0.01;
 
   H_encoders_and_gyro_.setZero();
   // Encoders are stored directly in the state matrix, so are a minor
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index 27119b1..1702ec4 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -52,22 +52,16 @@
     EXPECT_EQ(Xdot_ekf(StateIdx::kX, 0), ctheta * (left_vel + right_vel) / 2.0);
     EXPECT_EQ(Xdot_ekf(StateIdx::kY, 0), stheta * (left_vel + right_vel) / 2.0);
     EXPECT_EQ(Xdot_ekf(StateIdx::kTheta, 0), (right_vel - left_vel) / diameter);
-    EXPECT_EQ(Xdot_ekf(StateIdx::kLeftEncoder, 0),
-              left_vel + X(StateIdx::kAngularError, 0));
-    EXPECT_EQ(Xdot_ekf(StateIdx::kRightEncoder, 0),
-              right_vel - X(StateIdx::kAngularError, 0));
+    EXPECT_EQ(Xdot_ekf(StateIdx::kLeftEncoder, 0), left_vel);
+    EXPECT_EQ(Xdot_ekf(StateIdx::kRightEncoder, 0), right_vel);
 
     Eigen::Matrix<double, 2, 1> vel_x(X(StateIdx::kLeftVelocity, 0),
                                       X(StateIdx::kRightVelocity, 0));
     Eigen::Matrix<double, 2, 1> expected_vel_X =
         velocity_plant_coefs_.A_continuous * vel_x +
-        velocity_plant_coefs_.B_continuous *
-            (U + X.middleRows<2>(StateIdx::kLeftVoltageError));
+        velocity_plant_coefs_.B_continuous * U;
     EXPECT_EQ(Xdot_ekf(StateIdx::kLeftVelocity, 0), expected_vel_X(0, 0));
     EXPECT_EQ(Xdot_ekf(StateIdx::kRightVelocity, 0), expected_vel_X(1, 0));
-
-    // Dynamics don't expect error terms to change:
-    EXPECT_EQ(0.0, Xdot_ekf.bottomRows<3>().squaredNorm());
   }
   State DiffEq(const State &X, const Input &U) {
     return ekf_.DiffEq(X, U);
@@ -93,18 +87,14 @@
   CheckDiffEq(State::Zero(), Input::Zero());
   CheckDiffEq(State::Zero(), {-5.0, 5.0});
   CheckDiffEq(State::Zero(), {12.0, -3.0});
-  CheckDiffEq((State() << 100.0, 200.0, M_PI, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
-               0.3).finished(),
+  CheckDiffEq((State() << 100.0, 200.0, M_PI, 1.234, 0.5, 1.2, 0.6).finished(),
               {5.0, 6.0});
-  CheckDiffEq((State() << 100.0, 200.0, 2.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
-               0.3).finished(),
+  CheckDiffEq((State() << 100.0, 200.0, 2.0, 1.234, 0.5, 1.2, 0.6).finished(),
               {5.0, 6.0});
-  CheckDiffEq((State() << 100.0, 200.0, -2.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
-               0.3).finished(),
+  CheckDiffEq((State() << 100.0, 200.0, -2.0, 1.234, 0.5, 1.2, 0.6).finished(),
               {5.0, 6.0});
   // And check that a theta outisde of [-M_PI, M_PI] works.
-  CheckDiffEq((State() << 100.0, 200.0, 200.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
-               0.3).finished(),
+  CheckDiffEq((State() << 100.0, 200.0, 200.0, 1.234, 0.5, 1.2, 0.6).finished(),
               {5.0, 6.0});
 }
 
@@ -112,7 +102,7 @@
 // with zero change in time, the state should approach the estimation.
 TEST_F(HybridEkfTest, ZeroTimeCorrect) {
   HybridEkf<>::Output Z(0.5, 0.5, 1);
-  Eigen::Matrix<double, 3, 10> H;
+  Eigen::Matrix<double, 3, 7> H;
   H.setIdentity();
   auto h = [H](const State &X, const Input &) { return H * X; };
   auto dhdx = [H](const State &) { return H; };
@@ -140,7 +130,7 @@
   HybridEkf<>::Output Z(0, 0, 0);
   // Use true_X to track what we think the true robot state is.
   State true_X = ekf_.X_hat();
-  Eigen::Matrix<double, 3, 10> H;
+  Eigen::Matrix<double, 3, 7> H;
   H.setZero();
   auto h = [H](const State &X, const Input &) { return H * X; };
   auto dhdx = [H](const State &) { return H; };
@@ -171,9 +161,6 @@
   EXPECT_NEAR(ekf_.X_hat(StateIdx::kLeftVelocity) * 0.8,
               ekf_.X_hat(StateIdx::kRightVelocity),
               ekf_.X_hat(StateIdx::kLeftVelocity) * 0.1);
-  EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kLeftVoltageError));
-  EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kRightVoltageError));
-  EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kAngularError));
   const double ending_p_norm = ekf_.P().norm();
   // Due to lack of corrections, noise should've increased.
   EXPECT_GT(ending_p_norm, starting_p_norm * 1.10);
@@ -193,7 +180,7 @@
 TEST_P(HybridEkfOldCorrectionsTest, CreateOldCorrection) {
   HybridEkf<>::Output Z;
   Z.setZero();
-  Eigen::Matrix<double, 3, 10> H;
+  Eigen::Matrix<double, 3, 7> H;
   H.setZero();
   auto h_zero = [H](const State &X, const Input &) { return H * X; };
   auto dhdx_zero = [H](const State &) { return H; };
@@ -231,7 +218,7 @@
   expected_X_hat(0, 0) = Z(0, 0);
   expected_X_hat(1, 0) = Z(1, 0) + modeled_X_hat(0, 0);
   expected_X_hat(2, 0) = Z(2, 0);
-  EXPECT_LT((expected_X_hat.topRows<7>() - ekf_.X_hat().topRows<7>()).norm(),
+  EXPECT_LT((expected_X_hat - ekf_.X_hat()).norm(),
            1e-3)
       << "X_hat: " << ekf_.X_hat() << " expected " << expected_X_hat;
   // The covariance after the predictions but before the corrections should
@@ -249,7 +236,7 @@
 TEST_F(HybridEkfTest, DiscardTooOldCorrection) {
   HybridEkf<>::Output Z;
   Z.setZero();
-  Eigen::Matrix<double, 3, 10> H;
+  Eigen::Matrix<double, 3, 7> H;
   H.setZero();
   auto h_zero = [H](const State &X, const Input &) { return H * X; };
   auto dhdx_zero = [H](const State &) { return H; };
@@ -304,11 +291,11 @@
 }
 
 // Tests that encoder updates cause everything to converge properly in the
-// presence of voltage error.
+// presence of an initial velocity error.
 TEST_F(HybridEkfTest, PerfectEncoderUpdatesWithVoltageError) {
   State true_X = ekf_.X_hat();
-  true_X(StateIdx::kLeftVoltageError, 0) = 2.0;
-  true_X(StateIdx::kRightVoltageError, 0) = 2.0;
+  true_X(StateIdx::kLeftVelocity, 0) = 0.2;
+  true_X(StateIdx::kRightVelocity, 0) = 0.2;
   Input U(5.0, 5.0);
   for (int ii = 0; ii < 1000; ++ii) {
     true_X = Update(true_X, U);
@@ -328,11 +315,11 @@
 
 // Tests encoder/gyro updates when we have some errors in our estimate.
 TEST_F(HybridEkfTest, PerfectEncoderUpdateConverges) {
-  // In order to simulate modelling errors, we add an angular_error and start
-  // the encoder values slightly off.
+  // In order to simulate modelling errors, we start the encoder values slightly
+  // off.
   State true_X = ekf_.X_hat();
-  true_X(StateIdx::kAngularError, 0) = 1.0;
   true_X(StateIdx::kLeftEncoder, 0) += 2.0;
+  true_X(StateIdx::kLeftVelocity, 0) = 0.1;
   true_X(StateIdx::kRightEncoder, 0) -= 2.0;
   // After enough time, everything should converge to near-perfect (if there
   // were any errors in the original absolute state (x/y/theta) state, then we
@@ -350,7 +337,7 @@
                                    dt_config_.robot_radius / 2.0,
                                U, t0_ + (ii + 1) * dt_config_.dt);
   }
-  EXPECT_NEAR((true_X - ekf_.X_hat()).norm(), 0.0, 1e-5)
+  EXPECT_NEAR((true_X - ekf_.X_hat()).norm(), 0.0, 1e-4)
       << "Expected non-x/y estimates to converge to correct. "
          "Estimated X_hat:\n"
       << ekf_.X_hat() << "\ntrue X:\n"
@@ -359,11 +346,11 @@
 
 // Tests encoder/gyro updates in a realistic-ish scenario with noise:
 TEST_F(HybridEkfTest, RealisticEncoderUpdateConverges) {
-  // In order to simulate modelling errors, we add an angular_error and start
-  // the encoder values slightly off.
+  // In order to simulate modelling errors, we start the encoder values slightly
+  // off.
   State true_X = ekf_.X_hat();
-  true_X(StateIdx::kAngularError, 0) = 1.0;
   true_X(StateIdx::kLeftEncoder, 0) += 2.0;
+  true_X(StateIdx::kLeftVelocity, 0) = 0.1;
   true_X(StateIdx::kRightEncoder, 0) -= 2.0;
   Input U(10.0, 5.0);
   for (int ii = 0; ii < 100; ++ii) {
@@ -377,7 +364,7 @@
         U, t0_ + (ii + 1) * dt_config_.dt);
   }
   EXPECT_NEAR(
-      (true_X.bottomRows<9>() - ekf_.X_hat().bottomRows<9>()).squaredNorm(),
+      (true_X.bottomRows<6>() - ekf_.X_hat().bottomRows<6>()).squaredNorm(),
       0.0, 2e-3)
       << "Expected non-x/y estimates to converge to correct. "
          "Estimated X_hat:\n" << ekf_.X_hat() << "\ntrue X:\n" << true_X;
@@ -411,7 +398,7 @@
   // Check that we die when only one of h and dhdx are provided:
   EXPECT_DEATH(ekf_.Correct({1, 2, 3}, &U, {}, {},
                             [](const State &) {
-                              return Eigen::Matrix<double, 3, 10>::Zero();
+                              return Eigen::Matrix<double, 3, 7>::Zero();
                             },
                             {}, t0_ + ::std::chrono::seconds(1)),
                "make_h");
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index af07089..f0d81cb 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -95,9 +95,12 @@
   }
 
   void ResetPosition(double x, double y, double theta) override {
+    const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
+    const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
     ekf_.ResetInitialState(
         ::aos::monotonic_clock::now(),
-        (Ekf::State() << x, y, theta, 0, 0, 0, 0, 0, 0, 0).finished(),
+        (Ekf::State() << x, y, theta, left_encoder, 0, right_encoder, 0)
+            .finished(),
         ekf_.P());
   };
 
@@ -110,12 +113,8 @@
   double right_velocity() const override {
     return ekf_.X_hat(StateIdx::kRightVelocity);
   }
-  double left_voltage_error() const override {
-    return ekf_.X_hat(StateIdx::kLeftVoltageError);
-  }
-  double right_voltage_error() const override {
-    return ekf_.X_hat(StateIdx::kRightVoltageError);
-  }
+  double left_voltage_error() const override { return 0.0; }
+  double right_voltage_error() const override { return 0.0; }
 
   TrivialTargetSelector *target_selector() override {
     return &target_selector_;