diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 5350240..2c94473 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -98,6 +98,25 @@
 )
 
 aos_config(
+    name = "simulation_channels",
+    src = "drivetrain_simulation_channels.json",
+    flatbuffers = [
+        ":drivetrain_status_fbs",
+    ],
+    visibility = ["//visibility:public"],
+)
+
+aos_config(
+    name = "simulation_config",
+    src = "drivetrain_simulation_config.json",
+    visibility = ["//visibility:public"],
+    deps = [
+        ":config",
+        ":simulation_channels",
+    ],
+)
+
+aos_config(
     name = "config",
     src = "drivetrain_config.json",
     flatbuffers = [
@@ -395,6 +414,13 @@
     testonly = True,
     srcs = ["drivetrain_test_lib.cc"],
     hdrs = ["drivetrain_test_lib.h"],
+    defines =
+        cpu_select({
+            "amd64": [
+                "SUPPORT_PLOT=1",
+            ],
+            "arm": [],
+        }),
     deps = [
         ":drivetrain_config",
         ":drivetrain_goal_fbs",
@@ -410,7 +436,12 @@
         "//frc971/wpilib:imu_fbs",
         "//y2016:constants",
         "//y2016/control_loops/drivetrain:polydrivetrain_plants",
-    ],
+    ] + cpu_select({
+        "amd64": [
+            "//third_party/matplotlib-cpp",
+        ],
+        "arm": [],
+    }),
 )
 
 cc_test(
@@ -418,7 +449,7 @@
     srcs = [
         "drivetrain_lib_test.cc",
     ],
-    data = ["config.json"],
+    data = ["simulation_config.json"],
     defines =
         cpu_select({
             "amd64": [
@@ -437,6 +468,7 @@
         ":drivetrain_output_fbs",
         ":drivetrain_test_lib",
         "//aos/controls:control_loop_test",
+        "//aos/events/logging:logger",
         "//aos/testing:googletest",
         "//frc971/queues:gyro_fbs",
         "//frc971/wpilib:imu_fbs",
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index e29fed9..f606495 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -6,6 +6,7 @@
 #include "aos/controls/control_loop_test.h"
 #include "aos/controls/polytope.h"
 #include "aos/events/event_loop.h"
+#include "aos/events/logging/logger.h"
 #include "aos/time/time.h"
 #include "gflags/gflags.h"
 #include "gtest/gtest.h"
@@ -21,6 +22,9 @@
 #include "frc971/control_loops/drivetrain/localizer_generated.h"
 #include "frc971/queues/gyro_generated.h"
 
+DEFINE_string(output_file, "",
+              "If set, logs all channels to the provided logfile.");
+
 namespace frc971 {
 namespace control_loops {
 namespace drivetrain {
@@ -34,7 +38,7 @@
   DrivetrainTest()
       : ::aos::testing::ControlLoopTest(
             aos::configuration::ReadConfig(
-                "frc971/control_loops/drivetrain/config.json"),
+                "frc971/control_loops/drivetrain/simulation_config.json"),
             GetTestDrivetrainConfig().dt),
         test_event_loop_(MakeEventLoop("test")),
         drivetrain_goal_sender_(
@@ -56,6 +60,18 @@
     // Too many tests care...
     set_send_delay(chrono::nanoseconds(0));
     set_battery_voltage(12.0);
+
+    if (!FLAGS_output_file.empty()) {
+      unlink(FLAGS_output_file.c_str());
+      log_buffer_writer_ = std::make_unique<aos::logger::DetachedBufferWriter>(
+          FLAGS_output_file);
+      logger_event_loop_ = MakeEventLoop("logger");
+      logger_ = std::make_unique<aos::logger::Logger>(log_buffer_writer_.get(),
+                                                      logger_event_loop_.get());
+    }
+
+    // Run for enough time to allow the gyro/imu zeroing code to run.
+    RunFor(std::chrono::seconds(10));
   }
   virtual ~DrivetrainTest() {}
 
@@ -148,12 +164,14 @@
 
   ::std::unique_ptr<::aos::EventLoop> drivetrain_plant_event_loop_;
   DrivetrainSimulation drivetrain_plant_;
+
+  std::unique_ptr<aos::EventLoop> logger_event_loop_;
+  std::unique_ptr<aos::logger::DetachedBufferWriter> log_buffer_writer_;
+  std::unique_ptr<aos::logger::Logger> logger_;
 };
 
 // Tests that the drivetrain converges on a goal.
 TEST_F(DrivetrainTest, ConvergesCorrectly) {
-  // Run for enough time to let the gyro zero.
-  RunFor(std::chrono::seconds(100));
   SetEnabled(true);
   {
     auto builder = drivetrain_goal_sender_.MakeBuilder();
@@ -312,7 +330,8 @@
     EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
-  while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(6))) {
+  const auto start_time = monotonic_now();
+  while (monotonic_now() < start_time + chrono::seconds(6)) {
     RunFor(dt());
     ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
     EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage(),
@@ -353,7 +372,8 @@
     EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
-  while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(6))) {
+  const auto start_time = monotonic_now();
+  while (monotonic_now() < start_time + chrono::seconds(6)) {
     RunFor(dt());
     ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
     EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage(),
@@ -394,7 +414,8 @@
     EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
-  while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(3))) {
+  const auto start_time = monotonic_now();
+  while (monotonic_now() < start_time + chrono::seconds(3)) {
     RunFor(dt());
     ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
   }
diff --git a/frc971/control_loops/drivetrain/drivetrain_simulation_channels.json b/frc971/control_loops/drivetrain/drivetrain_simulation_channels.json
new file mode 100644
index 0000000..b0fc03b
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_simulation_channels.json
@@ -0,0 +1,12 @@
+{
+  "channels":
+  [
+    {
+      /* Channel providing the true state of the drivetrain in simulation. */
+      "name": "/drivetrain/truth",
+      "type": "frc971.control_loops.drivetrain.Status",
+      "frequency": 200,
+      "max_size": 2000
+    }
+  ]
+}
diff --git a/frc971/control_loops/drivetrain/drivetrain_simulation_config.json b/frc971/control_loops/drivetrain/drivetrain_simulation_config.json
new file mode 100644
index 0000000..aa67f2e
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_simulation_config.json
@@ -0,0 +1,6 @@
+{
+  "imports": [
+    "drivetrain_config.json",
+    "drivetrain_simulation_channels.json"
+  ]
+}
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 56e664b..8b4aab1 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -95,14 +95,16 @@
           event_loop_
               ->MakeSender<::frc971::control_loops::drivetrain::Position>(
                   "/drivetrain")),
+      drivetrain_truth_sender_(
+          event_loop_->MakeSender<::frc971::control_loops::drivetrain::Status>(
+              "/drivetrain/truth")),
       drivetrain_output_fetcher_(
           event_loop_->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
               "/drivetrain")),
       drivetrain_status_fetcher_(
           event_loop_->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
               "/drivetrain")),
-      imu_sender_(
-          event_loop->MakeSender<::frc971::IMUValues>("/drivetrain")),
+      imu_sender_(event_loop->MakeSender<::frc971::IMUValues>("/drivetrain")),
       dt_config_(dt_config),
       drivetrain_plant_(MakePlantFromConfig(dt_config_)),
       velocity_drivetrain_(
@@ -136,8 +138,12 @@
         }
         first_ = false;
         SendPositionMessage();
+        SendTruthMessage();
       },
       dt_config_.dt);
+
+  event_loop_->AddPhasedLoop([this](int) { SendImuMessage(); },
+                             std::chrono::microseconds(500));
 }
 
 void DrivetrainSimulation::Reinitialize() {
@@ -150,6 +156,16 @@
   last_right_position_ = drivetrain_plant_.Y(1, 0);
 }
 
