diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 2c94473..35aebc7 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -152,6 +152,7 @@
     hdrs = ["hybrid_ekf.h"],
     deps = [
         ":drivetrain_config",
+        "//aos:math",
         "//aos/containers:priority_queue",
         "//aos/util:math",
         "//frc971/control_loops:c2d",
@@ -184,6 +185,7 @@
     hdrs = ["localizer.h"],
     deps = [
         ":drivetrain_config",
+        ":drivetrain_status_fbs",
         ":hybrid_ekf",
         "//frc971/control_loops:pose",
     ],
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 7dfd0e6..a19f57b 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -243,7 +243,9 @@
     }
     localizer_->Update({last_last_left_voltage_, last_last_right_voltage_},
                        monotonic_now, position->left_encoder(),
-                       position->right_encoder(), last_gyro_rate_, last_accel_);
+                       position->right_encoder(),
+                       down_estimator_.avg_recent_yaw_rates(),
+                       down_estimator_.avg_recent_accel());
   }
 
   dt_openloop_.SetPosition(position, left_gear_, right_gear_);
@@ -321,6 +323,9 @@
     const flatbuffers::Offset<DownEstimatorState> down_estimator_state_offset =
         down_estimator_.PopulateStatus(status->fbb(), monotonic_now);
 
+    const flatbuffers::Offset<LocalizerState> localizer_offset =
+        localizer_->PopulateStatus(status->fbb());
+
     const flatbuffers::Offset<ImuZeroerState> zeroer_offset =
         imu_zeroer_.PopulateStatus(status->fbb());
 
@@ -363,6 +368,7 @@
     builder.add_line_follow_logging(line_follow_logging_offset);
     builder.add_trajectory_logging(trajectory_logging_offset);
     builder.add_down_estimator(down_estimator_state_offset);
+    builder.add_localizer(localizer_offset);
     builder.add_zeroing(zeroer_offset);
     status->Send(builder.Finish());
   }
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 1de5d40..3c206b3 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -82,9 +82,9 @@
   void VerifyNearGoal() {
     drivetrain_goal_fetcher_.Fetch();
     EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal(),
-                drivetrain_plant_.GetLeftPosition(), 1e-3);
+                drivetrain_plant_.GetLeftPosition(), 1e-2);
     EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal(),
-                drivetrain_plant_.GetRightPosition(), 1e-3);
+                drivetrain_plant_.GetRightPosition(), 1e-2);
   }
 
   void VerifyNearPosition(double x, double y) {
@@ -100,8 +100,8 @@
     const double expected_y =
         CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
     const ::Eigen::Vector2d actual = drivetrain_plant_.GetPosition();
-    EXPECT_NEAR(actual(0), expected_x, 3e-2);
-    EXPECT_NEAR(actual(1), expected_y, 3e-2);
+    EXPECT_NEAR(actual(0), expected_x, 4e-2);
+    EXPECT_NEAR(actual(1), expected_y, 4e-2);
   }
 
   void WaitForTrajectoryPlan() {
@@ -131,7 +131,7 @@
         drivetrain_status_fetcher_->theta();
     EXPECT_LT(
         std::abs(aos::math::DiffAngle(down_estimator_yaw, localizer_yaw)),
-        1e-5);
+        1e-2);
     const double true_yaw = (drivetrain_plant_.GetRightPosition() -
                              drivetrain_plant_.GetLeftPosition()) /
                             (dt_config_.robot_radius * 2.0);
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 3f3a7f7..fa723e0 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -4,6 +4,7 @@
 #include <chrono>
 
 #include "Eigen/Dense"
+#include "aos/commonmath.h"
 #include "aos/containers/priority_queue.h"
 #include "aos/util/math.h"
 #include "frc971/control_loops/c2d.h"
@@ -37,24 +38,104 @@
 // measurement updates with an encoder/gyro as well as a more generic
 // update function that can be used for arbitrary nonlinear updates (presumably
 // a camera update).
+//
+// Discussion of the model:
+// In the current model, we try to rely primarily on IMU measurements for
+// estimating robot state--we also need additional information (some combination
+// of output voltages, encoders, and camera data) to help eliminate the biases
+// that can accumulate due to integration of IMU data.
+// We use IMU measurements as inputs rather than measurement outputs because
+// that seemed to be easier to implement. I tried initially running with
+// the IMU as a measurement, but it seemed to blow up the complexity of the
+// model.
+//
+// On each prediction update, we take in inputs of the left/right voltages and
+// the current measured longitudinal/lateral accelerations. In the current
+// setup, the accelerometer readings will be used for estimating how the
+// evolution of the longitudinal/lateral velocities. The voltages (and voltage
+// errors) will solely be used for estimating the current rotational velocity of
+// the robot (I do this because currently I suspect that the accelerometer is a
+// much better indicator of current robot state than the voltages). We also
+// deliberately decay all of the velocity estimates towards zero to help address
+// potential accelerometer biases. We use two separate decay models:
+// -The longitudinal velocity is modelled as decaying at a constant rate (see
+//  the documentation on the VelocityAccel() method)--this needs a more
+//  complex model because the robot will, under normal circumstances, be
+//  travelling at non-zero velocities.
+// -The lateral velocity is modelled as exponentially decaying towards zero.
+//  This is simpler to model and should be reasonably valid, since we will
+//  not *normally* be travelling sideways consistently (this assumption may
+//  need to be revisited).
+// -The "longitudinal velocity offset" (described below) also uses an
+//  exponential decay, albeit with a different time constant. A future
+//  improvement may remove the decay modelling on the longitudinal velocity
+//  itself and instead use that decay model on the longitudinal velocity offset.
+//  This would place a bit more trust in the encoder measurements but also
+//  more correctly model situations where the robot is legitimately moving at
+//  a certain velocity.
+//
+// For modelling how the drivetrain encoders evolve, and to help prevent the
+// aforementioned decay functions from affecting legitimate high-velocity
+// maneuvers too much, we have a "longitudinal velocity offset" term. This term
+// models the difference between the actual longitudinal velocity of the robot
+// (estimated by the average of the left/right velocities) and the velocity
+// experienced by the wheels (which can be observed from the encoders more
+// directly). Because we model this velocity offset as decaying towards zero,
+// what this will do is allow the encoders to be a constant velocity off from
+// the accelerometer updates for short periods of time but then gradually
+// pull the "actual" longitudinal velocity offset towards that of the encoders,
+// helping to reduce constant biases.
 template <typename Scalar = double>
 class HybridEkf {
  public:
   // An enum specifying what each index in the state vector is for.
   enum StateIdx {
+    // Current X/Y position, in meters, of the robot.
     kX = 0,
     kY = 1,
+    // Current heading of the robot.
     kTheta = 2,
+    // Current estimated encoder reading of the left wheels, in meters.
+    // Rezeroed once on startup.
     kLeftEncoder = 3,
+    // Current estimated actual velocity of the left side of the robot, in m/s.
     kLeftVelocity = 4,
+    // Same variables, for the right side of the robot.
     kRightEncoder = 5,
     kRightVelocity = 6,
+    // Estimated offset to input voltage. Used as a generic error term, Volts.
     kLeftVoltageError = 7,
     kRightVoltageError = 8,
+    // These error terms are used to estimate the difference between the actual
+    // movement of the drivetrain and that implied by the wheel odometry.
+    // Angular error effectively estimates a constant angular rate offset of the
+    // encoders relative to the actual rotation of the robot.
+    // Semi-arbitrary units (we don't bother accounting for robot radius in
+    // this).
     kAngularError = 9,
+    // Estimate of slip between the drivetrain wheels and the actual
+    // forwards/backwards velocity of the robot, in m/s.
+    // I.e., (left velocity + right velocity) / 2.0 = (left wheel velocity +
+    //        right wheel velocity) / 2.0 + longitudinal velocity offset
+    kLongitudinalVelocityOffset = 10,
+    // Current estimate of the lateral velocity of the robot, in m/s.
+    // Positive implies the robot is moving to its left.
+    kLateralVelocity = 11,
   };
-  static constexpr int kNStates = 10;
-  static constexpr int kNInputs = 2;
+  static constexpr int kNStates = 12;
+  enum InputIdx {
+    // Left/right drivetrain voltages.
+    kLeftVoltage = 0,
+    kRightVoltage = 1,
+    // Current accelerometer readings, in m/s/s, along the longitudinal and
+    // lateral axes of the robot. Should be projected onto the X/Y plane, to
+    // compensate for tilt of the robot before being passed to this filter. The
+    // HybridEkf has no knowledge of the current pitch/roll of the robot, and so
+    // can't do anything to compensate for it.
+    kLongitudinalAccel = 2,
+    kLateralAccel = 3,
+  };
+  static constexpr int kNInputs = 4;
   // Number of previous samples to save.
   static constexpr int kSaveSamples = 50;
   // Assume that all correction steps will have kNOutputs
@@ -63,6 +144,10 @@
   // figuring out how to deal with storing variable size
   // observation matrices, though.
   static constexpr int kNOutputs = 3;
+  // Time constant to use for estimating how the longitudinal/lateral velocity
+  // offsets decay, in seconds.
+  static constexpr double kVelocityOffsetTimeConstant = 10.0;
+  static constexpr double kLateralVelocityTimeConstant = 1.0;
   // Inputs are [left_volts, right_volts]
   typedef Eigen::Matrix<Scalar, kNInputs, 1> Input;
   // Outputs are either:
@@ -71,19 +156,7 @@
   // 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
-  //   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.
+  // State contains the states defined by the StateIdx enum. See comments there.
   typedef Eigen::Matrix<Scalar, kNStates, 1> State;
 
   // Constructs a HybridEkf for a particular drivetrain.
@@ -136,28 +209,39 @@
   // matrix for linear cases?
   void Correct(
       const Output &z, const Input *U,
-      ::std::function<
+      std::function<
           void(const State &, const StateSquare &,
-               ::std::function<Output(const State &, const Input &)> *,
-               ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
-                   const State &)> *)>
-          make_h,
-      ::std::function<Output(const State &, const Input &)> h,
-      ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
-          dhdx,
-      const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
+               std::function<Output(const State &, const Input &)> *,
+               std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
+                   const State &)> *)> make_h,
+      std::function<Output(const State &, const Input &)> h,
+      std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
+          dhdx, const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
       aos::monotonic_clock::time_point t);
 
   // A utility function for specifically updating with encoder and gyro
   // measurements.
   void UpdateEncodersAndGyro(const Scalar left_encoder,
                              const Scalar right_encoder, const Scalar gyro_rate,
-                             const Input &U,
-                             ::aos::monotonic_clock::time_point t) {
+                             const Eigen::Matrix<Scalar, 2, 1> &voltage,
+                             const Eigen::Matrix<Scalar, 3, 1> &accel,
+                             aos::monotonic_clock::time_point t) {
+    Input U;
+    U.template block<2, 1>(0, 0) = voltage;
+    U.template block<2, 1>(kLongitudinalAccel, 0) =
+        accel.template block<2, 1>(0, 0);
+    RawUpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, t);
+  }
+  // Version of UpdateEncodersAndGyro that takes a input matrix rather than
+  // taking in a voltage/acceleration separately.
+  void RawUpdateEncodersAndGyro(const Scalar left_encoder,
+                                const Scalar right_encoder,
+                                const Scalar gyro_rate, const Input &U,
+                                aos::monotonic_clock::time_point t) {
     // Because the check below for have_zeroed_encoders_ will add an
     // Observation, do a check here to ensure that initialization has been
     // performed and so there is at least one observation.
-    AOS_CHECK(!observations_.empty());
+    CHECK(!observations_.empty());
     if (!have_zeroed_encoders_) {
       // This logic handles ensuring that on the first encoder reading, we
       // update the internal state for the encoders to match the reading.
@@ -165,35 +249,49 @@
       // wpilib_interface, then we can get some obnoxious initial corrections
       // that mess up the localization.
       State newstate = X_hat_;
-      newstate(kLeftEncoder, 0) = left_encoder;
-      newstate(kRightEncoder, 0) = right_encoder;
-      newstate(kLeftVoltageError, 0) = 0.0;
-      newstate(kRightVoltageError, 0) = 0.0;
-      newstate(kAngularError, 0) = 0.0;
+      newstate(kLeftEncoder) = left_encoder;
+      newstate(kRightEncoder) = right_encoder;
+      newstate(kLeftVoltageError) = 0.0;
+      newstate(kRightVoltageError) = 0.0;
+      newstate(kAngularError) = 0.0;
+      newstate(kLongitudinalVelocityOffset) = 0.0;
+      newstate(kLateralVelocity) = 0.0;
       ResetInitialState(t, newstate, P_);
       // We need to set have_zeroed_encoders_ after ResetInitialPosition because
       // the reset clears have_zeroed_encoders_...
       have_zeroed_encoders_ = true;
     }
+
     Output z(left_encoder, right_encoder, gyro_rate);
+
     Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
     R.setZero();
     R.diagonal() << encoder_noise_, encoder_noise_, gyro_noise_;
-    Correct(z, &U, {},
-            [this](const State &X, const Input &) {
-              return H_encoders_and_gyro_ * X;
-            },
-            [this](const State &) { return H_encoders_and_gyro_; }, R, t);
+    Correct(
+        z, &U, {},
+        [this](const State &X, const Input &) {
+          return H_encoders_and_gyro_ * X;
+        },
+        [this](const State &) { return H_encoders_and_gyro_; }, R, t);
   }
 
   // Sundry accessor:
   State X_hat() const { return X_hat_; }
