diff --git a/aos/logging/log_namer.cc b/aos/logging/log_namer.cc
index 83a39c8..f025054 100644
--- a/aos/logging/log_namer.cc
+++ b/aos/logging/log_namer.cc
@@ -144,21 +144,25 @@
 
   char *tmp;
   AllocateLogName(&tmp, folder, basename);
+
+  std::string log_base_name = tmp;
+  std::string log_roborio_name = log_base_name + "_roborio_data.bfbs";
+  free(tmp);
+
   char *tmp2;
-  if (asprintf(&tmp2, "%s/%s-current", folder, basename) == -1) {
+  if (asprintf(&tmp2, "%s/%s-current.bfbs", folder, basename) == -1) {
     PLOG(WARNING) << "couldn't create current symlink name";
   } else {
     if (unlink(tmp2) == -1 && (errno != EROFS && errno != ENOENT)) {
       LOG(WARNING) << "unlink('" << tmp2 << "') failed";
     }
-    if (symlink(tmp, tmp2) == -1) {
-      PLOG(WARNING) << "symlink('" << tmp << "', '" << tmp2 << "') failed";
+    if (symlink(log_roborio_name.c_str(), tmp2) == -1) {
+      PLOG(WARNING) << "symlink('" << log_roborio_name.c_str() << "', '" << tmp2
+                    << "') failed";
     }
     free(tmp2);
   }
-  std::string result = tmp;
-  free(tmp);
-  return result;
+  return log_base_name;
 }
 
 }  // namespace logging
diff --git a/aos/network/BUILD b/aos/network/BUILD
index 89853f0..8f3f394 100644
--- a/aos/network/BUILD
+++ b/aos/network/BUILD
@@ -70,6 +70,7 @@
     ],
     deps = [
         ":team_number",
+        "//aos:configuration",
         "//aos/testing:googletest",
     ],
 )
diff --git a/aos/network/team_number.cc b/aos/network/team_number.cc
index 14fadab..d2fbc8e 100644
--- a/aos/network/team_number.cc
+++ b/aos/network/team_number.cc
@@ -9,6 +9,8 @@
 
 #include "aos/util/string_to_num.h"
 