+void DrivetrainSimulation::SendTruthMessage() {
+  auto builder = drivetrain_truth_sender_.MakeBuilder();
+  auto status_builder =
+      builder.MakeBuilder<frc971::control_loops::drivetrain::Status>();
+  status_builder.add_x(state_.x());
+  status_builder.add_y(state_.y());
+  status_builder.add_theta(state_(2));
+  builder.Send(status_builder.Finish());
+}
+
 void DrivetrainSimulation::SendPositionMessage() {
   const double left_encoder = GetLeftPosition();
   const double right_encoder = GetRightPosition();
@@ -165,27 +181,27 @@
     position_builder.add_right_shifter_position(right_gear_high_ ? 1.0 : 0.0);
     builder.Send(position_builder.Finish());
   }
+}
 
-  {
-    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));
-    // 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(0.0);
-    imu_builder.add_accelerometer_z(1.0);
-    imu_builder.add_monotonic_timestamp_ns(
-        std::chrono::duration_cast<std::chrono::nanoseconds>(
-            event_loop_->monotonic_now().time_since_epoch())
-            .count());
-    builder.Send(imu_builder.Finish());
-  }
+void DrivetrainSimulation::SendImuMessage() {
+  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));
+  // 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);
+  imu_builder.add_monotonic_timestamp_ns(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(
+          event_loop_->monotonic_now().time_since_epoch())
+          .count());
+  builder.Send(imu_builder.Finish());
 }
 
 // Simulates the drivetrain moving for one timestep.