-  Scalar X_hat(long i) const { return X_hat_(i, 0); }
+  Scalar X_hat(long i) const { return X_hat_(i); }
   StateSquare P() const { return P_; }
-  ::aos::monotonic_clock::time_point latest_t() const {
+  aos::monotonic_clock::time_point latest_t() const {
     return observations_.top().t;
   }
 
+  static Scalar CalcLongitudinalVelocity(const State &X) {
+    return (X(kLeftVelocity) + X(kRightVelocity)) / 2.0;
+  }
+
+  Scalar CalcYawRate(const State &X) const {
+    return (X(kRightVelocity) - X(kLeftVelocity)) / 2.0 /
+           dt_config_.robot_radius;
+  }
+
  private:
   struct Observation {
     // Time when the observation was taken.
@@ -213,16 +311,14 @@
     // estimate. This is used by the camera to make it so that we only have to
     // match targets once.
     // Only called if h and dhdx are empty.
-    ::std::function<void(
-        const State &, const StateSquare &,
-        ::std::function<Output(const State &, const Input &)> *,
-        ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
-            const State &)> *)>
-        make_h;
+    std::function<void(const State &, const StateSquare &,
+                       std::function<Output(const State &, const Input &)> *,
+                       std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
+                           const State &)> *)> make_h;
     // A function to calculate the expected output at a given state/input.
     // TODO(james): For encoders/gyro, it is linear and the function call may
     // be expensive. Potential source of optimization.
-    ::std::function<Output(const State &, const Input &)> h;
+    std::function<Output(const State &, const Input &)> h;
     // The Jacobian of h with respect to x.
     // We assume that U has no impact on the Jacobian.
     // TODO(james): Currently, none of the users of this actually make use of
@@ -230,7 +326,7 @@
     // recalculate it to be strictly correct, but I was both too lazy to do
     // so and it seemed unnecessary). This is a potential source for future
     // optimizations if function calls are being expensive.
-    ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
+    std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
         dhdx;
     // The measurement noise matrix.
     Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
@@ -244,33 +340,116 @@
 
   void InitializeMatrices();
 