+DECLARE_string(override_hostname);
+
 namespace aos {
 namespace network {
 namespace team_number_internal {
@@ -101,10 +103,14 @@
 }  // namespace
 
 ::std::string GetHostname() {
-  char buf[256];
-  buf[sizeof(buf) - 1] = '\0';
-  PCHECK(gethostname(buf, sizeof(buf) - 1) == 0);
-  return buf;
+  if (FLAGS_override_hostname.empty()) {
+    char buf[256];
+    buf[sizeof(buf) - 1] = '\0';
+    PCHECK(gethostname(buf, sizeof(buf) - 1) == 0);
+    return buf;
+  } else {
+    return FLAGS_override_hostname;
+  }
 }
 
 uint16_t GetTeamNumber() {
diff --git a/frc971/analysis/plot_configs/localizer.pb b/frc971/analysis/plot_configs/localizer.pb
index f81ec89..644b516 100644
--- a/frc971/analysis/plot_configs/localizer.pb
+++ b/frc971/analysis/plot_configs/localizer.pb
@@ -23,6 +23,16 @@
   axes {
     line {
       y_signal {
+        channel: "DrivetrainStatus"
+        field: "trajectory_logging.y"
+      }
+      x_signal {
+        channel: "DrivetrainStatus"
+        field: "trajectory_logging.x"
+      }
+    }
+    line {
+      y_signal {
         channel: "DrivetrainTruthStatus"
         field: "y"
       }
@@ -31,7 +41,6 @@
         field: "x"
       }
     }
-    share_x_axis: false
     line {
       y_signal {
         channel: "DrivetrainStatus"
diff --git a/frc971/analysis/py_log_reader.cc b/frc971/analysis/py_log_reader.cc
index a9a1db1..08f3707 100644
--- a/frc971/analysis/py_log_reader.cc
+++ b/frc971/analysis/py_log_reader.cc
@@ -87,8 +87,14 @@
   tools->reader = std::make_unique<aos::logger::LogReader>(log_file_name);
   tools->reader->Register();
 
-  tools->event_loop =
-      tools->reader->event_loop_factory()->MakeEventLoop("data_fetcher");
+  if (aos::configuration::MultiNode(tools->reader->configuration())) {
+    tools->event_loop = tools->reader->event_loop_factory()->MakeEventLoop(
+        "data_fetcher",
+        aos::configuration::GetNode(tools->reader->configuration(), "roborio"));
+  } else {
+    tools->event_loop =
+        tools->reader->event_loop_factory()->MakeEventLoop("data_fetcher");
+  }
   tools->event_loop->SkipTimingReport();
   tools->event_loop->SkipAosLog();
 
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 9fef8f9..30abc87 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -691,6 +691,7 @@
         "improved_down_estimator.h",
     ],
     deps = [
+        ":drivetrain_config",
         ":drivetrain_status_fbs",
         "//aos/events:event_loop",
         "//frc971/control_loops:control_loops_fbs",
@@ -707,6 +708,7 @@
         "improved_down_estimator_test.cc",
     ],
     deps = [
+        ":drivetrain_test_lib",
         "//aos/testing:googletest",
         "//aos/testing:random_seed",
         "//frc971/control_loops/drivetrain:improved_down_estimator",
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 77c52c4..1c0ac4f 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -41,6 +41,7 @@
       gyro_reading_fetcher_(
           event_loop->MakeFetcher<::frc971::sensors::GyroReading>(
               "/drivetrain")),
+      down_estimator_(dt_config),
       localizer_(localizer),
       kf_(dt_config_.make_kf_drivetrain_loop()),
       dt_openloop_(dt_config_, &kf_),
@@ -177,6 +178,9 @@
       case IMUType::IMU_Y:
         last_accel_ = -imu_values_fetcher_->accelerometer_y();
         break;
+      case IMUType::IMU_Z:
+        last_accel_ = imu_values_fetcher_->accelerometer_z();
+        break;
     }
   }
 
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 5a29008..a33ff22 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -37,6 +37,7 @@
   IMU_X = 0,          // Use the x-axis of the IMU.
   IMU_Y = 1,          // Use the y-axis of the IMU.
   IMU_FLIPPED_X = 2,  // Use the flipped x-axis of the IMU.
+  IMU_Z = 3,          // Use the z-axis of the IMU.
 };
 
 template <typename Scalar = double>
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 336025a..6a90122 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -190,16 +190,23 @@
   auto builder = imu_sender_.MakeBuilder();
   frc971::IMUValues::Builder imu_builder =
       builder.MakeBuilder<frc971::IMUValues>();
-  imu_builder.add_gyro_x(0.0);
-  imu_builder.add_gyro_y(0.0);
-  imu_builder.add_gyro_z(
-      (drivetrain_plant_.X(3, 0) - drivetrain_plant_.X(1, 0)) /
-      (dt_config_.robot_radius * 2.0));
+  const Eigen::Vector3d gyro =
+      dt_config_.imu_transform.inverse() *
+      Eigen::Vector3d(0.0, 0.0,
+                      (drivetrain_plant_.X(3, 0) - drivetrain_plant_.X(1, 0)) /
+                          (dt_config_.robot_radius * 2.0));
+  imu_builder.add_gyro_x(gyro.x());
+  imu_builder.add_gyro_y(gyro.y());
+  imu_builder.add_gyro_z(gyro.z());
   // Acceleration due to gravity, in m/s/s.
   constexpr double kG = 9.807;
-  imu_builder.add_accelerometer_x(last_acceleration_.x() / kG);
-  imu_builder.add_accelerometer_y(last_acceleration_.y() / kG);
-  imu_builder.add_accelerometer_z(1.0);
+  const Eigen::Vector3d accel =
+      dt_config_.imu_transform.inverse() *
+      Eigen::Vector3d(last_acceleration_.x() / kG, last_acceleration_.y() / kG,
+                      1.0);
+  imu_builder.add_accelerometer_x(accel.x());
+  imu_builder.add_accelerometer_y(accel.y());
+  imu_builder.add_accelerometer_z(accel.z());
   imu_builder.add_monotonic_timestamp_ns(
       std::chrono::duration_cast<std::chrono::nanoseconds>(
           event_loop_->monotonic_now().time_since_epoch())
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.h b/frc971/control_loops/drivetrain/improved_down_estimator.h
index 27de867..c1c8e1b 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.h
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.h
@@ -6,6 +6,7 @@
 
 #include "aos/events/event_loop.h"
 #include "aos/time/time.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
 #include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "frc971/control_loops/runge_kutta.h"
 #include "glog/logging.h"
@@ -276,6 +277,8 @@
 // accelerometer calibration).
 class DrivetrainUkf : public QuaternionUkf {
  public:
+  DrivetrainUkf(const DrivetrainConfig<double> &dt_config)
+      : QuaternionUkf(dt_config.imu_transform) {}
   // UKF for http://kodlab.seas.upenn.edu/uploads/Arun/UKFpaper.pdf
   // Reference in case the link is dead:
   // Kraft, Edgar. "A quaternion-based unscented Kalman filter for orientation
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index 86ef5e9..2edce5a 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -4,6 +4,7 @@
 #include <random>
 
 #include "aos/testing/random_seed.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
 #include "frc971/control_loops/drivetrain/improved_down_estimator.h"
 #include "glog/logging.h"
 #include "gtest/gtest.h"
@@ -91,7 +92,8 @@
 }
 
 TEST(DownEstimatorTest, UkfConstantRotation) {
-  drivetrain::DrivetrainUkf dtukf;
+  drivetrain::DrivetrainUkf dtukf(
+      drivetrain::testing::GetTestDrivetrainConfig());
   const Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
   EXPECT_EQ(0.0,
             (Eigen::Vector3d(0.0, 0.0, 1.0) - dtukf.H(dtukf.X_hat().coeffs()))
@@ -112,7 +114,8 @@
 
 // Tests that the euler angles in the status message are correct.
 TEST(DownEstimatorTest, UkfEulerStatus) {
-  drivetrain::DrivetrainUkf dtukf;
+  drivetrain::DrivetrainUkf dtukf(
+      drivetrain::testing::GetTestDrivetrainConfig());
   const Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
   const Eigen::Vector3d uy = Eigen::Vector3d::UnitY();
   const Eigen::Vector3d uz = Eigen::Vector3d::UnitZ();
@@ -167,7 +170,8 @@
 // that we are slightly rotated, that we eventually adjust our estimate to be
 // correct.
 TEST(DownEstimatorTest, UkfAccelCorrectsBias) {
-  drivetrain::DrivetrainUkf dtukf;
+  drivetrain::DrivetrainUkf dtukf(
+      drivetrain::testing::GetTestDrivetrainConfig());
   const Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
   Eigen::Matrix<double, 3, 1> measurement;
   // Supply the accelerometer with a slightly off reading to ensure that we
@@ -192,7 +196,8 @@
 // that we are slightly rotated, that we eventually adjust our estimate to be
 // correct.
 TEST(DownEstimatorTest, UkfIgnoreBadAccel) {
-  drivetrain::DrivetrainUkf dtukf;
+  drivetrain::DrivetrainUkf dtukf(
+      drivetrain::testing::GetTestDrivetrainConfig());
   const Eigen::Vector3d uy = Eigen::Vector3d::UnitY();
   Eigen::Matrix<double, 3, 1> measurement;
   // Set up a scenario where, if we naively took the accelerometer readings, we
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index a6160e7..d769541 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -75,6 +75,9 @@
     ::std::swap(coefficients_, other.coefficients_);
     X_.swap(other.X_);
     Y_.swap(other.Y_);
+    A_.swap(other.A_);
+    B_.swap(other.B_);
+    DelayedU_.swap(other.DelayedU_);
   }
 
   virtual ~StateFeedbackHybridPlant() {}
@@ -167,8 +170,6 @@
     DelayedU_ = U;
   }
 
-  Eigen::Matrix<Scalar, number_of_inputs, 1> DelayedU_;
-
   Eigen::Matrix<Scalar, number_of_states, 1> Update(
       const Eigen::Matrix<Scalar, number_of_states, 1> X,
       const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
@@ -202,6 +203,8 @@
       number_of_states, number_of_inputs, number_of_outputs>>>
       coefficients_;
 
+  Eigen::Matrix<Scalar, number_of_inputs, 1> DelayedU_;
+
   int index_;
 
   DISALLOW_COPY_AND_ASSIGN(StateFeedbackHybridPlant);
@@ -243,9 +246,13 @@
           *observers)
       : coefficients_(::std::move(*observers)) {}
 
-  HybridKalman(HybridKalman &&other)
-      : X_hat_(other.X_hat_), index_(other.index_) {
+  HybridKalman(HybridKalman &&other) : index_(other.index_) {
     ::std::swap(coefficients_, other.coefficients_);
+
+    X_hat_.swap(other.X_hat_);
+    P_.swap(other.P_);
+    Q_.swap(other.Q_);
+    R_.swap(other.R_);
   }
 
   // Getters for Q
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 0d1b263..d04089b 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -393,97 +393,7 @@
                             self.held_x = self.x
 
     def do_button_press(self, event):
-        print("button press activated")
         # Be consistent with the scaling in the drawing_area
         self.x = event.x * 2
         self.y = event.y * 2
         self.button_press_action()
-
-
-class GridWindow(Gtk.Window):
-    def method_connect(self, event, cb):
-        def handler(obj, *args):
-            cb(*args)
-
-        print("Method_connect ran")
-        self.connect(event, handler)
-
-    def mouse_move(self, event):
-        #Changes event.x and event.y to be relative to the center.
-        x = event.x - self.drawing_area.window_shape[0] / 2
-        y = self.drawing_area.window_shape[1] / 2 - event.y
-        scale = self.drawing_area.get_current_scale()
-        event.x = x / scale + self.drawing_area.center[0]
-        event.y = y / scale + self.drawing_area.center[1]
-        self.drawing_area.mouse_move(event)
-        self.queue_draw()
-
-    def button_press(self, event):
-        print("button press activated")
-        o_x = event.x
-        o_y = event.y
-        x = event.x - self.drawing_area.window_shape[0] / 2
-        y = self.drawing_area.window_shape[1] / 2 - event.y
-        scale = 2 * self.drawing_area.get_current_scale()
-        event.x = x / scale + self.drawing_area.center[0]
-        event.y = y / scale + self.drawing_area.center[1]
-        self.drawing_area.do_button_press(event)
-        event.x = o_x
-        event.y = o_y
-
-    def key_press(self, event):
-        print("key press activated")
-        self.drawing_area.do_key_press(event, self.file_name_box.get_text())
-        self.queue_draw()
-
-    def configure(self, event):
-        print("configure activated")
-        self.drawing_area.window_shape = (event.width, event.height)
-
-    def __init__(self):
-        Gtk.Window.__init__(self)
-
-        self.set_default_size(1.5 * SCREEN_SIZE, SCREEN_SIZE)
-
-        flowBox = Gtk.FlowBox()
-        flowBox.set_valign(Gtk.Align.START)
-        flowBox.set_selection_mode(Gtk.SelectionMode.NONE)
-
-        flowBox.set_valign(Gtk.Align.START)
-
-        self.add(flowBox)
-
-        container = Gtk.Fixed()
-        flowBox.add(container)
-
-        self.eventBox = Gtk.EventBox()
-        container.add(self.eventBox)
-
-        self.eventBox.set_events(Gdk.EventMask.BUTTON_PRESS_MASK
-                                 | Gdk.EventMask.BUTTON_RELEASE_MASK
-                                 | Gdk.EventMask.POINTER_MOTION_MASK
-                                 | Gdk.EventMask.SCROLL_MASK
-                                 | Gdk.EventMask.KEY_PRESS_MASK)
-
-        #add the graph box
-        self.drawing_area = GTK_Widget()
-        self.eventBox.add(self.drawing_area)
-
-        self.method_connect("key-release-event", self.key_press)
-        self.method_connect("button-release-event", self.button_press)
-        self.method_connect("configure-event", self.configure)
-        self.method_connect("motion_notify_event", self.mouse_move)
-
-        self.file_name_box = Gtk.Entry()
-        self.file_name_box.set_size_request(200, 40)
-
-        self.file_name_box.set_text("File")
-        self.file_name_box.set_editable(True)
-
-        container.put(self.file_name_box, 0, 0)
-
-        self.show_all()
-
-
-window = GridWindow()
-RunApp()
diff --git a/frc971/control_loops/python/spline_graph.py b/frc971/control_loops/python/spline_graph.py
index 2fd7964..94ee683 100755
--- a/frc971/control_loops/python/spline_graph.py
+++ b/frc971/control_loops/python/spline_graph.py
@@ -11,11 +11,10 @@
         def handler(obj, *args):
             cb(*args)
 
-        print("Method_connect ran")
         self.connect(event, handler)
 
     def mouse_move(self, event):
-        #Changes event.x and event.y to be relative to the center.
+        # Changes event.x and event.y to be relative to the center.
         x = event.x - self.drawing_area.window_shape[0] / 2
         y = self.drawing_area.window_shape[1] / 2 - event.y
         scale = self.drawing_area.get_current_scale()
@@ -25,25 +24,22 @@
         self.queue_draw()
 
     def button_press(self, event):
-        print("button press activated")
-        o_x = event.x
-        o_y = event.y
+        original_x = event.x
+        original_y = event.y
         x = event.x - self.drawing_area.window_shape[0] / 2
         y = self.drawing_area.window_shape[1] / 2 - event.y
         scale = 2 * self.drawing_area.get_current_scale()
         event.x = x / scale + self.drawing_area.center[0]
         event.y = y / scale + self.drawing_area.center[1]
         self.drawing_area.do_button_press(event)
-        event.x = o_x
-        event.y = o_y
+        event.x = original_x
+        event.y = original_y
 
     def key_press(self, event):
-        print("key press activated")
         self.drawing_area.do_key_press(event, self.file_name_box.get_text())
         self.queue_draw()
 
     def configure(self, event):
-        print("configure activated")
         self.drawing_area.window_shape = (event.width, event.height)
 
     # handle submitting a constraint
@@ -88,7 +84,7 @@
         self.file_name_box = Gtk.Entry()
         self.file_name_box.set_size_request(200, 40)
 
-        self.file_name_box.set_text("File")
+        self.file_name_box.set_text("output_file_name.json")
         self.file_name_box.set_editable(True)
 
         container.put(self.file_name_box, 0, 0)
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 83a54f0..d63bca7 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -397,11 +397,11 @@
       : plant_(::std::move(other.plant_)),
         controller_(::std::move(other.controller_)),
         observer_(::std::move(other.observer_)) {
+    ff_U_.swap(other.ff_U_);
     R_.swap(other.R_);
     next_R_.swap(other.next_R_);
     U_.swap(other.U_);
     U_uncapped_.swap(other.U_uncapped_);
-    ff_U_.swap(other.ff_U_);
   }
 
   virtual ~StateFeedbackLoop() {}
diff --git a/y2020/control_loops/drivetrain/drivetrain_base.cc b/y2020/control_loops/drivetrain/drivetrain_base.cc
index 62679bc..b919902 100644
--- a/y2020/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_base.cc
@@ -25,8 +25,8 @@
   static DrivetrainConfig<double> kDrivetrainConfig{
       ::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
       ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
-      ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
-      ::frc971::control_loops::drivetrain::IMUType::IMU_FLIPPED_X,
+      ::frc971::control_loops::drivetrain::GyroType::IMU_X_GYRO,
+      ::frc971::control_loops::drivetrain::IMUType::IMU_Z,
 
       drivetrain::MakeDrivetrainLoop,
       drivetrain::MakeVelocityDrivetrainLoop,
@@ -53,8 +53,8 @@
       1.2 /* quickturn_wheel_multiplier */,
       1.2 /* wheel_multiplier */,
       true /*pistol_grip_shift_enables_line_follow*/,
-      (Eigen::Matrix<double, 3, 3>() << 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
-       1.0)
+      (Eigen::Matrix<double, 3, 3>() << 0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 1.0, 0.0,
+       0.0)
           .finished() /*imu_transform*/,
   };
 
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
index 3e391c8..6033184 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
@@ -52,13 +52,19 @@
         reader_.event_loop_factory()->MakeEventLoop("drivetrain", roborio_);
     drivetrain_event_loop_->SkipTimingReport();
 
+    frc971::control_loops::drivetrain::DrivetrainConfig<double> config =
+        GetDrivetrainConfig();
+    // Make the modification required to the imu transform to work with the 2016
+    // logs...
+    config.imu_transform << 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0;
+    config.gyro_type = frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO;
+
     localizer_ =
         std::make_unique<frc971::control_loops::drivetrain::DeadReckonEkf>(
-            drivetrain_event_loop_.get(), GetDrivetrainConfig());
+            drivetrain_event_loop_.get(), config);
     drivetrain_ =
         std::make_unique<frc971::control_loops::drivetrain::DrivetrainLoop>(
-            GetDrivetrainConfig(), drivetrain_event_loop_.get(),
-            localizer_.get());
+            config, drivetrain_event_loop_.get(), localizer_.get());
 
     test_event_loop_ =
         reader_.event_loop_factory()->MakeEventLoop("drivetrain", roborio_);
@@ -88,7 +94,7 @@
   ASSERT_TRUE(status_fetcher_->has_x());
   ASSERT_TRUE(status_fetcher_->has_y());
   ASSERT_TRUE(status_fetcher_->has_theta());
-  EXPECT_LT(std::abs(status_fetcher_->x()), 0.1);
+  EXPECT_LT(std::abs(status_fetcher_->x()), 0.25);
   // Because the encoders should not be affecting the y or yaw axes, expect a
   // reasonably precise result (although, since this is a real worl dtest, the
   // robot probably did actually move be some non-zero amount).
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index c438af5..c47faa7 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -92,7 +92,6 @@
 namespace chrono = std::chrono;
 using frc971::control_loops::drivetrain::testing::DrivetrainSimulation;
 using frc971::control_loops::drivetrain::DrivetrainLoop;
-using frc971::control_loops::drivetrain::testing::GetTestDrivetrainConfig;
 using aos::monotonic_clock;
 
 class LocalizedDrivetrainTest : public aos::testing::ControlLoopTest {
@@ -418,6 +417,22 @@
   EXPECT_TRUE(VerifyEstimatorAccurate(5e-4));
 }
 
+// Tests that we can drive in a straight line and have things estimate
+// correctly.
+TEST_F(LocalizedDrivetrainTest, NoCameraUpdateStraightLine) {
+  SetEnabled(true);
+  set_enable_cameras(false);
+  EXPECT_TRUE(VerifyEstimatorAccurate(1e-7));
+
+  SendGoal(1.0, 1.0);
+
+  RunFor(chrono::seconds(1));
+  VerifyNearGoal();
+  // Due to accelerometer drift, the straight-line driving tends to be less
+  // accurate...
+  EXPECT_TRUE(VerifyEstimatorAccurate(0.1));
+}
+
 // Tests that camera updates with a perfect models results in no errors.
 TEST_F(LocalizedDrivetrainTest, PerfectCameraUpdate) {
   SetEnabled(true);
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index 60276c9..b643fcb 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -36,6 +36,9 @@
         "//tools:armhf-debian",
     ],
     visibility = ["//y2020:__subpackages__"],
+    data = [
+        "//y2020:config.json",
+    ],
     deps = [
         ":v4l2_reader",
         ":vision_fbs",
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index 23986bf..d45ec3f 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -14,6 +14,11 @@
 #include "y2020/vision/v4l2_reader.h"
 #include "y2020/vision/vision_generated.h"
 
+// config used to allow running camera_reader independently.  E.g.,
+// bazel run //y2020/vision:camera_reader -- --config y2020/config.json
+//   --override_hostname pi-7971-1  --ignore_timestamps true
+DEFINE_string(config, "config.json", "Path to the config file to use.");
+
 namespace frc971 {
 namespace vision {
 namespace {
@@ -70,9 +75,26 @@
                             const cv::Mat &descriptors,
                             const std::vector<std::vector<cv::DMatch>> &matches,
                             const std::vector<cv::Mat> &camera_target_list,
+                            const std::vector<cv::Mat> &field_camera_list,
                             aos::Sender<sift::ImageMatchResult> *result_sender,
                             bool send_details);
 
+  // Returns the 2D (image) location for the specified training feature.
+  cv::Point2f Training2dPoint(int training_image_index,
+                              int feature_index) const {
+    const float x = training_data_->images()
+                        ->Get(training_image_index)
+                        ->features()
+                        ->Get(feature_index)
+                        ->x();
+    const float y = training_data_->images()
+                        ->Get(training_image_index)
+                        ->features()
+                        ->Get(feature_index)
+                        ->y();
+    return cv::Point2f(x, y);
+  }
+
   // Returns the 3D location for the specified training feature.
   cv::Point3f Training3dPoint(int training_image_index,
                               int feature_index) const {
@@ -170,6 +192,7 @@
     const cv::Mat &descriptors,
     const std::vector<std::vector<cv::DMatch>> &matches,
     const std::vector<cv::Mat> &camera_target_list,
+    const std::vector<cv::Mat> &field_camera_list,
     aos::Sender<sift::ImageMatchResult> *result_sender, bool send_details) {
   auto builder = result_sender->MakeBuilder();
   const auto camera_calibration_offset =
@@ -187,6 +210,7 @@
 
   std::vector<flatbuffers::Offset<sift::CameraPose>> camera_poses;
 
+  CHECK_EQ(camera_target_list.size(), field_camera_list.size());
   for (size_t i = 0; i < camera_target_list.size(); ++i) {
     cv::Mat camera_target = camera_target_list[i];
     CHECK(camera_target.isContinuous());
@@ -194,12 +218,21 @@
         reinterpret_cast<float *>(camera_target.data), camera_target.total());
     const flatbuffers::Offset<sift::TransformationMatrix> transform_offset =
         sift::CreateTransformationMatrix(*builder.fbb(), data_offset);
+
+    cv::Mat field_camera = field_camera_list[i];
+    CHECK(field_camera.isContinuous());
+    const auto fc_data_offset = builder.fbb()->CreateVector<float>(
+        reinterpret_cast<float *>(field_camera.data), field_camera.total());
+    const flatbuffers::Offset<sift::TransformationMatrix> fc_transform_offset =
+        sift::CreateTransformationMatrix(*builder.fbb(), fc_data_offset);
+
     const flatbuffers::Offset<sift::TransformationMatrix>
         field_to_target_offset =
             aos::CopyFlatBuffer(FieldToTarget(i), builder.fbb());
 
     sift::CameraPose::Builder pose_builder(*builder.fbb());
     pose_builder.add_camera_to_target(transform_offset);
+    pose_builder.add_field_to_camera(fc_transform_offset);
     pose_builder.add_field_to_target(field_to_target_offset);
     camera_poses.emplace_back(pose_builder.Finish());
   }
@@ -246,6 +279,8 @@
     std::vector<const std::vector<cv::DMatch> *> matches;
     std::vector<cv::Point3f> training_points_3d;
     std::vector<cv::Point2f> query_points;
+    std::vector<cv::Point2f> training_points;
+    cv::Mat homography;
   };
   std::vector<PerImageMatches> per_image_matches(number_training_images());
 
@@ -268,6 +303,8 @@
     CHECK_LT(training_image, static_cast<int>(per_image_matches.size()));
     PerImageMatches *const per_image = &per_image_matches[training_image];
     per_image->matches.push_back(&match);
+    per_image->training_points.push_back(
+        Training2dPoint(training_image, match[0].trainIdx));
     per_image->training_points_3d.push_back(
         Training3dPoint(training_image, match[0].trainIdx));
 
@@ -278,39 +315,139 @@
   // The minimum number of matches in a training image for us to use it.
   static constexpr int kMinimumMatchCount = 10;
   std::vector<cv::Mat> camera_target_list;
+  std::vector<cv::Mat> field_camera_list;
 
+  std::vector<PerImageMatches> per_image_good_matches;
+  std::vector<std::vector<cv::DMatch>> all_good_matches;
+  // Iterate through matches for each training image
   for (size_t i = 0; i < per_image_matches.size(); ++i) {
     const PerImageMatches &per_image = per_image_matches[i];
+
+    VLOG(2) << "Number of matches to start: " << per_image_matches.size()
+            << "\n";
+    // If we don't have enough matches to start, skip this set of matches
     if (per_image.matches.size() < kMinimumMatchCount) {
       continue;
     }
 
-    cv::Mat R_camera_target_vec, R_camera_target, T_camera_target;
-    // Compute the pose of the camera (global origin relative to camera)
-    cv::solvePnPRansac(per_image.training_points_3d, per_image.query_points,
-                       CameraIntrinsics(), CameraDistCoeffs(),
-                       R_camera_target_vec, T_camera_target);
-    T_camera_target = T_camera_target.t();
-    // Convert Camera from angle-axis (3x1) to homogenous (3x3) representation
-    cv::Rodrigues(R_camera_target_vec, R_camera_target);
+    // Use homography to determine which matches make sense physically
+    cv::Mat mask;
+    cv::Mat homography =
+        cv::findHomography(per_image.training_points, per_image.query_points,
+                           CV_RANSAC, 3.0, mask);
 
+    // If mask doesn't have enough leftover matches, skip these matches
+    if (cv::countNonZero(mask) < kMinimumMatchCount) {
+      continue;
+    }
+
+    VLOG(2) << "Number of matches after homography: " << cv::countNonZero(mask)
+            << "\n";
+    // Fill our match info for each good match
+    // TODO<Jim>: Could probably optimize some of the copies here
+    PerImageMatches per_image_good_match;
+    CHECK_EQ(per_image.training_points.size(),
+             (unsigned long)mask.size().height);
+    for (size_t j = 0; j < per_image.matches.size(); j++) {
+      // Skip if we masked out by homography
+      if (mask.at<uchar>(0, j) != 1) {
+        continue;
+      }
+
+      // Add this to our collection of all matches that passed our criteria
+      all_good_matches.push_back(
+          static_cast<std::vector<cv::DMatch>>(*per_image.matches[j]));
+
+      // Fill out the data for matches per image that made it past
+      // homography check
+      per_image_good_match.matches.push_back(per_image.matches[j]);
+      per_image_good_match.training_points.push_back(
+          per_image.training_points[j]);
+      per_image_good_match.training_points_3d.push_back(
+          per_image.training_points_3d[j]);
+      per_image_good_match.query_points.push_back(per_image.query_points[j]);
+      per_image_good_match.homography = homography;
+    }
+    per_image_good_matches.push_back(per_image_good_match);
+
+    // TODO: Use homography to compute target point on query image
+
+    // Pose transformations (rotations and translations) for various
+    // coordinate frames.  R_X_Y_vec is the Rodrigues (angle-axis)
+    // representation the 3x3 rotation R_X_Y from frame X to frame Y
+
+    // Tranform from camera to target frame
+    cv::Mat R_camera_target_vec, R_camera_target, T_camera_target;
+    // Tranform from camera to field origin (global) reference frame
+    cv::Mat R_camera_field_vec, R_camera_field, T_camera_field;
+    // Inverse of camera to field-- defines location of camera in
+    // global (field) reference frame
+    cv::Mat R_field_camera_vec, R_field_camera, T_field_camera;
+
+    // Compute the pose of the camera (global origin relative to camera)
+    cv::solvePnPRansac(per_image_good_match.training_points_3d,
+                       per_image_good_match.query_points, CameraIntrinsics(),
+                       CameraDistCoeffs(), R_camera_field_vec, T_camera_field);
+    CHECK_EQ(cv::Size(1, 3), T_camera_field.size());
+
+    // Convert to float32's (from float64) to be compatible with the rest
+    R_camera_field_vec.convertTo(R_camera_field_vec, CV_32F);
+    T_camera_field.convertTo(T_camera_field, CV_32F);
+
+    // Get matrix version of R_camera_field
+    cv::Rodrigues(R_camera_field_vec, R_camera_field);
+    CHECK_EQ(cv::Size(3, 3), R_camera_field.size());
+
+    // Compute H_field_camera = H_camera_field^-1
+    R_field_camera = R_camera_field.t();
+    T_field_camera = -R_field_camera * (T_camera_field);
+
+    // Extract the field_target transformation
+    const cv::Mat H_field_target(4, 4, CV_32F,
+                                 const_cast<void *>(static_cast<const void *>(
+                                     FieldToTarget(i)->data()->data())));
+
+    const cv::Mat R_field_target =
+        H_field_target(cv::Range(0, 3), cv::Range(0, 3));
+    const cv::Mat T_field_target =
+        H_field_target(cv::Range(0, 3), cv::Range(3, 4));
+
+    // Use it to get the relative pose from camera to target
+    R_camera_target = R_camera_field * (R_field_target);
+    T_camera_target = R_camera_field * (T_field_target) + T_camera_field;
+
+    // Set H_camera_target
     {
       CHECK_EQ(cv::Size(3, 3), R_camera_target.size());
-      CHECK_EQ(cv::Size(3, 1), T_camera_target.size());
-      cv::Mat camera_target = cv::Mat::zeros(4, 4, CV_32F);
-      R_camera_target.copyTo(camera_target(cv::Range(0, 3), cv::Range(0, 3)));
-      T_camera_target.copyTo(camera_target(cv::Range(3, 4), cv::Range(0, 3)));
-      camera_target.at<float>(3, 3) = 1;
-      CHECK(camera_target.isContinuous());
-      camera_target_list.push_back(camera_target);
+      CHECK_EQ(cv::Size(1, 3), T_camera_target.size());
+      cv::Mat H_camera_target = cv::Mat::zeros(4, 4, CV_32F);
+      R_camera_target.copyTo(H_camera_target(cv::Range(0, 3), cv::Range(0, 3)));
+      T_camera_target.copyTo(H_camera_target(cv::Range(0, 3), cv::Range(3, 4)));
+      H_camera_target.at<float>(3, 3) = 1;
+      CHECK(H_camera_target.isContinuous());
+      camera_target_list.push_back(H_camera_target.clone());
+    }
+
+    // Set H_field_camera
+    {
+      CHECK_EQ(cv::Size(3, 3), R_field_camera.size());
+      CHECK_EQ(cv::Size(1, 3), T_field_camera.size());
+      cv::Mat H_field_camera = cv::Mat::zeros(4, 4, CV_32F);
+      R_field_camera.copyTo(H_field_camera(cv::Range(0, 3), cv::Range(0, 3)));
+      T_field_camera.copyTo(H_field_camera(cv::Range(0, 3), cv::Range(3, 4)));
+      H_field_camera.at<float>(3, 3) = 1;
+      CHECK(H_field_camera.isContinuous());
+      field_camera_list.push_back(H_field_camera.clone());
     }
   }
   // Now, send our two messages-- one large, with details for remote
   // debugging(features), and one smaller
-  SendImageMatchResult(image, keypoints, descriptors, matches,
-                       camera_target_list, &detailed_result_sender_, true);
-  SendImageMatchResult(image, keypoints, descriptors, matches,
-                       camera_target_list, &result_sender_, false);
+  SendImageMatchResult(image, keypoints, descriptors, all_good_matches,
+                       camera_target_list, field_camera_list,
+                       &detailed_result_sender_, true);
+  SendImageMatchResult(image, keypoints, descriptors, all_good_matches,
+                       camera_target_list, field_camera_list, &result_sender_,
+                       false);
 }
 
 void CameraReader::ReadImage() {
@@ -391,7 +528,7 @@
 
 void CameraReaderMain() {
   aos::FlatbufferDetachedBuffer<aos::Configuration> config =
-      aos::configuration::ReadConfig("config.json");
+      aos::configuration::ReadConfig(FLAGS_config);
 
   const auto training_data_bfbs = SiftTrainingData();
   const sift::TrainingData *const training_data =
diff --git a/y2020/vision/sift/sift.fbs b/y2020/vision/sift/sift.fbs
index 8806957..43ea152 100644
--- a/y2020/vision/sift/sift.fbs
+++ b/y2020/vision/sift/sift.fbs
@@ -105,16 +105,21 @@
 // Contains the information the EKF wants from an image matched against a single
 // training image.
 //
-// This is represented as a transformation to a target in field coordinates.
+// This is represented as a transformation from the camera to the target
+// (camera_to_target) and a transformatoin from the field to the target
+// (field_to_target).
+//
+// We also send the map from the field to the camera, which can be computed
+// with the first two, to make it easier to display.
 table CameraPose {
-  // Transformation matrix from the target to the camera's origin.
+  // Transformation matrix from the camera to the target.
   // (0, 0, 0) is the aperture of the camera (we pretend it's an ideal pinhole
   // camera). Positive Z is out of the camera. Positive X and Y are right
   // handed, but which way they face depends on the camera extrinsics.
   camera_to_target:TransformationMatrix;
 
   // Field coordinates of the target, represented as a transformation matrix
-  // from the target to the field.
+  // from the field to the target.
   // (0, 0, 0) is the center of the field, on the level of most of the field
   // (not the region under the truss). Positive X is towards the red alliance's
   // PLAYER STATION. Positive Z is up. The coordinate system is right-handed.
@@ -125,6 +130,9 @@
   // The value here will be selected from a small, static set of targets we
   // train images on.
   field_to_target:TransformationMatrix;
+
+  // The pose of the camera in the field coordinate frame
+  field_to_camera:TransformationMatrix;
 }
 
 table ImageMatchResult {
diff --git a/y2020/vision/v4l2_reader.cc b/y2020/vision/v4l2_reader.cc
index f1944c1..91777c7 100644
--- a/y2020/vision/v4l2_reader.cc
+++ b/y2020/vision/v4l2_reader.cc
@@ -6,6 +6,9 @@
 #include <sys/stat.h>
 #include <sys/types.h>
 
+DEFINE_bool(ignore_timestamps, false,
+            "Don't require timestamps on images.  Used to allow webcams");
+
 namespace frc971 {
 namespace vision {
 
@@ -137,8 +140,11 @@
            buffer.m.userptr);
   CHECK_EQ(ImageSize(), buffer.length);
   CHECK(buffer.flags & V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC);
-  CHECK_EQ(buffer.flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK,
-           static_cast<uint32_t>(V4L2_BUF_FLAG_TSTAMP_SRC_EOF));
+  if (!FLAGS_ignore_timestamps) {
+    // Require that we have good timestamp on images
+    CHECK_EQ(buffer.flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK,
+             static_cast<uint32_t>(V4L2_BUF_FLAG_TSTAMP_SRC_EOF));
+  }
   return {static_cast<int>(buffer.index),
           aos::time::from_timeval(buffer.timestamp)};
 }
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index 8c30918..4f39acf 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -218,8 +218,7 @@
 
     {
       auto builder = superstructure_position_sender_.MakeBuilder();
-      superstructure::Position::Builder position_builder =
-          builder.MakeBuilder<superstructure::Position>();
+
       // TODO(alex): check new absolute encoder api.
       // Hood
       frc971::AbsolutePositionT hood;
@@ -246,6 +245,14 @@
       flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
           frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &turret);
 
+      // Control Panel
+      frc971::RelativePositionT control_panel;
+      CopyPosition(*control_panel_encoder_, &control_panel,
+                   Values::kControlPanelEncoderCountsPerRevolution(),
+                   Values::kControlPanelEncoderRatio(), false);
+      flatbuffers::Offset<frc971::RelativePosition> control_panel_offset =
+          frc971::RelativePosition::Pack(*builder.fbb(), &control_panel);
+
       // Shooter
       y2020::control_loops::superstructure::ShooterPositionT shooter;
       shooter.theta_finisher =
@@ -266,14 +273,8 @@
               y2020::control_loops::superstructure::ShooterPosition::Pack(
                   *builder.fbb(), &shooter);
 
-      // Control Panel
-      frc971::RelativePositionT control_panel;
-      CopyPosition(*control_panel_encoder_, &control_panel,
-                   Values::kControlPanelEncoderCountsPerRevolution(),
-                   Values::kControlPanelEncoderRatio(), false);
-      flatbuffers::Offset<frc971::RelativePosition> control_panel_offset =
-          frc971::RelativePosition::Pack(*builder.fbb(), &control_panel);
-
+      superstructure::Position::Builder position_builder =
+          builder.MakeBuilder<superstructure::Position>();
       position_builder.add_hood(hood_offset);
       position_builder.add_intake_joint(intake_joint_offset);
       position_builder.add_turret(turret_offset);
@@ -481,6 +482,7 @@
     // Thread 3.
     ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
     SensorReader sensor_reader(&sensor_reader_event_loop);
+    sensor_reader.set_pwm_trigger(true);
     sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
     sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
     // TODO: pin numbers