@@ -230,12 +246,23 @@
         return ContinuousDynamics(velocity_drivetrain_->plant(),
                                   dt_config_.Tlr_to_la(), X, U);
       };
+  const Eigen::Matrix<double, 5, 1> last_state = state_;
   state_ = RungeKuttaU(dynamics, state_, U, dt_float);
-  const Eigen::Matrix<double, 5, 1> Xdot = dynamics(state_, U);
-  // TODO(james): Account for centripetal accelerations.
+  // Calculate Xdot from the actual state change rather than getting Xdot at the
+  // current state_.
+  // TODO(james): This seemed to help make the simulation perform better, but
+  // I'm not sure that it is actually helping. Regardless, we should be
+  // calculating Xdot at all the intermediate states at the 2 kHz that
+  // the IMU sends at, rather than doing a sample-and-hold like we do now.
+  const Eigen::Matrix<double, 5, 1> Xdot = (state_ - last_state) / dt_float;
+
+  const double yaw_rate = Xdot(2);
+  const double longitudinal_velocity =
+      (state_(4) + state_(3)) / 2.0;
+  const double centripetal_accel = yaw_rate * longitudinal_velocity;
   // TODO(james): Allow inputting arbitrary calibrations, e.g., for testing
   // situations where the IMU is not perfectly flat in the CG of the robot.
-  last_acceleration_ << (Xdot(3, 0) + Xdot(4, 0)) / 2.0, 0.0, 0.0;
+  last_acceleration_ << (Xdot(3, 0) + Xdot(4, 0)) / 2.0, centripetal_accel, 0.0;
 }
 
 void DrivetrainSimulation::MaybePlot() {
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index a906ae2..e8b513c 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -76,6 +76,10 @@
  private:
   // Sends out the position queue messages.
   void SendPositionMessage();
+  // Sends out the IMU messages.
+  void SendImuMessage();
+  // Sends out the "truth" status message.
+  void SendTruthMessage();
 
   // Simulates the drivetrain moving for one timestep.
   void Simulate();
@@ -85,6 +89,8 @@
 
   ::aos::Sender<::frc971::control_loops::drivetrain::Position>
       drivetrain_position_sender_;
+  ::aos::Sender<::frc971::control_loops::drivetrain::Status>
+      drivetrain_truth_sender_;
   ::aos::Fetcher<::frc971::control_loops::drivetrain::Output>
       drivetrain_output_fetcher_;
   ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
@@ -107,7 +113,7 @@
   Eigen::Matrix<double, 2, 1> last_U_;
 
   // Last robot acceleration, in m/s/s.
-  Eigen::Vector3d last_acceleration_;
+  Eigen::Vector3d last_acceleration_{0, 0, 1};
 
   bool left_gear_high_ = false;
   bool right_gear_high_ = false;