-  StateSquare AForState(const State &X) const {
+  // These constants and functions define how the longitudinal velocity
+  // (the average of the left and right velocities) decays. We model it as
+  // decaying at a constant rate, except very near zero where the decay rate is
+  // exponential (this is more numerically stable than just using a constant
+  // rate the whole time). We use this model rather than a simpler exponential
+  // decay because an exponential decay will result in the robot's velocity
+  // estimate consistently being far too low when at high velocities, and since
+  // the acceleromater-based estimate of the velocity will only drift at a
+  // relatively slow rate and doesn't get worse at higher velocities, we can
+  // safely decay pretty slowly.
+  static constexpr double kMaxVelocityAccel = 0.005;
+  static constexpr double kMaxVelocityGain = 1.0;
+  static Scalar VelocityAccel(Scalar velocity) {
+    return -std::clamp(kMaxVelocityGain * velocity, -kMaxVelocityAccel,
+                       kMaxVelocityAccel);
+  }
+
+  static Scalar VelocityAccelDiff(Scalar velocity) {
+    return (std::abs(kMaxVelocityGain * velocity) > kMaxVelocityAccel)
+               ? 0.0
+               : -kMaxVelocityGain;
+  }
+
+  // Returns the "A" matrix for a given state. See DiffEq for discussion of
+  // ignore_accel.
+  StateSquare AForState(const State &X, bool ignore_accel = false) const {
+    // Calculate the A matrix for a given state. Note that A = partial Xdot /
+    // partial X. This is distinct from saying that Xdot = A * X. This is
+    // particularly relevant for the (kX, kTheta) members which otherwise seem
+    // odd.
     StateSquare A_continuous = A_continuous_;
-    const Scalar theta = X(kTheta, 0);
-    const Scalar linear_vel =
-        (X(kLeftVelocity, 0) + X(kRightVelocity, 0)) / 2.0;
-    const Scalar stheta = ::std::sin(theta);
-    const Scalar ctheta = ::std::cos(theta);
+    const Scalar theta = X(kTheta);
+    const Scalar stheta = std::sin(theta);
+    const Scalar ctheta = std::cos(theta);
+    const Scalar lng_vel = CalcLongitudinalVelocity(X);
+    const Scalar lat_vel = X(kLateralVelocity);
+    const Scalar diameter = 2.0 * dt_config_.robot_radius;
+    const Scalar yaw_rate = CalcYawRate(X);
     // X and Y derivatives
-    A_continuous(kX, kTheta) = -stheta * linear_vel;
+    A_continuous(kX, kTheta) =
+        -stheta * lng_vel - ctheta * lat_vel;
     A_continuous(kX, kLeftVelocity) = ctheta / 2.0;
     A_continuous(kX, kRightVelocity) = ctheta / 2.0;
-    A_continuous(kY, kTheta) = ctheta * linear_vel;
+    A_continuous(kX, kLateralVelocity) = -stheta;
+    A_continuous(kY, kTheta) = ctheta * lng_vel - stheta * lat_vel;
     A_continuous(kY, kLeftVelocity) = stheta / 2.0;
     A_continuous(kY, kRightVelocity) = stheta / 2.0;
+    A_continuous(kY, kLateralVelocity) = ctheta;
+
+    if (!ignore_accel) {
+      const Eigen::Matrix<Scalar, 1, kNStates> lng_vel_row =
+          (A_continuous.row(kLeftVelocity) + A_continuous.row(kRightVelocity)) /
+          2.0;
+      A_continuous.row(kLeftVelocity) -= lng_vel_row;
+      A_continuous.row(kRightVelocity) -= lng_vel_row;
+      // Terms to account for centripetal accelerations.
+      // lateral centripetal accel = -yaw_rate * lng_vel
+      A_continuous(kLateralVelocity, kLeftVelocity) +=
+          X(kLeftVelocity) / diameter;
+      A_continuous(kLateralVelocity, kRightVelocity) +=
+          -X(kRightVelocity) / diameter;
+      A_continuous(kRightVelocity, kLateralVelocity) += yaw_rate;
+      A_continuous(kLeftVelocity, kLateralVelocity) += yaw_rate;
+      const Scalar dlng_accel_dwheel_vel = X(kLateralVelocity) / diameter;
+      A_continuous(kRightVelocity, kRightVelocity) += dlng_accel_dwheel_vel;
+      A_continuous(kLeftVelocity, kRightVelocity) += dlng_accel_dwheel_vel;
+      A_continuous(kRightVelocity, kLeftVelocity) += -dlng_accel_dwheel_vel;
+      A_continuous(kLeftVelocity, kLeftVelocity) += -dlng_accel_dwheel_vel;
+
+      A_continuous(kRightVelocity, kRightVelocity) +=
+          VelocityAccelDiff(lng_vel) / 2.0;
+      A_continuous(kRightVelocity, kLeftVelocity) +=
+          VelocityAccelDiff(lng_vel) / 2.0;
+      A_continuous(kLeftVelocity, kRightVelocity) +=
+          VelocityAccelDiff(lng_vel) / 2.0;
+      A_continuous(kLeftVelocity, kLeftVelocity) +=
+          VelocityAccelDiff(lng_vel) / 2.0;
+    }
     return A_continuous;
   }
 
-  State DiffEq(const State &X, const Input &U) const {
+  // Returns dX / dt given X and U. If ignore_accel is set, then we ignore the
+  // accelerometer-based components of U (this is solely used in testing).
+  State DiffEq(const State &X, const Input &U,
+               bool ignore_accel = false) const {
     State Xdot = A_continuous_ * X + B_continuous_ * U;
     // And then we need to add on the terms for the x/y change:
-    const Scalar theta = X(kTheta, 0);
-    const Scalar linear_vel =
-        (X(kLeftVelocity, 0) + X(kRightVelocity, 0)) / 2.0;
-    const Scalar stheta = ::std::sin(theta);
-    const Scalar ctheta = ::std::cos(theta);
-    Xdot(kX, 0) = ctheta * linear_vel;
-    Xdot(kY, 0) = stheta * linear_vel;
+    const Scalar theta = X(kTheta);
+    const Scalar lng_vel = CalcLongitudinalVelocity(X);
+    const Scalar lat_vel = X(kLateralVelocity);
+    const Scalar stheta = std::sin(theta);
+    const Scalar ctheta = std::cos(theta);
+    Xdot(kX) = ctheta * lng_vel - stheta * lat_vel;
+    Xdot(kY) = stheta * lng_vel + ctheta * lat_vel;
+
+    const Scalar yaw_rate = CalcYawRate(X);
+    const Scalar expected_lat_accel = lng_vel * yaw_rate;
+    const Scalar expected_lng_accel =
+        CalcLongitudinalVelocity(Xdot) - yaw_rate * lat_vel;
+    const Scalar lng_accel_offset =
+        U(kLongitudinalAccel) - expected_lng_accel;
+    constexpr double kAccelWeight = 1.0;
+    if (!ignore_accel) {
+      Xdot(kLeftVelocity) += kAccelWeight * lng_accel_offset;
+      Xdot(kRightVelocity) += kAccelWeight * lng_accel_offset;
+      Xdot(kLateralVelocity) += U(kLateralAccel) - expected_lat_accel;
+
+      Xdot(kRightVelocity) += VelocityAccel(lng_vel);
+      Xdot(kLeftVelocity) += VelocityAccel(lng_vel);
+    }
     return Xdot;
   }
 
@@ -283,7 +462,7 @@
 
     *state = RungeKuttaU(
         [this](const State &X, const Input &U) { return DiffEq(X, U); }, *state,
-        U, ::aos::time::DurationInSeconds(dt));
+        U, aos::time::DurationInSeconds(dt));
 
     StateSquare Ptemp = A_d * *P * A_d.transpose() + Q_d;
     *P = Ptemp;
@@ -310,7 +489,7 @@
       PredictImpl(obs->U, dt, state, P);
     }
     if (!(obs->h && obs->dhdx)) {
-      AOS_CHECK(obs->make_h);
+      CHECK(obs->make_h);
       obs->make_h(*state, *P, &obs->h, &obs->dhdx);
     }
     CorrectImpl(obs->R, obs->z, obs->h(*state, obs->U), obs->dhdx(*state),
@@ -330,45 +509,42 @@
 
   bool have_zeroed_encoders_ = false;
 
-  aos::PriorityQueue<Observation, kSaveSamples, ::std::less<Observation>>
+  aos::PriorityQueue<Observation, kSaveSamples, std::less<Observation>>
       observations_;
 
+
   friend class testing::HybridEkfTest;
-  friend class ::y2019::control_loops::testing::ParameterizedLocalizerTest;
+  friend class y2019::control_loops::testing::ParameterizedLocalizerTest;
 };  // class HybridEkf
 
 template <typename Scalar>
 void HybridEkf<Scalar>::Correct(
     const Output &z, const Input *U,
-    ::std::function<
-        void(const State &, const StateSquare &,
-             ::std::function<Output(const State &, const Input &)> *,
-             ::std::function<
-                 Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)> *)>
-        make_h,
-    ::std::function<Output(const State &, const Input &)> h,
-    ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
-        dhdx,
-    const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
+    std::function<void(const State &, const StateSquare &,
+                       std::function<Output(const State &, const Input &)> *,
+                       std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
+                           const State &)> *)> make_h,
+    std::function<Output(const State &, const Input &)> h,
+    std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
+        dhdx, const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
     aos::monotonic_clock::time_point t) {
-  AOS_CHECK(!observations_.empty());
+  CHECK(!observations_.empty());
   if (!observations_.full() && t < observations_.begin()->t) {
-    AOS_LOG(ERROR,
-            "Dropped an observation that was received before we "
-            "initialized.\n");
+    LOG(ERROR) << "Dropped an observation that was received before we "
+                  "initialized.\n";
     return;
   }
   auto cur_it =
       observations_.PushFromBottom({t, t, State::Zero(), StateSquare::Zero(),
                                     Input::Zero(), z, make_h, h, dhdx, R});
   if (cur_it == observations_.end()) {
-    AOS_LOG(
-        DEBUG,
-        "Camera dropped off of end with time of %fs; earliest observation in "
-        "queue has time of %fs.\n",
-        ::aos::time::DurationInSeconds(t.time_since_epoch()),
-        ::aos::time::DurationInSeconds(
-            observations_.begin()->t.time_since_epoch()));
+    VLOG(1) << "Camera dropped off of end with time of "
+            << aos::time::DurationInSeconds(t.time_since_epoch())
+            << "s; earliest observation in "
+               "queue has time of "
+            << aos::time::DurationInSeconds(
+                   observations_.begin()->t.time_since_epoch())
+            << "s.\n";
     return;
   }
 
@@ -396,7 +572,7 @@
     --prev_it;
     cur_it->prev_t = prev_it->t;
     // TODO(james): Figure out a saner way of handling this.
-    AOS_CHECK(U != nullptr);
+    CHECK(U != nullptr);
     cur_it->U = *U;
   } else {
     cur_it->X_hat = next_it->X_hat;
@@ -412,7 +588,7 @@
     // We use X_hat_ and P_ to store the intermediate states, and then
     // once we reach the end they will all be up-to-date.
     ProcessObservation(&*cur_it, cur_it->t - cur_it->prev_t, &X_hat_, &P_);
-    AOS_CHECK(X_hat_.allFinite());
+    CHECK(X_hat_.allFinite());
     if (next_it != observations_.end()) {
       next_it->X_hat = X_hat_;
       next_it->P = P_;
@@ -435,8 +611,10 @@
   // Encoder derivatives
   A_continuous_(kLeftEncoder, kLeftVelocity) = 1.0;
   A_continuous_(kLeftEncoder, kAngularError) = 1.0;
+  A_continuous_(kLeftEncoder, kLongitudinalVelocityOffset) = -1.0;
   A_continuous_(kRightEncoder, kRightVelocity) = 1.0;
   A_continuous_(kRightEncoder, kAngularError) = -1.0;
+  A_continuous_(kRightEncoder, kLongitudinalVelocityOffset) = -1.0;
 
   // Pull velocity derivatives from velocity matrices.
   // Note that this looks really awkward (doesn't use
@@ -449,11 +627,21 @@
   A_continuous_(kRightVelocity, kLeftVelocity) = vel_coefs.A_continuous(1, 0);
   A_continuous_(kRightVelocity, kRightVelocity) = vel_coefs.A_continuous(1, 1);
 
-  // Provide for voltage error terms:
+  A_continuous_(kLongitudinalVelocityOffset, kLongitudinalVelocityOffset) =
+      -1.0 / kVelocityOffsetTimeConstant;
+  A_continuous_(kLateralVelocity, kLateralVelocity) =
+      -1.0 / kLateralVelocityTimeConstant;
+
+  // We currently ignore all voltage-related modelling terms.
+  // TODO(james): Decide what to do about these terms. They don't really matter
+  // too much when we have accelerometer readings available.
   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_;
+  B_continuous_.template block<1, 2>(kLeftVelocity, kLeftVoltage) =
+      vel_coefs.B_continuous.row(0);
+  B_continuous_.template block<1, 2>(kRightVelocity, kLeftVoltage) =
+      vel_coefs.B_continuous.row(1);
+  A_continuous_.template block<kNStates, 2>(0, kLeftVoltageError) =
+      B_continuous_.template block<kNStates, 2>(0, kLeftVoltage);
 
   Q_continuous_.setZero();
   // TODO(james): Improve estimates of process noise--e.g., X/Y noise can
@@ -463,16 +651,22 @@
   Q_continuous_(kX, kX) = 0.002;
   Q_continuous_(kY, kY) = 0.002;
   Q_continuous_(kTheta, kTheta) = 0.0001;
-  Q_continuous_(kLeftEncoder, kLeftEncoder) = ::std::pow(0.15, 2.0);
-  Q_continuous_(kRightEncoder, kRightEncoder) = ::std::pow(0.15, 2.0);
-  Q_continuous_(kLeftVelocity, kLeftVelocity) = ::std::pow(0.5, 2.0);
-  Q_continuous_(kRightVelocity, kRightVelocity) = ::std::pow(0.5, 2.0);
-  Q_continuous_(kLeftVoltageError, kLeftVoltageError) = ::std::pow(10.0, 2.0);
-  Q_continuous_(kRightVoltageError, kRightVoltageError) = ::std::pow(10.0, 2.0);
-  Q_continuous_(kAngularError, kAngularError) = ::std::pow(2.0, 2.0);
+  Q_continuous_(kLeftEncoder, kLeftEncoder) = std::pow(0.15, 2.0);
+  Q_continuous_(kRightEncoder, kRightEncoder) = std::pow(0.15, 2.0);
+  Q_continuous_(kLeftVelocity, kLeftVelocity) = std::pow(0.5, 2.0);
+  Q_continuous_(kRightVelocity, kRightVelocity) = std::pow(0.5, 2.0);
+  Q_continuous_(kLeftVoltageError, kLeftVoltageError) = std::pow(10.0, 2.0);
+  Q_continuous_(kRightVoltageError, kRightVoltageError) = std::pow(10.0, 2.0);
+  Q_continuous_(kAngularError, kAngularError) = std::pow(2.0, 2.0);
+  // This noise value largely governs whether we will trust the encoders or
+  // accelerometer more for estimating the robot position.
+  Q_continuous_(kLongitudinalVelocityOffset, kLongitudinalVelocityOffset) =
+      std::pow(1.1, 2.0);
+  Q_continuous_(kLateralVelocity, kLateralVelocity) = std::pow(0.1, 2.0);
 
   P_.setZero();
-  P_.diagonal() << 0.01, 0.01, 0.01, 0.02, 0.01, 0.02, 0.01, 1, 1, 0.03;
+  P_.diagonal() << 0.01, 0.01, 0.01, 0.02, 0.01, 0.02, 0.01, 1, 1, 0.03, 0.01,
+      0.01;
 
   H_encoders_and_gyro_.setZero();
   // Encoders are stored directly in the state matrix, so are a minor
@@ -483,12 +677,8 @@
   H_encoders_and_gyro_(2, kLeftVelocity) = -1.0 / diameter;
   H_encoders_and_gyro_(2, kRightVelocity) = 1.0 / diameter;
 
-  const Eigen::Matrix<Scalar, 4, 4> R_kf_drivetrain =
-      dt_config_.make_kf_drivetrain_loop().observer().coefficients().R;
-  // TODO(james): The multipliers here are hand-waving things that I put in when
-  // tuning things. I haven't yet tried messing with these values again.
-  encoder_noise_ = 0.5 * R_kf_drivetrain(0, 0);
-  gyro_noise_ = 0.1 * R_kf_drivetrain(2, 2);
+  encoder_noise_ = 5e-9;
+  gyro_noise_ = 1e-13;
 
   X_hat_.setZero();
   P_.setZero();
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index fa6acc5..bdfdd4e 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -14,11 +14,13 @@
 namespace testing {
 
 typedef HybridEkf<>::StateIdx StateIdx;
+typedef HybridEkf<>::InputIdx InputIdx;
+typedef HybridEkf<>::State State;
+typedef HybridEkf<>::StateSquare StateSquare;
+typedef HybridEkf<>::Input Input;
 
 class HybridEkfTest : public ::testing::Test {
  public:
-  typedef HybridEkf<>::State State;
-  typedef HybridEkf<>::Input Input;
   ::aos::testing::TestSharedMemory shm_;
   HybridEkfTest()
       : dt_config_(GetTestDrivetrainConfig()),
@@ -27,47 +29,84 @@
         velocity_plant_coefs_(dt_config_.make_hybrid_drivetrain_velocity_loop()
                                   .plant()
                                   .coefficients()) {
-    ekf_.ResetInitialState(t0_, State::Zero(),
-                           HybridEkf<>::StateSquare::Identity());
+    ekf_.ResetInitialState(t0_, State::Zero(), StateSquare::Identity());
   }
 
  protected:
-  const State Update(const State &X, const Input &U) {
+  const State Update(const State &X, const Input &U, bool ignore_accel) {
     return RungeKuttaU(
-        ::std::bind(&HybridEkfTest::DiffEq, this, ::std::placeholders::_1,
-                    ::std::placeholders::_2),
-        X, U, ::aos::time::DurationInSeconds(dt_config_.dt));
+        std::bind(&HybridEkfTest::DiffEq, this, std::placeholders::_1,
+                  std::placeholders::_2, ignore_accel),
+        X, U, aos::time::DurationInSeconds(dt_config_.dt));
   }
-  void CheckDiffEq(const State &X, const Input &U) {
+  void CheckDiffEq(const State &X, const Input &U, bool ignore_accel) {
     // Re-implement dynamics as a sanity check:
     const double diameter = 2.0 * dt_config_.robot_radius;
-    const double theta = X(StateIdx::kTheta, 0);
-    const double stheta = ::std::sin(theta);
-    const double ctheta = ::std::cos(theta);
-    const double left_vel = X(StateIdx::kLeftVelocity, 0);
-    const double right_vel = X(StateIdx::kRightVelocity, 0);
-    const State Xdot_ekf = DiffEq(X, U);
-    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));
+    const double theta = X(StateIdx::kTheta);
+    const double stheta = std::sin(theta);
+    const double ctheta = std::cos(theta);
+    const double left_vel = X(StateIdx::kLeftVelocity);
+    const double right_vel = X(StateIdx::kRightVelocity);
+    const double lng_vel = (left_vel + right_vel) / 2.0;
+    const double yaw_rate =
+        (right_vel - left_vel) / 2.0 / dt_config_.robot_radius;
+    const double lat_vel = X(StateIdx::kLateralVelocity);
+    const State Xdot_ekf = DiffEq(X, U, ignore_accel);
+    EXPECT_EQ(Xdot_ekf(StateIdx::kX), ctheta * lng_vel - stheta * lat_vel);
+    EXPECT_EQ(Xdot_ekf(StateIdx::kY), stheta * lng_vel + ctheta * lat_vel);
+    EXPECT_EQ(Xdot_ekf(StateIdx::kTheta), (right_vel - left_vel) / diameter);
+    EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kLeftEncoder),
+                    left_vel + X(StateIdx::kAngularError) -
+                        X(StateIdx::kLongitudinalVelocityOffset));
+    EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kRightEncoder),
+                    right_vel - X(StateIdx::kAngularError) -
+                        X(StateIdx::kLongitudinalVelocityOffset));
 
