diff --git a/y2020/vision/extrinsics_calibration.cc b/y2020/vision/extrinsics_calibration.cc
index e775e3e..be64efb 100644
--- a/y2020/vision/extrinsics_calibration.cc
+++ b/y2020/vision/extrinsics_calibration.cc
@@ -6,10 +6,7 @@
 #include "Eigen/Dense"
 #include "Eigen/Geometry"
 
-#include "frc971/wpilib/imu_batch_generated.h"
 #include "absl/strings/str_format.h"
-#include "frc971/control_loops/quaternion_utils.h"
-#include "y2020/vision/charuco_lib.h"
 #include "aos/events/logging/log_reader.h"
 #include "aos/events/shm_event_loop.h"
 #include "aos/init.h"
@@ -17,16 +14,17 @@
 #include "aos/time/time.h"
 #include "aos/util/file.h"
 #include "frc971/control_loops/drivetrain/improved_down_estimator.h"
+#include "frc971/control_loops/quaternion_utils.h"
+#include "frc971/wpilib/imu_batch_generated.h"
+#include "y2020/vision/calibration_accumulator.h"
+#include "y2020/vision/charuco_lib.h"
 #include "y2020/vision/sift/sift_generated.h"
 #include "y2020/vision/sift/sift_training_generated.h"
 #include "y2020/vision/tools/python_code/sift_training_data.h"
 #include "y2020/vision/vision_generated.h"
-#include "y2020/vision/charuco_lib.h"
 
 DEFINE_string(config, "config.json", "Path to the config file to use.");
 DEFINE_string(pi, "pi-7971-2", "Pi name to calibrate.");