-    Eigen::Matrix<double, 2, 1> vel_x(X(StateIdx::kLeftVelocity, 0),
-                                      X(StateIdx::kRightVelocity, 0));
-    Eigen::Matrix<double, 2, 1> expected_vel_X =
+    const Eigen::Matrix<double, 2, 1> vel_x(X(StateIdx::kLeftVelocity),
+                                            X(StateIdx::kRightVelocity));
+    // Don't expect any contribution from the voltage terms, since we currently
+    // disable them.
+    const Eigen::Matrix<double, 2, 1> expected_accel =
         velocity_plant_coefs_.A_continuous * vel_x +
         velocity_plant_coefs_.B_continuous *
-            (U + X.middleRows<2>(StateIdx::kLeftVoltageError));
-    EXPECT_EQ(Xdot_ekf(StateIdx::kLeftVelocity, 0), expected_vel_X(0, 0));
-    EXPECT_EQ(Xdot_ekf(StateIdx::kRightVelocity, 0), expected_vel_X(1, 0));
+            (U.topRows(2) + X.block<2, 1>(StateIdx::kLeftVoltageError, 0));
+    const double nominal_lng_accel =
+        (expected_accel(0) + expected_accel(1)) / 2.0;
+    if (ignore_accel) {
+      EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kLeftVelocity), expected_accel(0));
+      EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kRightVelocity), expected_accel(1));
+      EXPECT_EQ(Xdot_ekf(StateIdx::kLateralVelocity), 0.0);
+      EXPECT_EQ(
+          Xdot_ekf(StateIdx::kLongitudinalVelocityOffset), 0.0);
+    } else {
+      const double lng_accel = U(InputIdx::kLongitudinalAccel) +
+                               lat_vel * yaw_rate + ekf_.VelocityAccel(lng_vel);
+      EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kLeftVelocity),
+                      lng_accel + expected_accel(0) - nominal_lng_accel);
+      EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kRightVelocity),
+                      lng_accel + expected_accel(1) - nominal_lng_accel);
+
+      EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kLongitudinalVelocityOffset),
+                      -X(StateIdx::kLongitudinalVelocityOffset) /
+                          HybridEkf<>::kVelocityOffsetTimeConstant);
+      const double centripetal_accel = lng_vel * yaw_rate;
+      const double lat_accel = U(InputIdx::kLateralAccel) - centripetal_accel;
+      EXPECT_EQ(Xdot_ekf(StateIdx::kLateralVelocity),
+                lat_accel - X(StateIdx::kLateralVelocity) /
+                                HybridEkf<>::kLateralVelocityTimeConstant);
+    }
 
     // Dynamics don't expect error terms to change:
-    EXPECT_EQ(0.0, Xdot_ekf.bottomRows<3>().squaredNorm());
+    EXPECT_EQ(0.0, Xdot_ekf(StateIdx::kLeftVoltageError));
+    EXPECT_EQ(0.0, Xdot_ekf(StateIdx::kRightVoltageError));
+    EXPECT_EQ(0.0, Xdot_ekf(StateIdx::kAngularError));
   }
-  State DiffEq(const State &X, const Input &U) { return ekf_.DiffEq(X, U); }
+  State DiffEq(const State &X, const Input &U, bool ignore_accel) {
+    return ekf_.DiffEq(X, U, ignore_accel);
+  }
+  StateSquare AForState(const State &X, bool ignore_accel) {
+    return ekf_.AForState(X, ignore_accel);
+  }
 
   // Returns a random value sampled from a normal distribution with a standard
   // deviation of std and a mean of zero.
@@ -75,49 +114,113 @@
 
   DrivetrainConfig<double> dt_config_;
   HybridEkf<> ekf_;
-  ::aos::monotonic_clock::time_point t0_;
+  aos::monotonic_clock::time_point t0_;
   StateFeedbackHybridPlantCoefficients<2, 2, 2> velocity_plant_coefs_;
 
-  ::std::mt19937 gen_{static_cast<uint32_t>(::aos::testing::RandomSeed())};
-  ::std::normal_distribution<> normal_;
+  std::mt19937 gen_{static_cast<uint32_t>(::aos::testing::RandomSeed())};
+  std::normal_distribution<> normal_;
 };
 
-// Tests that the dynamics from DiffEq behave as expected:
-TEST_F(HybridEkfTest, CheckDynamics) {
-  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(),
-      {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(),
-      {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(),
-      {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(),
-      {5.0, 6.0});
-}
 
 // Tests that if we provide a bunch of observations of the position
 // with zero change in time, the state should approach the estimation.
+struct DiffEqInputs {
+  State X;
+  Input U;
+  bool ignore_accel;
+};
+
+DiffEqInputs MakeDiffEqInputs(State X, Input U, bool ignore_accel) {
+  return {.X = X, .U = U, .ignore_accel = ignore_accel};
+}
+
+class HybridEkfDiffEqTest : public HybridEkfTest,
+                            public ::testing::WithParamInterface<DiffEqInputs> {
+};
+
+// Tests that the dynamics from DiffEq behave as expected:
+TEST_P(HybridEkfDiffEqTest, CheckDynamics) {
+  CheckDiffEq(GetParam().X, GetParam().U, GetParam().ignore_accel);
+
+  // Calculate whether A seems to be empirically correct.
+  const StateSquare A = AForState(GetParam().X, GetParam().ignore_accel);
+  for (int ii = 0; ii < HybridEkf<>::kNStates; ++ii) {
+    SCOPED_TRACE(ii);
+    State perturbation = State::Zero();
+    constexpr double kEps = 1e-5;
+    perturbation(ii) = kEps;
+    const State Xdot_plus = DiffEq(GetParam().X + perturbation, GetParam().U,
+                                   GetParam().ignore_accel);
+    const State Xdot_minus = DiffEq(GetParam().X - perturbation, GetParam().U,
+                                   GetParam().ignore_accel);
+    const State numerical_dXdot_dX = (Xdot_plus - Xdot_minus) / (2.0 * kEps);
+    const State A_based_dXdot_dX = A * perturbation / kEps;
+    EXPECT_LT((A_based_dXdot_dX - numerical_dXdot_dX).norm(), 1e-8)
+        << "A * X: " << A_based_dXdot_dX << " numerical: " << numerical_dXdot_dX
+        << " difference: " << (A_based_dXdot_dX - numerical_dXdot_dX);
+  }
+}
+
+INSTANTIATE_TEST_CASE_P(
+    CheckMathTest, HybridEkfDiffEqTest,
+    ::testing::Values(DiffEqInputs{State::Zero(), Input::Zero(), false},
+                      DiffEqInputs{State::Zero(), Input::Zero(), true},
+                      DiffEqInputs{State::Zero(), {-5.0, 5.0, 0.0, 0.0}, false},
+                      DiffEqInputs{State::Zero(), {-5.0, 5.0, 0.0, 0.0}, true},
+                      DiffEqInputs{State::Zero(), {12.0, 3.0, 0.0, 0.0}, false},
+                      DiffEqInputs{State::Zero(), {12.0, 3.0, 0.0, 0.0}, true},
+                      DiffEqInputs{(State() << 100.0, 200.0, M_PI, 1.234, 0.5,
+                                    1.2, 0.6, 3.0, -4.0, 0.3, 0.0, 0.0)
+                                       .finished(),
+                                   {3.0, 4.0, 5.0, 6.0},
+                                   false},
+                      DiffEqInputs{(State() << 100.0, 200.0, M_PI, 1.234, 0.5,
+                                    1.2, 0.6, 3.0, -4.0, 0.3, 0.0, 0.0)
+                                       .finished(),
+                                   {3.0, 4.0, 5.0, 6.0},
+                                   true},
+                      DiffEqInputs{(State() << 100.0, 200.0, 2.0, 1.234, 0.5,
+                                    1.2, 0.6, 3.0, -4.0, 0.3, 0.0, 0.0)
+                                       .finished(),
+                                   {3.0, 4.0, 5.0, 6.0},
+                                   false},
+                      DiffEqInputs{(State() << 100.0, 200.0, 2.0, 1.234, 0.5,
+                                    1.2, 0.6, 3.0, -4.0, 0.3, 0.0, 0.0)
+                                       .finished(),
+                                   {3.0, 4.0, 5.0, 6.0},
+                                   true},
+                      DiffEqInputs{(State() << 100.0, 200.0, 2.0, 1.234, 0.5,
+                                    1.2, 0.6, 3.0, -4.0, 0.3, 0.1, 0.2)
+                                       .finished(),
+                                   {3.0, 4.0, 5.0, 6.0},
+                                   false},
+                      DiffEqInputs{(State() << 100.0, 200.0, -2.0, 1.234, 0.5,
+                                    1.2, 0.6, 3.0, -4.0, 0.3, 0.0, 0.0)
+                                       .finished(),
+                                   {-3.0, -4.0, -5.0, -6.0},
+                                   false},
+                      DiffEqInputs{(State() << 100.0, 200.0, -2.0, 1.234, 0.5,
+                                    1.2, 0.6, 3.0, -4.0, 0.3, 0.0, 0.0)
+                                       .finished(),
+                                   {-3.0, -4.0, -5.0, -6.0},
+                                   true},
+                      // And check that a theta outside of [-M_PI, M_PI] works.
+                      DiffEqInputs{(State() << 100.0, 200.0, 200.0, 1.234, 0.5,
+                                    1.2, 0.6, 3.0, -4.0, 0.3, 0.0, 0.0)
+                                       .finished(),
+                                   {3.0, 4.0, 5.0, 6.0},
+                                   false}));
+
 TEST_F(HybridEkfTest, ZeroTimeCorrect) {
   HybridEkf<>::Output Z(0.5, 0.5, 1);
-  Eigen::Matrix<double, 3, 10> H;
+  Eigen::Matrix<double, 3, 12> H;
   H.setIdentity();
   auto h = [H](const State &X, const Input &) { return H * X; };
   auto dhdx = [H](const State &) { return H; };
   Eigen::Matrix<double, 3, 3> R;
   R.setIdentity();
   R *= 1e-3;
-  Input U(0, 0);
+  Input U = Input::Zero();
   EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kTheta));
   const double starting_p_norm = ekf_.P().norm();
   for (int ii = 0; ii < 100; ++ii) {
@@ -138,14 +241,14 @@
   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, 12> H;
   H.setZero();
   auto h = [H](const State &X, const Input &) { return H * X; };
   auto dhdx = [H](const State &) { return H; };
   // Provide constant input voltage.
-  Eigen::Matrix<double, 2, 1> U;
-  U << 12.0, 10.0;
-  // The exact value of the noise matrix ix unimportant so long as it is
+  Input U;
+  U << 12.0, 10.0, 1.0, -0.1;
+  // The exact value of the noise matrix is unimportant so long as it is
   // non-zero.
   Eigen::Matrix<double, 3, 3> R;
   R.setIdentity();
@@ -153,25 +256,23 @@
   const double starting_p_norm = ekf_.P().norm();
   for (int ii = 0; ii < 100; ++ii) {
     ekf_.Correct(Z, &U, {}, h, dhdx, R, t0_ + dt_config_.dt * (ii + 1));
-    true_X = Update(true_X, U);
+    true_X = Update(true_X, U, false);
     EXPECT_EQ(true_X, ekf_.X_hat());
   }
   // We don't care about precise results, just that they are generally sane:
   // robot should've travelled forwards and slightly to the right.
-  EXPECT_NEAR(0.9, ekf_.X_hat(StateIdx::kX), 0.1);
-  EXPECT_NEAR(-0.15, ekf_.X_hat(StateIdx::kY), 0.05);
-  EXPECT_NEAR(-0.3, ekf_.X_hat(StateIdx::kTheta), 0.05);
-  EXPECT_NEAR(1.0, ekf_.X_hat(StateIdx::kLeftEncoder), 0.1);
-  EXPECT_NEAR(ekf_.X_hat(StateIdx::kLeftEncoder) * 0.8,
-              ekf_.X_hat(StateIdx::kRightEncoder),
-              ekf_.X_hat(StateIdx::kLeftEncoder) * 0.1);
-  EXPECT_NEAR(2.4, ekf_.X_hat(StateIdx::kLeftVelocity), 0.1);
-  EXPECT_NEAR(ekf_.X_hat(StateIdx::kLeftVelocity) * 0.8,
-              ekf_.X_hat(StateIdx::kRightVelocity),
-              ekf_.X_hat(StateIdx::kLeftVelocity) * 0.1);
+  EXPECT_NEAR(0.1, ekf_.X_hat(StateIdx::kX), 0.05);
+  EXPECT_NEAR(-0.02, ekf_.X_hat(StateIdx::kY), 0.01);
+  EXPECT_GT(0.01, ekf_.X_hat(StateIdx::kTheta));
+  EXPECT_NEAR(0.3, ekf_.X_hat(StateIdx::kLeftEncoder), 0.1);
+  EXPECT_NEAR(0.7, ekf_.X_hat(StateIdx::kLeftVelocity), 0.1);
+  EXPECT_NEAR(0.3, ekf_.X_hat(StateIdx::kRightVelocity), 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));
+  // Without encoder updates, the longitudinal velocity offset should be zero.
+  EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kLongitudinalVelocityOffset));
+  EXPECT_NEAR(0.04, ekf_.X_hat(StateIdx::kLateralVelocity), 0.01);
   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);