-DEFINE_bool(display_undistorted, false,
-            "If true, display the undistorted image.");
 
 namespace frc971 {
 namespace vision {
@@ -34,74 +32,37 @@
 using aos::distributed_clock;
 using aos::monotonic_clock;
 
-// Class to both accumulate and replay camera and IMU data in time order.
-class CalibrationData {
+class PoseFilter : public CalibrationDataObserver {
  public:
-  CalibrationData()
-      : x_hat_(Eigen::Matrix<double, 9, 1>::Zero()),
+  PoseFilter()
+      : accel_(Eigen::Matrix<double, 3, 1>::Zero()),
+        omega_(Eigen::Matrix<double, 3, 1>::Zero()),
+        x_hat_(Eigen::Matrix<double, 9, 1>::Zero()),
         q_(Eigen::Matrix<double, 9, 9>::Zero()) {}
 
-  // Adds an IMU point to the list at the provided time.
-  void AddImu(distributed_clock::time_point distributed_now,
-              Eigen::Vector3d gyro, Eigen::Vector3d accel) {
-    imu_points_.emplace_back(distributed_now, std::make_pair(gyro, accel));
-  }
-
-  // Adds a camera/charuco detection to the list at the provided time.
-  void AddCharuco(distributed_clock::time_point distributed_now,
-                  Eigen::Vector3d rvec, Eigen::Vector3d tvec) {
-    rot_trans_points_.emplace_back(distributed_now, std::make_pair(rvec, tvec));
-  }
-
-  // Processes the data points by calling UpdateCamera and UpdateIMU in time
-  // order.
-  void ReviewData() {
-    size_t next_imu_point = 0;
-    size_t next_camera_point = 0;
-    while (true) {
-      if (next_imu_point != imu_points_.size()) {
-        // There aren't that many combinations, so just brute force them all
-        // rather than being too clever.
-        if (next_camera_point != rot_trans_points_.size()) {
-          if (imu_points_[next_imu_point].first >
-              rot_trans_points_[next_camera_point].first) {
-            // Camera!
-            UpdateCamera(rot_trans_points_[next_camera_point].first,
-                         rot_trans_points_[next_camera_point].second);
-            ++next_camera_point;
-          } else {
-            // IMU!
-            UpdateIMU(imu_points_[next_imu_point].first,
-                      imu_points_[next_imu_point].second);
-            ++next_imu_point;
-          }
-        } else {
-          if (next_camera_point != rot_trans_points_.size()) {
-            // Camera!
-            UpdateCamera(rot_trans_points_[next_camera_point].first,
-                         rot_trans_points_[next_camera_point].second);
-            ++next_camera_point;
-          } else {
-            // Nothing left for either list of points, so we are done.
-            break;
-          }
-        }
-      }
-    }
-  }
-
   void UpdateCamera(distributed_clock::time_point t,
-                    std::pair<Eigen::Vector3d, Eigen::Vector3d> rt) {
+                    std::pair<Eigen::Vector3d, Eigen::Vector3d> rt) override {
+    Integrate(t);
+    // TODO(austin): take the sample.
     LOG(INFO) << t << " Camera " << rt.second.transpose();
   }
 
   void UpdateIMU(distributed_clock::time_point t,
-                 std::pair<Eigen::Vector3d, Eigen::Vector3d> wa) {
+                 std::pair<Eigen::Vector3d, Eigen::Vector3d> wa) override {
+    Integrate(t);
+    omega_ = wa.first;
+    accel_ = wa.second;
     LOG(INFO) << t << " IMU " << wa.first.transpose();
   }
 
  private:
-  // TODO(austin): Actually use these.  Or make a new "callback" object which has these.
+  void Integrate(distributed_clock::time_point t) { LOG(INFO) << t; }
+
+  Eigen::Matrix<double, 3, 1> accel_;
+  Eigen::Matrix<double, 3, 1> omega_;
+
+  // TODO(austin): Actually use these.  Or make a new "callback" object which
+  // has these.
   Eigen::Matrix<double, 9, 1> x_hat_;
   Eigen::Matrix<double, 9, 9> q_;
 
@@ -118,160 +79,6 @@
   // Measurement:
   //   xyz position
   //   orientation rotation vector
-
-  std::vector<std::pair<distributed_clock::time_point,
-                        std::pair<Eigen::Vector3d, Eigen::Vector3d>>>
-      imu_points_;
-
-  // Store pose samples as timestamp, along with
-  // pair of rotation, translation vectors
-  std::vector<std::pair<distributed_clock::time_point,
-                        std::pair<Eigen::Vector3d, Eigen::Vector3d>>>
-      rot_trans_points_;
-};
-
-// Class to register image and IMU callbacks in AOS and route them to the
-// corresponding CalibrationData class.
-class Calibration {
- public:
-  Calibration(aos::SimulatedEventLoopFactory *event_loop_factory,
-              aos::EventLoop *image_event_loop, aos::EventLoop *imu_event_loop,
-              std::string_view pi, CalibrationData *data)
-      : image_event_loop_(image_event_loop),
-        image_factory_(event_loop_factory->GetNodeEventLoopFactory(
-            image_event_loop_->node())),
-        imu_event_loop_(imu_event_loop),
-        imu_factory_(event_loop_factory->GetNodeEventLoopFactory(
-            imu_event_loop_->node())),
-        charuco_extractor_(
-            image_event_loop_, pi,
-            [this](cv::Mat rgb_image, monotonic_clock::time_point eof,
-                   std::vector<int> charuco_ids,
-                   std::vector<cv::Point2f> charuco_corners, bool valid,
-                   Eigen::Vector3d rvec_eigen, Eigen::Vector3d tvec_eigen) {
-              HandleCharuco(rgb_image, eof, charuco_ids, charuco_corners, valid,
-                            rvec_eigen, tvec_eigen);
-            }),
-        data_(data) {
-    imu_event_loop_->MakeWatcher(
-        "/drivetrain", [this](const frc971::IMUValuesBatch &imu) {
-          if (!imu.has_readings()) {
-            return;
-          }
-          for (const frc971::IMUValues *value : *imu.readings()) {
-            HandleIMU(value);
-          }
-        });
-  }
-
-  // Processes a charuco detection.
-  void HandleCharuco(cv::Mat rgb_image, const monotonic_clock::time_point eof,
-                     std::vector<int> /*charuco_ids*/,
-                     std::vector<cv::Point2f> /*charuco_corners*/, bool valid,
-                     Eigen::Vector3d rvec_eigen, Eigen::Vector3d tvec_eigen) {
-    if (valid) {
-      Eigen::Quaternion<double> rotation(
-          frc971::controls::ToQuaternionFromRotationVector(rvec_eigen));
-      Eigen::Translation3d translation(tvec_eigen);
-
-      const Eigen::Affine3d board_to_camera = translation * rotation;
-      (void)board_to_camera;
-
-      // TODO(austin): Need a gravity vector input.
-      //
-      // TODO(austin): Need a state, covariance, and model.
-      //
-      // TODO(austin): Need to record all the values out of a log and run it
-      // as a batch run so we can feed it into ceres.
-
-      // LOG(INFO) << "rotation " << rotation.matrix();
-      // LOG(INFO) << "translation " << translation.matrix();
-      // Z -> up
-      // Y -> away from cameras 2 and 3
-      // X -> left
-      Eigen::Vector3d imu(last_value_.accelerometer_x,
-                          last_value_.accelerometer_y,
-                          last_value_.accelerometer_z);
-
-      // For cameras 2 and 3...
-      Eigen::Quaternion<double> imu_to_camera(
-          Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitX()));
-
-      Eigen::Quaternion<double> board_to_world(
-          Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitX()));
-
-      Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ",\n", "[", "]",
-                               "[", "]");
-
-      LOG(INFO);
-      LOG(INFO) << "World Gravity "
-                << (board_to_world * rotation.inverse() * imu_to_camera * imu)
-                       .transpose()
-                       .format(HeavyFmt);
-      LOG(INFO) << "Board Gravity "
-                << (rotation.inverse() * imu_to_camera * imu)
-                       .transpose()
-                       .format(HeavyFmt);
-      LOG(INFO) << "Camera Gravity "
-                << (imu_to_camera * imu).transpose().format(HeavyFmt);
-      LOG(INFO) << "IMU Gravity " << imu.transpose().format(HeavyFmt);
-
-      const double age_double =
-          std::chrono::duration_cast<std::chrono::duration<double>>(
-              image_event_loop_->monotonic_now() - eof)
-              .count();
-      LOG(INFO) << std::fixed << std::setprecision(6) << "Age: " << age_double
-                << ", Pose is R:" << rvec_eigen.transpose().format(HeavyFmt)
-                << " T:" << tvec_eigen.transpose().format(HeavyFmt);
-
-      data_->AddCharuco(image_factory_->ToDistributedClock(eof), rvec_eigen,
-                        tvec_eigen);
-    }
-
-    cv::imshow("Display", rgb_image);
-
-    if (FLAGS_display_undistorted) {
-      const cv::Size image_size(rgb_image.cols, rgb_image.rows);
-      cv::Mat undistorted_rgb_image(image_size, CV_8UC3);
-      cv::undistort(rgb_image, undistorted_rgb_image,
-                    charuco_extractor_.camera_matrix(),
-                    charuco_extractor_.dist_coeffs());
-
-      cv::imshow("Display undist", undistorted_rgb_image);
-    }
-
-    int keystroke = cv::waitKey(1);
-    if ((keystroke & 0xFF) == static_cast<int>('q')) {
-      // image_event_loop_->Exit();
-    }
-  }
-
-  // Processes an IMU reading.
-  void HandleIMU(const frc971::IMUValues *imu) {
-    VLOG(1) << "IMU " << imu;
-    imu->UnPackTo(&last_value_);
-    Eigen::Vector3d gyro(last_value_.gyro_x, last_value_.gyro_y,
-                         last_value_.gyro_z);
-    Eigen::Vector3d accel(last_value_.accelerometer_x,
-                          last_value_.accelerometer_y,
-                          last_value_.accelerometer_z);
-
-    data_->AddImu(imu_factory_->ToDistributedClock(monotonic_clock::time_point(
-                      chrono::nanoseconds(imu->monotonic_timestamp_ns()))),
-                  gyro, accel);
-  }
-
-  frc971::IMUValuesT last_value_;
-
- private:
-  aos::EventLoop *image_event_loop_;
-  aos::NodeEventLoopFactory *image_factory_;
-  aos::EventLoop *imu_event_loop_;
-  aos::NodeEventLoopFactory *imu_factory_;
-
-  CharucoExtractor charuco_extractor_;
-
-  CalibrationData *data_;
 };
 
 void Main(int argc, char **argv) {
@@ -316,7 +123,11 @@
 
   LOG(INFO) << "Done with event_loop running";
   // And now we have it, we can start processing it.
-  data.ReviewData();
+
+  {
+    PoseFilter filter;
+    data.ReviewData(&filter);
+  }
 }
 
 }  // namespace vision