@@ -191,12 +292,12 @@
 TEST_P(HybridEkfOldCorrectionsTest, CreateOldCorrection) {
   HybridEkf<>::Output Z;
   Z.setZero();
-  Eigen::Matrix<double, 3, 10> H;
+  Eigen::Matrix<double, 3, 12> H;
   H.setZero();
   auto h_zero = [H](const State &X, const Input &) { return H * X; };
   auto dhdx_zero = [H](const State &) { return H; };
-  Eigen::Matrix<double, 2, 1> U;
-  U << 12.0, 12.0;
+  Input U;
+  U << 12.0, 12.0, 0.0, 0.0;
   Eigen::Matrix<double, 3, 3> R;
   R = R.Identity();
   EXPECT_EQ(0.0, ekf_.X_hat().norm());
@@ -247,11 +348,12 @@
 TEST_F(HybridEkfTest, DiscardTooOldCorrection) {
   HybridEkf<>::Output Z;
   Z.setZero();
-  Eigen::Matrix<double, 3, 10> H;
+  Eigen::Matrix<double, 3, 12> H;
   H.setZero();
   auto h_zero = [H](const State &X, const Input &) { return H * X; };
   auto dhdx_zero = [H](const State &) { return H; };
-  Eigen::Matrix<double, 2, 1> U(12.0, 12.0);
+  Input U;
+  U << 12.0, 12.0, 0.0, 0.0;
   Eigen::Matrix<double, 3, 3> R;
   R.setIdentity();
 
@@ -286,15 +388,16 @@
 // provided:
 TEST_F(HybridEkfTest, PerfectEncoderUpdate) {
   State true_X = ekf_.X_hat();
-  Input U(-1.0, 5.0);
+  Input U;
+  U << -1.0, 5.0, 0.0, 0.0;
   for (int ii = 0; ii < 100; ++ii) {
-    true_X = Update(true_X, U);
-    ekf_.UpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
-                               true_X(StateIdx::kRightEncoder, 0),
-                               (true_X(StateIdx::kRightVelocity, 0) -
-                                true_X(StateIdx::kLeftVelocity, 0)) /
-                                   dt_config_.robot_radius / 2.0,
-                               U, t0_ + (ii + 1) * dt_config_.dt);
+    true_X = Update(true_X, U, false);
+    ekf_.RawUpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
+                                  true_X(StateIdx::kRightEncoder, 0),
+                                  (true_X(StateIdx::kRightVelocity, 0) -
+                                   true_X(StateIdx::kLeftVelocity, 0)) /
+                                      dt_config_.robot_radius / 2.0,
+                                  U, t0_ + (ii + 1) * dt_config_.dt);
     EXPECT_NEAR((true_X - ekf_.X_hat()).squaredNorm(), 0.0, 1e-25)
         << "Expected only floating point precision errors in update step. "
            "Estimated X_hat:\n"
@@ -303,29 +406,6 @@
   }
 }
 
-// Tests that encoder updates cause everything to converge properly in the
-// presence of voltage 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;
-  Input U(5.0, 5.0);
-  for (int ii = 0; ii < 1000; ++ii) {
-    true_X = Update(true_X, U);
-    ekf_.UpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
-                               true_X(StateIdx::kRightEncoder, 0),
-                               (true_X(StateIdx::kRightVelocity, 0) -
-                                true_X(StateIdx::kLeftVelocity, 0)) /
-                                   dt_config_.robot_radius / 2.0,
-                               U, t0_ + (ii + 1) * dt_config_.dt);
-  }
-  EXPECT_NEAR((true_X - ekf_.X_hat()).norm(), 0.0, 5e-2)
-      << "Expected non-x/y estimates to converge to correct. "
-         "Estimated X_hat:\n"
-      << ekf_.X_hat() << "\ntrue X:\n"
-      << true_X;
-}
-
 // 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
@@ -340,15 +420,16 @@
   // Note: Because we don't have any absolute measurements used for corrections,
   // we will get slightly off on the absolute x/y/theta, but the errors are so
   // small they are negligible.
-  Input U(10.0, 5.0);
+  Input U;
+  U << 10.0, 5.0, 0.0, 0.0;
   for (int ii = 0; ii < 100; ++ii) {
-    true_X = Update(true_X, U);
-    ekf_.UpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
-                               true_X(StateIdx::kRightEncoder, 0),
-                               (true_X(StateIdx::kRightVelocity, 0) -
-                                true_X(StateIdx::kLeftVelocity, 0)) /
-                                   dt_config_.robot_radius / 2.0,
-                               U, t0_ + (ii + 1) * dt_config_.dt);
+    true_X = Update(true_X, U, false);
+    ekf_.RawUpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
+                                  true_X(StateIdx::kRightEncoder, 0),
+                                  (true_X(StateIdx::kRightVelocity, 0) -
+                                   true_X(StateIdx::kLeftVelocity, 0)) /
+                                      dt_config_.robot_radius / 2.0,
+                                  U, t0_ + (ii + 1) * dt_config_.dt);
   }
   EXPECT_NEAR((true_X - ekf_.X_hat()).norm(), 0.0, 3e-5)
       << "Expected non-x/y estimates to converge to correct. "
@@ -365,10 +446,11 @@
   true_X(StateIdx::kAngularError, 0) = 1.0;
   true_X(StateIdx::kLeftEncoder, 0) += 2.0;
   true_X(StateIdx::kRightEncoder, 0) -= 2.0;
-  Input U(10.0, 5.0);
+  Input U;
+  U << 10.0, 5.0, 0.0, 0.0;
   for (int ii = 0; ii < 100; ++ii) {
-    true_X = Update(true_X, U);
-    ekf_.UpdateEncodersAndGyro(
+    true_X = Update(true_X, U, false);
+    ekf_.RawUpdateEncodersAndGyro(
         true_X(StateIdx::kLeftEncoder, 0) + Normal(1e-3),
         true_X(StateIdx::kRightEncoder, 0) + Normal(1e-3),
         (true_X(StateIdx::kRightVelocity, 0) -
@@ -391,15 +473,16 @@
 TEST_F(HybridEkfDeathTest, DieIfUninitialized) {
   HybridEkf<> ekf(dt_config_);
   // Expect death if we fail to initialize before starting to provide updates.
-  EXPECT_DEATH(ekf.UpdateEncodersAndGyro(0.0, 1.0, 2.0, {3.0, 3.0}, t0_),
-               "observations_.empty()");
+  EXPECT_DEATH(
+      ekf.RawUpdateEncodersAndGyro(0.0, 1.0, 2.0, {3.0, 3.0, 0.0, 0.0}, t0_),
+      "observations_.empty()");
 }
 
 TEST_F(HybridEkfDeathTest, DieOnNoU) {
   // Expect death if the user does not provide U when creating a fresh
   // measurement.
   EXPECT_DEATH(ekf_.Correct({1, 2, 3}, nullptr, {}, {}, {}, {},
-                            t0_ + ::std::chrono::seconds(1)),
+                            t0_ + std::chrono::seconds(1)),
                "U != nullptr");
 }
 
@@ -407,22 +490,23 @@
 // that we die when an improper combination is provided.
 TEST_F(HybridEkfDeathTest, DieOnNoH) {
   // Check that we die when no h-related functions are provided:
-  Input U(1, 2);
+  Input U;
+  U << 1.0, 2.0, 0.0, 0.0;
   EXPECT_DEATH(ekf_.Correct({1, 2, 3}, &U, {}, {}, {}, {},
-                            t0_ + ::std::chrono::seconds(1)),
+                            t0_ + std::chrono::seconds(1)),
                "make_h");
   // 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, 12>::Zero();
                             },
-                            {}, t0_ + ::std::chrono::seconds(1)),
+                            {}, t0_ + std::chrono::seconds(1)),
                "make_h");
   EXPECT_DEATH(ekf_.Correct({1, 2, 3}, &U, {},
                             [](const State &, const Input &) {
                               return Eigen::Matrix<double, 3, 1>::Zero();
                             },
-                            {}, {}, t0_ + ::std::chrono::seconds(1)),
+                            {}, {}, t0_ + std::chrono::seconds(1)),
                "make_h");
 }
 
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 2589d4b..91fd16a 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -3,6 +3,7 @@
 
 #include "aos/events/event_loop.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "frc971/control_loops/drivetrain/hybrid_ekf.h"
 #include "frc971/control_loops/pose.h"
 
@@ -48,13 +49,30 @@
   virtual void Update(const ::Eigen::Matrix<double, 2, 1> &U,
                       ::aos::monotonic_clock::time_point now,
                       double left_encoder, double right_encoder,
-                      double gyro_rate, double longitudinal_accelerometer) = 0;
+                      double gyro_rate, const Eigen::Vector3d &accel) = 0;
   // Reset the absolute position of the estimator.
   virtual void ResetPosition(::aos::monotonic_clock::time_point t, double x,
                              double y, double theta, double theta_uncertainty,
                              bool reset_theta) = 0;
+  flatbuffers::Offset<LocalizerState> PopulateStatus(
+      flatbuffers::FlatBufferBuilder *fbb) {
+    LocalizerState::Builder builder(*fbb);
+    builder.add_x(x());
+    builder.add_y(y());
+    builder.add_theta(theta());
+    builder.add_left_velocity(left_velocity());
+    builder.add_right_velocity(right_velocity());
+    builder.add_left_encoder(left_encoder());
+    builder.add_right_encoder(right_encoder());
+    builder.add_left_voltage_error(left_voltage_error());
+    builder.add_right_voltage_error(right_voltage_error());
+    builder.add_angular_error(angular_error());
+    builder.add_longitudinal_velocity_offset(longitudinal_velocity_offset());
+    builder.add_lateral_velocity(lateral_velocity());
+    return builder.Finish();
+  }
   // There are several subtly different norms floating around for state
-  // matrices. In order to avoid that mess, we jus tprovide direct accessors for
+  // matrices. In order to avoid that mess, we just provide direct accessors for
   // the values that most people care about.
   virtual double x() const = 0;
   virtual double y() const = 0;
@@ -65,6 +83,9 @@
   virtual double right_encoder() const = 0;
   virtual double left_voltage_error() const = 0;
   virtual double right_voltage_error() const = 0;
+  virtual double angular_error() const = 0;
+  virtual double longitudinal_velocity_offset() const = 0;
+  virtual double lateral_velocity() const = 0;
   virtual TargetSelectorInterface *target_selector() = 0;
 };
 
@@ -110,8 +131,9 @@
   void Update(const ::Eigen::Matrix<double, 2, 1> &U,
               ::aos::monotonic_clock::time_point now, double left_encoder,
               double right_encoder, double gyro_rate,
-              double /*longitudinal_accelerometer*/) override {
-    ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, now);
+              const Eigen::Vector3d &accel) override {
+    ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, accel,
+                               now);
   }
 
   void ResetPosition(::aos::monotonic_clock::time_point t, double x, double y,
@@ -119,9 +141,10 @@
                      bool /*reset_theta*/) override {
     const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
     const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
-    ekf_.ResetInitialState(t, (Ekf::State() << x, y, theta, left_encoder, 0,
-                               right_encoder, 0, 0, 0,
-                               0).finished(),
+    ekf_.ResetInitialState(t,
+                           (Ekf::State() << x, y, theta, left_encoder, 0,
+                            right_encoder, 0, 0, 0, 0, 0, 0)
+                               .finished(),
                            ekf_.P());
   };
 
@@ -146,6 +169,15 @@
   double right_voltage_error() const override {
     return ekf_.X_hat(StateIdx::kRightVoltageError);
   }
+  double angular_error() const override {
+    return ekf_.X_hat(StateIdx::kAngularError);
+  }
+  double longitudinal_velocity_offset() const override {
+    return ekf_.X_hat(StateIdx::kLongitudinalVelocityOffset);
+  }
+  double lateral_velocity() const override {
+    return ekf_.X_hat(StateIdx::kLateralVelocity);
+  }
 
   TrivialTargetSelector *target_selector() override {
     return &target_selector_;
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index c436232..16cd723 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -58,17 +58,17 @@
   localizer_.ResetInitialState(now, state, newP);
 }
 
-void EventLoopLocalizer::Update(
-    const ::Eigen::Matrix<double, 2, 1> &U,
-    ::aos::monotonic_clock::time_point now, double left_encoder,
-    double right_encoder, double gyro_rate,
-    double /*longitudinal_accelerometer*/) {
+void EventLoopLocalizer::Update(const ::Eigen::Matrix<double, 2, 1> &U,
+                                ::aos::monotonic_clock::time_point now,
+                                double left_encoder, double right_encoder,
+                                double gyro_rate,
+                                const Eigen::Vector3d &accel) {
   AOS_CHECK(U.allFinite());
   AOS_CHECK(::std::isfinite(left_encoder));
   AOS_CHECK(::std::isfinite(right_encoder));
   AOS_CHECK(::std::isfinite(gyro_rate));
   localizer_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U,
-                                   now);
+                                   accel, now);
   while (frame_fetcher_.FetchNext()) {
     HandleFrame(frame_fetcher_.get());
   }
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.h b/y2019/control_loops/drivetrain/event_loop_localizer.h
index 83b8e79..4c6150d 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -60,7 +60,7 @@
   void Update(const ::Eigen::Matrix<double, 2, 1> &U,
               ::aos::monotonic_clock::time_point now, double left_encoder,
               double right_encoder, double gyro_rate,
-              double /*longitudinal_accelerometer*/) override;
+              const Eigen::Vector3d &accel) override;
 
   double x() const override {
     return localizer_.X_hat(StateIdx::kX); }
@@ -86,6 +86,15 @@
   double right_voltage_error() const override {
     return localizer_.X_hat(StateIdx::kRightVoltageError);
   }
+  double angular_error() const override {
+    return localizer_.X_hat(StateIdx::kAngularError);
+  }
+  double longitudinal_velocity_offset() const override {
+    return localizer_.X_hat(StateIdx::kLongitudinalVelocityOffset);
+  }
+  double lateral_velocity() const override {
+    return localizer_.X_hat(StateIdx::kLateralVelocity);
+  }
 
   TargetSelector *target_selector() override {
     return &target_selector_;
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index 9b237f4..1459248 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -111,8 +111,7 @@
     const Eigen::Matrix<double, 5, 1> true_state = drivetrain_plant_.state();
     EXPECT_NEAR(localizer_.x(), true_state(0, 0), eps);
     EXPECT_NEAR(localizer_.y(), true_state(1, 0), eps);
-    // TODO(james): Uncomment this.
-    //EXPECT_NEAR(localizer_.theta(), true_state(2, 0), 2.0 * eps);
+    EXPECT_NEAR(localizer_.theta(), true_state(2, 0), eps);
     EXPECT_NEAR(localizer_.left_velocity(), true_state(3, 0), eps);
     EXPECT_NEAR(localizer_.right_velocity(), true_state(4, 0), eps);
   }
@@ -290,8 +289,7 @@
   // Everything but X-value should be correct:
   EXPECT_NEAR(true_state.x(), localizer_.x() + 0.05, 1e-5);
   EXPECT_NEAR(true_state.y(), localizer_.y(), 1e-5);
-  // TODO(james): Uncomment this.
-  //EXPECT_NEAR(true_state(2, 0), localizer_.theta(), 1e-3);
+  EXPECT_NEAR(true_state(2, 0), localizer_.theta(), 1e-3);
   EXPECT_NEAR(true_state(3, 0), localizer_.left_velocity(), 1e-4);
   EXPECT_NEAR(true_state(4, 0), localizer_.right_velocity(), 1e-4);
 }
@@ -383,10 +381,9 @@
   VerifyEstimatorAccurate(0.2);
   // Due to the fact that we aren't modulating the throttle, we don't try to hit
   // the target exactly. Instead, just run slightly past the target:
-  // TODO(james): Uncomment this.
-  //EXPECT_LT(
-  //    ::std::abs(::aos::math::DiffAngle(M_PI, drivetrain_plant_.state()(2, 0))),
-  //    0.5);
+  EXPECT_LT(
+      ::std::abs(::aos::math::DiffAngle(M_PI, drivetrain_plant_.state()(2, 0))),
+      0.5);
   EXPECT_GT(HPSlotLeft().abs_pos().x(), drivetrain_plant_.state().x());
   EXPECT_NEAR(HPSlotLeft().abs_pos().y(), drivetrain_plant_.state().y(), 0.2);
 }
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index aa7f22f..7b25ca5 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -142,9 +142,6 @@
   }
 
   void SetUp() {
-    // Turn on -v 1
-    FLAGS_v = std::max(FLAGS_v, 1);
-
     flatbuffers::DetachedBuffer goal_buffer;
     {
       flatbuffers::FlatBufferBuilder fbb;
@@ -315,8 +312,12 @@
   }
   // The differential equation for the dynamics of the system.
   TestLocalizer::State DiffEq(const TestLocalizer::State &X,
-                              const TestLocalizer::Input &U) {
-    return localizer_.DiffEq(X, U);
+                              const Eigen::Vector2d &voltage) {
+    TestLocalizer::Input U;
+    U.setZero();
+    U(0) = voltage(0);
+    U(1) = voltage(1);
+    return localizer_.DiffEq(X, U, true);
   }
 
   Field field_;
@@ -433,7 +434,7 @@
     output.left_voltage = 0;
     output.right_voltage = 0;
     spline_drivetrain_.SetOutput(&output);
-    TestLocalizer::Input U(output.left_voltage, output.right_voltage);
+    Eigen::Vector2d U(output.left_voltage, output.right_voltage);
 
     const ::Eigen::Matrix<double, 5, 1> goal_state =
         spline_drivetrain_.CurrentGoalState();
@@ -454,7 +455,7 @@
     U(1, 0) = ::std::max(::std::min(U(1, 0), 12.0), -12.0);
 
     state = ::frc971::control_loops::RungeKuttaU(
-        [this](const ::Eigen::Matrix<double, 10, 1> &X,
+        [this](const ::Eigen::Matrix<double, 12, 1> &X,
                const ::Eigen::Matrix<double, 2, 1> &U) { return DiffEq(X, U); },
         state, U, ::aos::time::DurationInSeconds(dt_config_.dt));
 
@@ -462,6 +463,8 @@
     // interest here are that we (a) stop adding disturbances at the very end of
     // the trajectory, to allow us to actually converge to the goal, and (b)
     // scale disturbances by the corruent velocity.
+    // TODO(james): Figure out how to persist good accelerometer values through
+    // the disturbances.
     if (GetParam().disturb && i % 75 == 0) {
       // Scale the disturbance so that when we have near-zero velocity we don't
       // have much disturbance.
@@ -470,7 +473,8 @@
                            ::std::pow(state(StateIdx::kRightVelocity, 0), 2)) /
                    3.0);
       TestLocalizer::State disturbance;
-      disturbance << 0.02, 0.02, 0.001, 0.03, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0;
+      disturbance << 0.02, 0.02, 0.001, 0.03, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0,
+          0.0, 0.0;
       disturbance *= disturbance_scale;
       state += disturbance;
     }
@@ -482,13 +486,18 @@
     const double left_enc = state(StateIdx::kLeftEncoder, 0);
     const double right_enc = state(StateIdx::kRightEncoder, 0);
 
-    const double gyro = (state(StateIdx::kRightVelocity, 0) -
-                         state(StateIdx::kLeftVelocity, 0)) /
+    const double gyro = (state(StateIdx::kRightVelocity) -
+                         state(StateIdx::kLeftVelocity)) /
                         dt_config_.robot_radius / 2.0;
+    const TestLocalizer::State xdot = DiffEq(state, U);
+    const Eigen::Vector3d accel(
+        localizer_.CalcLongitudinalVelocity(xdot) -
+            gyro * state(StateIdx::kLateralVelocity),
+        gyro * localizer_.CalcLongitudinalVelocity(state), 1.0);
 
     localizer_.UpdateEncodersAndGyro(left_enc + Normal(encoder_noise_),
                                      right_enc + Normal(encoder_noise_),
-                                     gyro + Normal(gyro_noise_), U, t);
+                                     gyro + Normal(gyro_noise_), U, accel, t);
 
     for (size_t cam_idx = 0; cam_idx < camera_queues.size(); ++cam_idx) {
       auto &camera_queue = camera_queues[cam_idx];
@@ -551,14 +560,14 @@
             /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
             /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
             (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-             0.0, 0.0)
+             0.0, 0.0, 0.0, 0.0)
                 .finished(),
             (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-             0.0, 0.0)
+             0.0, 0.0, 0.0, 0.0)
                 .finished(),
             /*noisify=*/false,
             /*disturb=*/false,
-            /*estimate_tolerance=*/1e-5,
+            /*estimate_tolerance=*/1e-2,
             /*goal_tolerance=*/2e-2,
         }),
         // Checks "perfect" estimation, but start off the spline and check
@@ -567,14 +576,14 @@
             /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
             /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
             (TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-             0.0, 0.0)
+             0.0, 0.0, 0.0, 0.0)
                 .finished(),
             (TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-             0.0, 0.0)
+             0.0, 0.0, 0.0, 0.0)
                 .finished(),
             /*noisify=*/false,
             /*disturb=*/false,
-            /*estimate_tolerance=*/1e-5,
+            /*estimate_tolerance=*/1e-2,
             /*goal_tolerance=*/2e-2,
         }),
         // Repeats perfect scenario, but add sensor noise.
@@ -582,10 +591,10 @@
             /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
             /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
             (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-             0.0, 0.0)
+             0.0, 0.0, 0.0, 0.0)
                 .finished(),
             (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-             0.0, 0.0)
+             0.0, 0.0, 0.0, 0.0)
                 .finished(),
             /*noisify=*/true,
             /*disturb=*/false,
@@ -597,14 +606,14 @@
             /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
             /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
             (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-             0.0, 0.0)
+             0.0, 0.0, 0.0, 0.0)
                 .finished(),
             (TestLocalizer::State() << 0.1, -5.1, -0.01, 0.0, 0.0, 0.0, 0.0,
-             0.0, 0.0, 0.0)
+             0.0, 0.0, 0.0, 0.0, 0.0)
                 .finished(),
             /*noisify=*/false,
             /*disturb=*/false,
-            /*estimate_tolerance=*/1e-4,
+            /*estimate_tolerance=*/1e-2,
             /*goal_tolerance=*/2e-2,
         }),
         // Repeats perfect scenario, but add voltage + angular errors:
@@ -612,14 +621,14 @@
             /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
             /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
             (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
-             0.5, 0.02)
+             0.5, 0.02, 0.0, 0.0)
                 .finished(),
             (TestLocalizer::State() << 0.1, -5.1, -0.01, 0.0, 0.0, 0.0, 0.0,
-             0.0, 0.0, 0.0)
+             0.0, 0.0, 0.0, 0.0, 0.0)
                 .finished(),
             /*noisify=*/false,
             /*disturb=*/false,
-            /*estimate_tolerance=*/1e-4,
+            /*estimate_tolerance=*/1e-2,
             /*goal_tolerance=*/2e-2,
         }),
         // Add disturbances while we are driving:
@@ -627,14 +636,14 @@
             /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
             /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
             (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-             0.0, 0.0)
+             0.0, 0.0, 0.0, 0.0)
                 .finished(),
             (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-             0.0, 0.0)
+             0.0, 0.0, 0.0, 0.0)
                 .finished(),
             /*noisify=*/false,
             /*disturb=*/true,
-            /*estimate_tolerance=*/3e-2,
+            /*estimate_tolerance=*/4e-2,
             /*goal_tolerance=*/0.15,
         }),
         // Add noise and some initial error in addition:
@@ -642,15 +651,15 @@
             /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
             /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
             (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-             0.0, 0.0)
+             0.0, 0.0, 0.0, 0.0)
                 .finished(),
             (TestLocalizer::State() << 0.1, -5.1, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0,
-             0.0, 0.0)
+             0.0, 0.0, 0.0, 0.0)
                 .finished(),
             /*noisify=*/true,
             /*disturb=*/true,
-            /*estimate_tolerance=*/0.2,
-            /*goal_tolerance=*/0.5,
+            /*estimate_tolerance=*/0.5,
+            /*goal_tolerance=*/1.0,
         }),
         // Try another spline, just in case the one I was using is special for
         // some reason; this path will also go straight up to a target, to
@@ -659,10 +668,10 @@
             /*control_pts_x=*/{{0.5, 3.5, 4.0, 8.0, 11.0, 10.2}},
             /*control_pts_y=*/{{1.0, 1.0, -3.0, -2.0, -3.5, -3.65}},
             (TestLocalizer::State() << 0.6, 1.01, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
-             0.0, 0.0)
+             0.0, 0.0, 0.0, 0.0)
                 .finished(),
             (TestLocalizer::State() << 0.5, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-             0.0, 0.0)
+             0.0, 0.0, 0.0, 0.0)
                 .finished(),
             /*noisify=*/true,
             /*disturb=*/false,