diff --git a/aos/network/team_number.cc b/aos/network/team_number.cc
index 175c3a9..bd90b96 100644
--- a/aos/network/team_number.cc
+++ b/aos/network/team_number.cc
@@ -130,6 +130,8 @@
     return std::string_view("pi");
   } else if (hostname.substr(0, 5) == "orin-") {
     return std::string_view("orin");
+  } else if (hostname.substr(0, 4) == "imu-") {
+    return std::string_view("orin");
   } else
     return std::nullopt;
 }
diff --git a/aos/network/team_number_test.cc b/aos/network/team_number_test.cc
index 9922b1b..278a364 100644
--- a/aos/network/team_number_test.cc
+++ b/aos/network/team_number_test.cc
@@ -28,7 +28,7 @@
   EXPECT_FALSE(ParseRoborioTeamNumber("roboRIO--FRC"));
 }
 
-TEST(TeamNumberTest, ParsePiOrOrinTeamNumber) {
+TEST(HostnameParseTest, ParsePiOrOrinTeamNumber) {
   EXPECT_EQ(971u, *ParsePiOrOrinTeamNumber("pi-971-1"));
   EXPECT_EQ(8971u, *ParsePiOrOrinTeamNumber("pi-8971-22"));
   EXPECT_EQ(8971u, *ParsePiOrOrinTeamNumber("pi-8971-"));
@@ -37,12 +37,16 @@
   EXPECT_EQ(8971u, *ParsePiOrOrinTeamNumber("orin-8971-22"));
   EXPECT_EQ(8971u, *ParsePiOrOrinTeamNumber("orin-8971-"));
 
+  EXPECT_FALSE(ParsePiOrOrinTeamNumber("roboRIO-971-FRC"));
+
   EXPECT_FALSE(ParseRoborioTeamNumber("pi"));
   EXPECT_FALSE(ParseRoborioTeamNumber("pi-"));
   EXPECT_FALSE(ParseRoborioTeamNumber("pi-971"));
   EXPECT_FALSE(ParseRoborioTeamNumber("pi-971a-1"));
   EXPECT_FALSE(ParseRoborioTeamNumber("orin-971-1"));
+}
 
+TEST(HostnameParseTest, ParsePiOrOrinNumber) {
   EXPECT_EQ(1u, *ParsePiOrOrinNumber("pi-971-1"));
   EXPECT_EQ(22u, *ParsePiOrOrinNumber("pi-8971-22"));
   EXPECT_EQ(1u, *ParsePiOrOrinNumber("orin-971-1"));
@@ -59,4 +63,19 @@
   EXPECT_FALSE(ParsePiOrOrinNumber("orin-971"));
 }
 
+TEST(HostnameParseTest, ParsePiOrOrin) {
+  EXPECT_EQ("pi", *ParsePiOrOrin("pi-971-1"));
+  EXPECT_EQ("pi", *ParsePiOrOrin("pi-8971-22"));
+  EXPECT_EQ("pi", *ParsePiOrOrin("pi-8971-"));
+
+  EXPECT_EQ("orin", *ParsePiOrOrin("orin-971-1"));
+  EXPECT_EQ("orin", *ParsePiOrOrin("orin-8971-22"));
+  EXPECT_EQ("orin", *ParsePiOrOrin("orin-8971-"));
+
+  EXPECT_EQ("orin", *ParsePiOrOrin("imu-971-1"));
+
+  EXPECT_FALSE(ParsePiOrOrin("roboRIO-971-FRC"));
+  EXPECT_FALSE(ParsePiOrOrin("laptop"));
+}
+
 }  // namespace aos::network::testing
diff --git a/frc971/control_loops/double_jointed_arm/trajectory.cc b/frc971/control_loops/double_jointed_arm/trajectory.cc
index 0eaf67e..a04d6cc 100644
--- a/frc971/control_loops/double_jointed_arm/trajectory.cc
+++ b/frc971/control_loops/double_jointed_arm/trajectory.cc
@@ -391,14 +391,14 @@
   const auto x_blocked = X.block<4, 1>(0, 0);
 
   const ::Eigen::Matrix<double, 4, 4> final_A =
-      ::frc971::control_loops::NumericalJacobianX<4, 2>(
+      ::frc971::control_loops::NumericalJacobianX<4, 2, double>(
           [this](const auto &x_blocked, const auto &U, double dt) {
             return this->dynamics_->UnboundedDiscreteDynamics(x_blocked, U, dt);
           },
           x_blocked, U, 0.00505);
 
   const ::Eigen::Matrix<double, 4, 2> final_B =
-      ::frc971::control_loops::NumericalJacobianU<4, 2>(
+      ::frc971::control_loops::NumericalJacobianU<4, 2, double>(
           [this](const auto &x_blocked, const auto &U, double dt) {
             return this->dynamics_->UnboundedDiscreteDynamics(x_blocked, U, dt);
           },
diff --git a/frc971/control_loops/jacobian.h b/frc971/control_loops/jacobian.h
index 5442ec8..e82c668 100644
--- a/frc971/control_loops/jacobian.h
+++ b/frc971/control_loops/jacobian.h
@@ -5,18 +5,18 @@
 
 namespace frc971::control_loops {
 
-template <int num_states, int num_inputs, typename F>
-::Eigen::Matrix<double, num_states, num_inputs> NumericalJacobian(
-    const F &fn, ::Eigen::Matrix<double, num_inputs, 1> input) {
-  constexpr double kEpsilon = 1e-5;
-  ::Eigen::Matrix<double, num_states, num_inputs> result =
-      ::Eigen::Matrix<double, num_states, num_inputs>::Zero();
+template <int num_states, int num_inputs, typename Scalar, typename F>
+::Eigen::Matrix<Scalar, num_states, num_inputs> NumericalJacobian(
+    const F &fn, ::Eigen::Matrix<Scalar, num_inputs, 1> input) {
+  constexpr Scalar kEpsilon = 1e-5;
+  ::Eigen::Matrix<Scalar, num_states, num_inputs> result =
+      ::Eigen::Matrix<Scalar, num_states, num_inputs>::Zero();
 
   // It's more expensive, but +- epsilon will be more accurate
   for (int i = 0; i < num_inputs; ++i) {
-    ::Eigen::Matrix<double, num_inputs, 1> dX_plus = input;
+    ::Eigen::Matrix<Scalar, num_inputs, 1> dX_plus = input;
     dX_plus(i, 0) += kEpsilon;
-    ::Eigen::Matrix<double, num_inputs, 1> dX_minus = input;
+    ::Eigen::Matrix<Scalar, num_inputs, 1> dX_minus = input;
     dX_minus(i, 0) -= kEpsilon;
     result.col(i) = (fn(dX_plus) - fn(dX_minus)) / (kEpsilon * 2.0);
   }
@@ -24,24 +24,26 @@
 }
 
 // Implements a numerical jacobian with respect to X for f(X, U, ...).
-template <int num_states, int num_u, typename F, typename... Args>
-::Eigen::Matrix<double, num_states, num_states> NumericalJacobianX(
-    const F &fn, ::Eigen::Matrix<double, num_states, 1> X,
-    ::Eigen::Matrix<double, num_u, 1> U, Args &&...args) {
+template <int num_states, int num_u, typename Scalar, typename F,
+          typename... Args>
+::Eigen::Matrix<Scalar, num_states, num_states> NumericalJacobianX(
+    const F &fn, ::Eigen::Matrix<Scalar, num_states, 1> X,
+    ::Eigen::Matrix<Scalar, num_u, 1> U, Args &&...args) {
   return NumericalJacobian<num_states, num_states>(
-      [&](::Eigen::Matrix<double, num_states, 1> X) {
+      [&](::Eigen::Matrix<Scalar, num_states, 1> X) {
         return fn(X, U, args...);
       },
       X);
 }
 
 // Implements a numerical jacobian with respect to U for f(X, U, ...).
-template <int num_states, int num_u, typename F, typename... Args>
-::Eigen::Matrix<double, num_states, num_u> NumericalJacobianU(
-    const F &fn, ::Eigen::Matrix<double, num_states, 1> X,
-    ::Eigen::Matrix<double, num_u, 1> U, Args &&...args) {
+template <int num_states, int num_u, typename Scalar, typename F,
+          typename... Args>
+::Eigen::Matrix<Scalar, num_states, num_u> NumericalJacobianU(
+    const F &fn, ::Eigen::Matrix<Scalar, num_states, 1> X,
+    ::Eigen::Matrix<Scalar, num_u, 1> U, Args &&...args) {
   return NumericalJacobian<num_states, num_u>(
-      [&](::Eigen::Matrix<double, num_u, 1> U) { return fn(X, U, args...); },
+      [&](::Eigen::Matrix<Scalar, num_u, 1> U) { return fn(X, U, args...); },
       U);
 }
 
diff --git a/frc971/control_loops/jacobian_test.cc b/frc971/control_loops/jacobian_test.cc
index 5606004..92a15a3 100644
--- a/frc971/control_loops/jacobian_test.cc
+++ b/frc971/control_loops/jacobian_test.cc
@@ -19,17 +19,17 @@
 
 // Test that we can recover A from AxBufn pretty accurately.
 TEST(RungeKuttaTest, Ax) {
-  ::Eigen::Matrix<double, 4, 4> NewA =
-      NumericalJacobianX<4, 2>(AxBufn, ::Eigen::Matrix<double, 4, 1>::Zero(),
-                               ::Eigen::Matrix<double, 2, 1>::Zero());
+  ::Eigen::Matrix<double, 4, 4> NewA = NumericalJacobianX<4, 2, double>(
+      AxBufn, ::Eigen::Matrix<double, 4, 1>::Zero(),
+      ::Eigen::Matrix<double, 2, 1>::Zero());
   EXPECT_TRUE(NewA.isApprox(A));
 }
 
 // Test that we can recover B from AxBufn pretty accurately.
 TEST(RungeKuttaTest, Bu) {
-  ::Eigen::Matrix<double, 4, 2> NewB =
-      NumericalJacobianU<4, 2>(AxBufn, ::Eigen::Matrix<double, 4, 1>::Zero(),
-                               ::Eigen::Matrix<double, 2, 1>::Zero());
+  ::Eigen::Matrix<double, 4, 2> NewB = NumericalJacobianU<4, 2, double>(
+      AxBufn, ::Eigen::Matrix<double, 4, 1>::Zero(),
+      ::Eigen::Matrix<double, 2, 1>::Zero());
   EXPECT_TRUE(NewB.isApprox(B));
 }
 
diff --git a/frc971/control_loops/quaternion_utils.h b/frc971/control_loops/quaternion_utils.h
index 3e40e3f4..c2c854c 100644
--- a/frc971/control_loops/quaternion_utils.h
+++ b/frc971/control_loops/quaternion_utils.h
@@ -8,19 +8,17 @@
 
 namespace frc971::controls {
 
-// Function to compute the quaternion average of a bunch of quaternions. Each
-// column in the input matrix is a quaternion (optionally scaled by it's
-// weight).
-template <int SM>
-inline Eigen::Matrix<double, 4, 1> QuaternionMean(
-    Eigen::Matrix<double, 4, SM> input) {
+// Helper function to extract mean quaternion from A*A^T of quaternion list
+// This allows us to support multiple formats of the input quaternion list
+inline Eigen::Matrix<double, 4, 1> ExtractQuaternionMean(
+    Eigen::Matrix<double, 4, 4> input) {
   // Algorithm to compute the average of a bunch of quaternions:
-  // http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf
-
-  Eigen::Matrix<double, 4, 4> m = input * input.transpose();
+  //  http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf
+  // See also:
+  //  https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20070017872.pdf
 
   Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>> solver;
-  solver.compute(m);
+  solver.compute(input);
 
   Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvectorsType
       eigenvectors = solver.eigenvectors();
@@ -47,6 +45,37 @@
   return eigenvectors.col(max_index).real().normalized();
 }
 
+// Function to compute the quaternion average of a bunch of quaternions. Each
+// column in the 4xN input Matrix is a quaternion (optionally scaled by it's
+// weight).
+template <int N>
+inline Eigen::Matrix<double, 4, 1> QuaternionMean(
+    Eigen::Matrix<double, 4, N> quaternions) {
+  Eigen::Matrix<double, 4, 4> m = quaternions * quaternions.transpose();
+
+  return ExtractQuaternionMean(m);
+}
+
+// Function to compute the quaternion average of a bunch of quaternions.
+// This allows for passing in a variable size list (vector) of quaternions
+
+// For reference (since I've been bitten by it):
+// Eigen::Quaternion stores and prints coefficients as [x, y, z, w]
+// and initializes using a Vector4d in this order, BUT
+// initializes with scalars as Eigen::Quaternion{w, x, y, z}
+inline Eigen::Vector4d QuaternionMean(
+    std::vector<Eigen::Vector4d> quaternion_list) {
+  CHECK(quaternion_list.size() != 0)
+      << "Must have at least one quaternion to compute an average!";
+
+  Eigen::Matrix<double, 4, 4> m = Eigen::Matrix4d::Zero();
+  for (Eigen::Vector4d quaternion : quaternion_list) {
+    m += quaternion * quaternion.transpose();
+  }
+
+  return Eigen::Vector4d(ExtractQuaternionMean(m));
+}
+
 // Converts from a quaternion to a rotation vector, where the rotation vector's
 // direction represents the axis to rotate around and its magnitude represents
 // the number of radians to rotate.
@@ -74,7 +103,7 @@
 inline Eigen::Vector4d QuaternionDerivative(Eigen::Vector3d omega,
                                             const Eigen::Vector4d &q_matrix) {
   // See https://www.ashwinnarayan.com/post/how-to-integrate-quaternions/ for
-  // another resource on quaternion integration and derivitives.
+  // another resource on quaternion integration and derivatives.
   Eigen::Quaternion<double> q(q_matrix);
 
   Eigen::Quaternion<double> omega_q;
diff --git a/frc971/control_loops/quaternion_utils_test.cc b/frc971/control_loops/quaternion_utils_test.cc
index 2eaeaa0..63f4507 100644
--- a/frc971/control_loops/quaternion_utils_test.cc
+++ b/frc971/control_loops/quaternion_utils_test.cc
@@ -17,6 +17,7 @@
 // return the original quaternion.
 TEST(DownEstimatorTest, QuaternionMean) {
   Eigen::Matrix<double, 4, 7> vectors;
+  std::vector<Eigen::Vector4d> quaternion_list;
   vectors.col(0) << 0, 0, 0, 1;
   for (int i = 0; i < 3; ++i) {
     Eigen::Matrix<double, 4, 1> perturbation;
@@ -29,6 +30,7 @@
 
   for (int i = 0; i < 7; ++i) {
     vectors.col(i).normalize();
+    quaternion_list.push_back(Eigen::Vector4d(vectors.col(i)));
   }
 
   Eigen::Matrix<double, 4, 1> mean = QuaternionMean(vectors);
@@ -36,6 +38,15 @@
   for (int i = 0; i < 4; ++i) {
     EXPECT_NEAR(mean(i, 0), vectors(i, 0), 0.001) << ": Failed on index " << i;
   }
+
+  // Test version of code that passes in a vector of quaternions
+  mean = QuaternionMean(quaternion_list);
+
+  for (int i = 0; i < 4; ++i) {
+    EXPECT_NEAR(mean(i, 0), quaternion_list[0](i, 0), 0.001)
+        << ": Failed on index " << i << " with mean = " << mean
+        << " and quat_list = " << quaternion_list[0];
+  }
 }
 
 // Tests that ToRotationVectorFromQuaternion works for a 0 rotation.
diff --git a/frc971/imu_fdcan/README.md b/frc971/imu_fdcan/README.md
index 8e82f73..7bf7328 100644
--- a/frc971/imu_fdcan/README.md
+++ b/frc971/imu_fdcan/README.md
@@ -23,7 +23,8 @@
     * The main code lives in [`Dual_IMU/Core/Src`](/Dual_IMU/Core/Src/). Make sure your changes happen inside sections marked `/* USER CODE BEGIN ... */` `/* USER CODE END ... */`. Code outside these markers will be overwritten by CubeIDE when generating code after changes to the `.ioc` file.
 3) Build + Run:
     * Option 1: Open CubeIDE GUI to build, debug, or run.
-    * Option 2: 
+    <!-- TODO(sindy): fix this build script -->
+    * Option 2 (DO NOT USE. NOT SAFE) 
         1) SSH onto the build server. 
         2) Run `bazel build -c opt --config=cortex-m4f-imu //frc971/imu_fdcan/Dual_IMU/Core:main.elf`. The output .elf file should be in bazel-bin/frc971/imu_fdcan/Dual_IMU/Core.
         3) (If deploying code locally) Move file to local directory. For example: `scp <username>@build.frc971.org:<path/to/main.elf> <local/path/to/save/file/`. A good spot to put this locally is ./Dual_IMU/Debug/.
diff --git a/frc971/orin/set_orin_clock.sh b/frc971/orin/set_orin_clock.sh
index bc9dd8f..fd615c5 100755
--- a/frc971/orin/set_orin_clock.sh
+++ b/frc971/orin/set_orin_clock.sh
@@ -12,7 +12,7 @@
 
 for orin in $ORIN_LIST; do
     echo "========================================================"
-    echo "Setting clock for ${ROBOT_PREFIX}71.10${orin}"
+    echo "Setting clock for 10.${ROBOT_PREFIX}.71.10${orin}"
     echo "========================================================"
     current_time=`sudo hwclock`
     IFS="."
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 48a8fb1..dc40bab 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -238,6 +238,7 @@
         ":target_map_fbs",
         "//aos/events:simulated_event_loop",
         "//frc971/control_loops:control_loop",
+        "//frc971/vision:vision_util_lib",
         "//frc971/vision:visualize_robot",
         "//frc971/vision/ceres:pose_graph_3d_lib",
         "//third_party:opencv",
@@ -437,11 +438,16 @@
     hdrs = ["vision_util_lib.h"],
     visibility = ["//visibility:public"],
     deps = [
+        "//aos/util:math",
+        "//frc971/control_loops:quaternion_utils",
         "//frc971/vision:calibration_fbs",
+        "//frc971/vision/ceres:pose_graph_3d_lib",
         "//third_party:opencv",
         "@com_google_absl//absl/log",
         "@com_google_absl//absl/log:check",
         "@com_google_absl//absl/strings:str_format",
+        "@com_google_ceres_solver//:ceres",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
 
diff --git a/frc971/vision/image_logger.cc b/frc971/vision/image_logger.cc
index cb8fc4e..01ceaf4 100644
--- a/frc971/vision/image_logger.cc
+++ b/frc971/vision/image_logger.cc
@@ -75,6 +75,8 @@
     });
   }
 
+  LOG(INFO) << "Starting image_logger; will wait on joystick enabled to start "
+               "logging";
   event_loop.OnRun([]() {
     errno = 0;
     setpriority(PRIO_PROCESS, 0, -20);
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index 36f0a29..99b67d2 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -5,6 +5,7 @@
 #include "frc971/control_loops/control_loop.h"
 #include "frc971/vision/ceres/pose_graph_3d_error_term.h"
 #include "frc971/vision/geometry.h"
+#include "frc971/vision/vision_util_lib.h"
 
 ABSL_FLAG(uint64_t, max_num_iterations, 100,
           "Maximum number of iterations for the ceres solver");
@@ -28,89 +29,6 @@
           "Whether to do a final fit of the solved map to the original map");
 
 namespace frc971::vision {
-Eigen::Affine3d PoseUtils::Pose3dToAffine3d(
-    const ceres::examples::Pose3d &pose3d) {
-  Eigen::Affine3d H_world_pose =
-      Eigen::Translation3d(pose3d.p(0), pose3d.p(1), pose3d.p(2)) * pose3d.q;
-  return H_world_pose;
-}
-
-ceres::examples::Pose3d PoseUtils::Affine3dToPose3d(const Eigen::Affine3d &H) {
-  return ceres::examples::Pose3d{.p = H.translation(),
-                                 .q = Eigen::Quaterniond(H.rotation())};
-}
-
-ceres::examples::Pose3d PoseUtils::ComputeRelativePose(
-    const ceres::examples::Pose3d &pose_1,
-    const ceres::examples::Pose3d &pose_2) {
-  Eigen::Affine3d H_world_1 = Pose3dToAffine3d(pose_1);
-  Eigen::Affine3d H_world_2 = Pose3dToAffine3d(pose_2);
-
-  // Get the location of 2 in the 1 frame
-  Eigen::Affine3d H_1_2 = H_world_1.inverse() * H_world_2;
-  return Affine3dToPose3d(H_1_2);
-}
-
-ceres::examples::Pose3d PoseUtils::ComputeOffsetPose(
-    const ceres::examples::Pose3d &pose_1,
-    const ceres::examples::Pose3d &pose_2_relative) {
-  auto H_world_1 = Pose3dToAffine3d(pose_1);
-  auto H_1_2 = Pose3dToAffine3d(pose_2_relative);
-  auto H_world_2 = H_world_1 * H_1_2;
-
-  return Affine3dToPose3d(H_world_2);
-}
-
-Eigen::Quaterniond PoseUtils::EulerAnglesToQuaternion(
-    const Eigen::Vector3d &rpy) {
-  Eigen::AngleAxisd roll_angle(rpy.x(), Eigen::Vector3d::UnitX());
-  Eigen::AngleAxisd pitch_angle(rpy.y(), Eigen::Vector3d::UnitY());
-  Eigen::AngleAxisd yaw_angle(rpy.z(), Eigen::Vector3d::UnitZ());
-
-  return yaw_angle * pitch_angle * roll_angle;
-}
-
-Eigen::Vector3d PoseUtils::QuaternionToEulerAngles(
-    const Eigen::Quaterniond &q) {
-  return RotationMatrixToEulerAngles(q.toRotationMatrix());
-}
-
-Eigen::Vector3d PoseUtils::RotationMatrixToEulerAngles(
-    const Eigen::Matrix3d &R) {
-  double roll = aos::math::NormalizeAngle(std::atan2(R(2, 1), R(2, 2)));
-  double pitch = aos::math::NormalizeAngle(-std::asin(R(2, 0)));
-  double yaw = aos::math::NormalizeAngle(std::atan2(R(1, 0), R(0, 0)));
-
-  return Eigen::Vector3d(roll, pitch, yaw);
-}
-
-flatbuffers::Offset<TargetPoseFbs> PoseUtils::TargetPoseToFbs(
-    const TargetMapper::TargetPose &target_pose,
-    flatbuffers::FlatBufferBuilder *fbb) {
-  const auto position_offset =
-      CreatePosition(*fbb, target_pose.pose.p(0), target_pose.pose.p(1),
-                     target_pose.pose.p(2));
-  const auto orientation_offset =
-      CreateQuaternion(*fbb, target_pose.pose.q.w(), target_pose.pose.q.x(),
-                       target_pose.pose.q.y(), target_pose.pose.q.z());
-  return CreateTargetPoseFbs(*fbb, target_pose.id, position_offset,
-                             orientation_offset);
-}
-
-TargetMapper::TargetPose PoseUtils::TargetPoseFromFbs(
-    const TargetPoseFbs &target_pose_fbs) {
-  return {.id = static_cast<TargetMapper::TargetId>(target_pose_fbs.id()),
-          .pose = ceres::examples::Pose3d{
-              Eigen::Vector3d(target_pose_fbs.position()->x(),
-                              target_pose_fbs.position()->y(),
-                              target_pose_fbs.position()->z()),
-              Eigen::Quaterniond(target_pose_fbs.orientation()->w(),
-                                 target_pose_fbs.orientation()->x(),
-                                 target_pose_fbs.orientation()->y(),
-                                 target_pose_fbs.orientation()->z())
-                  .normalized()}};
-}
-
 ceres::examples::VectorOfConstraints DataAdapter::MatchTargetDetections(
     const std::vector<DataAdapter::TimestampedDetection>
         &timestamped_target_detections,
@@ -245,7 +163,7 @@
       aos::JsonFileToFlatbuffer<TargetMap>(target_poses_path);
   for (const auto *target_pose_fbs : *target_map.message().target_poses()) {
     ideal_target_poses_[target_pose_fbs->id()] =
-        PoseUtils::TargetPoseFromFbs(*target_pose_fbs).pose;
+        TargetPoseFromFbs(*target_pose_fbs).pose;
   }
   target_poses_ = ideal_target_poses_;
   CountConstraints();
@@ -263,6 +181,33 @@
   CountConstraints();
 }
 
+flatbuffers::Offset<TargetPoseFbs> TargetMapper::TargetPoseToFbs(
+    const TargetMapper::TargetPose &target_pose,
+    flatbuffers::FlatBufferBuilder *fbb) {
+  const auto position_offset =
+      CreatePosition(*fbb, target_pose.pose.p(0), target_pose.pose.p(1),
+                     target_pose.pose.p(2));
+  const auto orientation_offset =
+      CreateQuaternion(*fbb, target_pose.pose.q.w(), target_pose.pose.q.x(),
+                       target_pose.pose.q.y(), target_pose.pose.q.z());
+  return CreateTargetPoseFbs(*fbb, target_pose.id, position_offset,
+                             orientation_offset);
+}
+
+TargetMapper::TargetPose TargetMapper::TargetPoseFromFbs(
+    const TargetPoseFbs &target_pose_fbs) {
+  return {.id = static_cast<TargetMapper::TargetId>(target_pose_fbs.id()),
+          .pose = ceres::examples::Pose3d{
+              Eigen::Vector3d(target_pose_fbs.position()->x(),
+                              target_pose_fbs.position()->y(),
+                              target_pose_fbs.position()->z()),
+              Eigen::Quaterniond(target_pose_fbs.orientation()->w(),
+                                 target_pose_fbs.orientation()->x(),
+                                 target_pose_fbs.orientation()->y(),
+                                 target_pose_fbs.orientation()->z())
+                  .normalized()}};
+}
+
 namespace {
 std::pair<TargetMapper::TargetId, TargetMapper::TargetId> MakeIdPair(
     const ceres::examples::Constraint3d &constraint) {
@@ -580,7 +525,7 @@
   std::vector<flatbuffers::Offset<TargetPoseFbs>> target_poses_fbs;
   for (const auto &[id, pose] : target_poses_) {
     target_poses_fbs.emplace_back(
-        PoseUtils::TargetPoseToFbs(TargetPose{.id = id, .pose = pose}, &fbb));
+        TargetPoseToFbs(TargetPose{.id = id, .pose = pose}, &fbb));
   }
 
   const auto field_name_offset = fbb.CreateString(field_name);
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index cae1c69..aa34006 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -9,8 +9,11 @@
 #include "aos/events/simulated_event_loop.h"
 #include "frc971/vision/ceres/types.h"
 #include "frc971/vision/target_map_generated.h"
+#include "frc971/vision/vision_util_lib.h"
 #include "frc971/vision/visualize_robot.h"
 
+ABSL_DECLARE_FLAG(double, outlier_std_devs);
+
 namespace frc971::vision {
 
 // Estimates positions of vision targets (ex. April Tags) using
@@ -46,6 +49,15 @@
   // Prints target poses into a TargetMap flatbuffer json
   std::string MapToJson(std::string_view field_name) const;
 
+  // Builds a TargetPoseFbs from a TargetPose
+  static flatbuffers::Offset<TargetPoseFbs> TargetPoseToFbs(
+      const TargetMapper::TargetPose &target_pose,
+      flatbuffers::FlatBufferBuilder *fbb);
+
+  // Converts a TargetPoseFbs to a TargetPose
+  static TargetMapper::TargetPose TargetPoseFromFbs(
+      const TargetPoseFbs &target_pose_fbs);
+
   static std::optional<TargetPose> GetTargetPoseById(
       std::vector<TargetPose> target_poses, TargetId target_id);
 
@@ -134,43 +146,6 @@
   Stats stats_with_outliers_;
 };
 
-// Utility functions for dealing with ceres::examples::Pose3d structs
-class PoseUtils {
- public:
-  // Embeds a 3d pose into an affine transformation
-  static Eigen::Affine3d Pose3dToAffine3d(
-      const ceres::examples::Pose3d &pose3d);
-  // Inverse of above function
-  static ceres::examples::Pose3d Affine3dToPose3d(const Eigen::Affine3d &H);
-
-  // Computes pose_2 relative to pose_1. This is equivalent to (pose_1^-1 *
-  // pose_2)
-  static ceres::examples::Pose3d ComputeRelativePose(
-      const ceres::examples::Pose3d &pose_1,
-      const ceres::examples::Pose3d &pose_2);
-
-  // Computes pose_2 given a pose_1 and pose_2 relative to pose_1. This is
-  // equivalent to (pose_1 * pose_2_relative)
-  static ceres::examples::Pose3d ComputeOffsetPose(
-      const ceres::examples::Pose3d &pose_1,
-      const ceres::examples::Pose3d &pose_2_relative);
-
-  // Converts a rotation with roll, pitch, and yaw into a quaternion
-  static Eigen::Quaterniond EulerAnglesToQuaternion(const Eigen::Vector3d &rpy);
-  // Inverse of above function
-  static Eigen::Vector3d QuaternionToEulerAngles(const Eigen::Quaterniond &q);
-  // Converts a 3d rotation matrix into a rotation with roll, pitch, and yaw
-  static Eigen::Vector3d RotationMatrixToEulerAngles(const Eigen::Matrix3d &R);
-
-  // Builds a TargetPoseFbs from a TargetPose
-  static flatbuffers::Offset<TargetPoseFbs> TargetPoseToFbs(
-      const TargetMapper::TargetPose &target_pose,
-      flatbuffers::FlatBufferBuilder *fbb);
-  // Converts a TargetPoseFbs to a TargetPose
-  static TargetMapper::TargetPose TargetPoseFromFbs(
-      const TargetPoseFbs &target_pose_fbs);
-};
-
 // Transforms robot position and target detection data into target constraints
 // to be used for mapping.
 class DataAdapter {
diff --git a/frc971/vision/target_mapper_test.cc b/frc971/vision/target_mapper_test.cc
index 4e9db6f..62f969c 100644
--- a/frc971/vision/target_mapper_test.cc
+++ b/frc971/vision/target_mapper_test.cc
@@ -12,11 +12,13 @@
 #include "aos/testing/path.h"
 #include "aos/testing/random_seed.h"
 #include "aos/util/math.h"
+#include "vision_util_lib.h"
 
 ABSL_DECLARE_FLAG(int32_t, min_target_id);
 ABSL_DECLARE_FLAG(int32_t, max_target_id);
 
 namespace frc971::vision {
+using frc971::vision::PoseUtils;
 
 namespace {
 constexpr double kToleranceMeters = 0.05;
@@ -151,44 +153,6 @@
 
 }  // namespace
 
-TEST(PoseUtilsTest, EulerAnglesAndQuaternionConversions) {
-  // Make sure that the conversions are consistent back and forth.
-  // These angles shouldn't get changed to a different, equivalent roll pitch
-  // yaw.
-  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, M_PI);
-  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, -M_PI);
-  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, M_PI_2);
-  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, -M_PI_2);
-  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, 0.0);
-  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, M_PI_4, 0.0);
-  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, -M_PI_4, 0.0);
-  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, -M_PI_4, M_PI_4);
-  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(M_PI_4, -M_PI_4, M_PI_4);
-  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(-M_PI_2, -M_PI_4, M_PI_4);
-
-  // Now, do a sweep of roll, pitch, and yaws in the normalized
-  // range.
-  // - roll: (-pi/2, pi/2)
-  // - pitch: (-pi/2, pi/2)
-  // - yaw: [-pi, pi)
-  constexpr double kThetaMaxRoll = M_PI_2 - kToleranceRadians;
-  constexpr double kThetaMaxPitch = M_PI_2 - kToleranceRadians;
-  constexpr double kThetaMaxYaw = M_PI;
-  constexpr double kDeltaTheta = M_PI / 16;
-
-  for (double roll = -kThetaMaxRoll; roll < kThetaMaxRoll;
-       roll += kDeltaTheta) {
-    for (double pitch = -kThetaMaxPitch; pitch < kThetaMaxPitch;
-         pitch += kDeltaTheta) {
-      for (double yaw = -kThetaMaxYaw; yaw < kThetaMaxYaw; yaw += kDeltaTheta) {
-        SCOPED_TRACE(
-            absl::StrFormat("roll: %f, pitch: %f, yaw: %f", roll, pitch, yaw));
-        EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(roll, pitch, yaw);
-      }
-    }
-  }
-}
-
 TEST(DataAdapterTest, ComputeConfidence) {
   // Check the confidence matrices. Don't check the actual values
   // in case the constants change, just check that the confidence of contraints
@@ -500,7 +464,7 @@
   ceres::examples::MapOfPoses target_poses;
   for (const auto *target_pose_fbs : *target_map_fbs.message().target_poses()) {
     const TargetMapper::TargetPose target_pose =
-        PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
+        TargetMapper::TargetPoseFromFbs(*target_pose_fbs);
     actual_target_poses.emplace_back(target_pose);
     target_poses[target_pose.id] = target_pose.pose;
   }
diff --git a/frc971/vision/vision_util_lib.cc b/frc971/vision/vision_util_lib.cc
index 089f3ac..944fa76 100644
--- a/frc971/vision/vision_util_lib.cc
+++ b/frc971/vision/vision_util_lib.cc
@@ -1,9 +1,14 @@
 #include "frc971/vision/vision_util_lib.h"
 
+#include <numeric>
+
 #include "absl/log/check.h"
 #include "absl/log/log.h"
 #include "absl/strings/str_format.h"
 
+#include "aos/util/math.h"
+#include "frc971/control_loops/quaternion_utils.h"
+
 namespace frc971::vision {
 
 std::optional<cv::Mat> CameraExtrinsics(
@@ -75,4 +80,147 @@
   return calibration_filename;
 }
 
+Eigen::Affine3d PoseUtils::Pose3dToAffine3d(
+    const ceres::examples::Pose3d &pose3d) {
+  Eigen::Affine3d H_world_pose =
+      Eigen::Translation3d(pose3d.p(0), pose3d.p(1), pose3d.p(2)) * pose3d.q;
+  return H_world_pose;
+}
+
+ceres::examples::Pose3d PoseUtils::Affine3dToPose3d(const Eigen::Affine3d &H) {
+  return ceres::examples::Pose3d{.p = H.translation(),
+                                 .q = Eigen::Quaterniond(H.rotation())};
+}
+
+ceres::examples::Pose3d PoseUtils::ComputeRelativePose(
+    const ceres::examples::Pose3d &pose_1,
+    const ceres::examples::Pose3d &pose_2) {
+  Eigen::Affine3d H_world_1 = Pose3dToAffine3d(pose_1);
+  Eigen::Affine3d H_world_2 = Pose3dToAffine3d(pose_2);
+
+  // Get the location of 2 in the 1 frame
+  Eigen::Affine3d H_1_2 = H_world_1.inverse() * H_world_2;
+  return Affine3dToPose3d(H_1_2);
+}
+
+ceres::examples::Pose3d PoseUtils::ComputeOffsetPose(
+    const ceres::examples::Pose3d &pose_1,
+    const ceres::examples::Pose3d &pose_2_relative) {
+  auto H_world_1 = Pose3dToAffine3d(pose_1);
+  auto H_1_2 = Pose3dToAffine3d(pose_2_relative);
+  auto H_world_2 = H_world_1 * H_1_2;
+
+  return Affine3dToPose3d(H_world_2);
+}
+
+Eigen::Quaterniond PoseUtils::EulerAnglesToQuaternion(
+    const Eigen::Vector3d &rpy) {
+  Eigen::AngleAxisd roll_angle(rpy.x(), Eigen::Vector3d::UnitX());
+  Eigen::AngleAxisd pitch_angle(rpy.y(), Eigen::Vector3d::UnitY());
+  Eigen::AngleAxisd yaw_angle(rpy.z(), Eigen::Vector3d::UnitZ());
+
+  return yaw_angle * pitch_angle * roll_angle;
+}
+
+Eigen::Vector3d PoseUtils::QuaternionToEulerAngles(
+    const Eigen::Quaterniond &q) {
+  return RotationMatrixToEulerAngles(q.toRotationMatrix());
+}
+
+Eigen::Vector3d PoseUtils::RotationMatrixToEulerAngles(
+    const Eigen::Matrix3d &R) {
+  double roll = aos::math::NormalizeAngle(std::atan2(R(2, 1), R(2, 2)));
+  double pitch = aos::math::NormalizeAngle(-std::asin(R(2, 0)));
+  double yaw = aos::math::NormalizeAngle(std::atan2(R(1, 0), R(0, 0)));
+
+  return Eigen::Vector3d(roll, pitch, yaw);
+}
+
+// Compute the average pose given a list of translations (as
+// Eigen::Vector3d's) and rotations (as Eigen::Vector4d quaternions)
+// Returned as an Eigen::Affine3d pose
+// Also, compute the variance of each of these list of vectors
+
+// NOTE: variance for rotations can get dicey, so we've cheated a
+// little by doing two things:
+// 1) Computing variance relative to the mean, so that we're doing
+// this on small angles and don't have to deal with wrapping at
+// 180/360 degrees
+// 2) Returning the variance in Euler angles, since I'm not sure of a
+// better way to represent variance for rotations.  (Maybe log of
+// rotations, in the Lie algebra?)
+
+Eigen::Affine3d ComputeAveragePose(
+    std::vector<Eigen::Vector3d> &translation_list,
+    std::vector<Eigen::Vector4d> &rotation_list,
+    Eigen::Vector3d *translation_variance, Eigen::Vector3d *rotation_variance) {
+  Eigen::Vector3d avg_translation =
+      std::accumulate(translation_list.begin(), translation_list.end(),
+                      Eigen::Vector3d{0, 0, 0}) /
+      translation_list.size();
+
+  Eigen::Quaterniond avg_rotation_q(
+      frc971::controls::QuaternionMean(rotation_list));
+  Eigen::Affine3d average_pose =
+      Eigen::Translation3d(avg_translation) * avg_rotation_q;
+
+  CHECK_EQ(translation_list.size(), rotation_list.size());
+  if (translation_variance != nullptr) {
+    CHECK(rotation_variance != nullptr);
+    Eigen::Vector3d translation_variance_sum(0.0, 0.0, 0.0);
+    Eigen::Vector3d rotation_variance_sum(0.0, 0.0, 0.0);
+    for (uint i = 0; i < translation_list.size(); i++) {
+      Eigen::Quaterniond rotation_q(rotation_list[i]);
+      Eigen::Affine3d pose =
+          Eigen::Translation3d(translation_list[i]) * rotation_q;
+      Eigen::Affine3d delta_pose = average_pose * pose.inverse();
+      translation_variance_sum =
+          translation_variance_sum +
+          Eigen::Vector3d(delta_pose.translation().array().square());
+      rotation_variance_sum =
+          rotation_variance_sum +
+          Eigen::Vector3d(PoseUtils::RotationMatrixToEulerAngles(
+                              delta_pose.rotation().matrix())
+                              .array()
+                              .square());
+    }
+    // Compute the variance on the translations (in m)
+    if (translation_variance != nullptr) {
+      CHECK(translation_list.size() > 1)
+          << "Have to have at least two translations to compute variance";
+      *translation_variance =
+          translation_variance_sum / translation_list.size();
+    }
+
+    // Compute the variance on the rotations (in euler angles, radians),
+    // referenced to the mean, to remove issues with Euler angles by
+    // keeping them near zero
+    if (rotation_variance != nullptr) {
+      CHECK(rotation_list.size() > 1)
+          << "Have to have at least two rotations to compute variance";
+      *rotation_variance = rotation_variance_sum / rotation_list.size();
+    }
+  }
+  return average_pose;
+}
+
+// Helper function to compute average pose when supplied with list
+// of Eigen::Affine3d's
+Eigen::Affine3d ComputeAveragePose(std::vector<Eigen::Affine3d> &pose_list,
+                                   Eigen::Vector3d *translation_variance,
+                                   Eigen::Vector3d *rotation_variance) {
+  std::vector<Eigen::Vector3d> translation_list;
+  std::vector<Eigen::Vector4d> rotation_list;
+
+  for (Eigen::Affine3d pose : pose_list) {
+    translation_list.push_back(pose.translation());
+    Eigen::Quaterniond quat(pose.rotation().matrix());
+    rotation_list.push_back(
+        Eigen::Vector4d(quat.x(), quat.y(), quat.z(), quat.w()));
+  }
+
+  return ComputeAveragePose(translation_list, rotation_list,
+                            translation_variance, rotation_variance);
+}
+
 }  // namespace frc971::vision
diff --git a/frc971/vision/vision_util_lib.h b/frc971/vision/vision_util_lib.h
index a26af3b..ee621dd 100644
--- a/frc971/vision/vision_util_lib.h
+++ b/frc971/vision/vision_util_lib.h
@@ -3,9 +3,13 @@
 #include <optional>
 #include <string_view>
 
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+#include "Eigen/SVD"
 #include "opencv2/imgproc.hpp"
 
 #include "frc971/vision/calibration_generated.h"
+#include "frc971/vision/ceres/types.h"
 
 // Extract the CameraExtrinsics from a CameraCalibration struct
 namespace frc971::vision {
@@ -31,6 +35,55 @@
                                 int camera_number, std::string camera_id,
                                 std::string timestamp);
 
+// Utility functions for dealing with ceres::examples::Pose3d structs
+class PoseUtils {
+ public:
+  // Embeds a 3d pose into an affine transformation
+  static Eigen::Affine3d Pose3dToAffine3d(
+      const ceres::examples::Pose3d &pose3d);
+  // Inverse of above function
+  static ceres::examples::Pose3d Affine3dToPose3d(const Eigen::Affine3d &H);
+
+  // Computes pose_2 relative to pose_1. This is equivalent to (pose_1^-1 *
+  // pose_2)
+  static ceres::examples::Pose3d ComputeRelativePose(
+      const ceres::examples::Pose3d &pose_1,
+      const ceres::examples::Pose3d &pose_2);
+
+  // Computes pose_2 given a pose_1 and pose_2 relative to pose_1. This is
+  // equivalent to (pose_1 * pose_2_relative)
+  static ceres::examples::Pose3d ComputeOffsetPose(
+      const ceres::examples::Pose3d &pose_1,
+      const ceres::examples::Pose3d &pose_2_relative);
+
+  // Converts a rotation with roll, pitch, and yaw into a quaternion
+  static Eigen::Quaterniond EulerAnglesToQuaternion(const Eigen::Vector3d &rpy);
+  // Inverse of above function
+  static Eigen::Vector3d QuaternionToEulerAngles(const Eigen::Quaterniond &q);
+  // Converts a 3d rotation matrix into a rotation with roll, pitch, and yaw
+  static Eigen::Vector3d RotationMatrixToEulerAngles(const Eigen::Matrix3d &R);
+};
+
+// Compute the average of a set of quaternions
+Eigen::Vector4d QuaternionAverage(std::vector<Eigen::Vector4d> quaternions);
+
+// Compute the average pose given a list of translations (as
+// Eigen::Vector3d's) and rotations (as Eigen::Vector4d quaternions)
+// Returned as an Eigen::Affine3d pose
+// Also, compute the variance of each of these list of vectors
+Eigen::Affine3d ComputeAveragePose(
+    std::vector<Eigen::Vector3d> &translation_list,
+    std::vector<Eigen::Vector4d> &rotation_list,
+    Eigen::Vector3d *translation_variance = nullptr,
+    Eigen::Vector3d *rotation_variance = nullptr);
+
+// Helper function to compute average pose when supplied with list
+// of Eigen::Affine3d's
+Eigen::Affine3d ComputeAveragePose(
+    std::vector<Eigen::Affine3d> &pose_list,
+    Eigen::Vector3d *translation_variance = nullptr,
+    Eigen::Vector3d *rotation_variance = nullptr);
+
 }  // namespace frc971::vision
 
 #endif  // FRC971_VISION_VISION_UTIL_LIB_H_
diff --git a/frc971/vision/vision_util_lib_test.cc b/frc971/vision/vision_util_lib_test.cc
index ff9c0a3..bdf8258 100644
--- a/frc971/vision/vision_util_lib_test.cc
+++ b/frc971/vision/vision_util_lib_test.cc
@@ -1,7 +1,10 @@
 #include "frc971/vision/vision_util_lib.h"
 
+#include "absl/strings/str_format.h"
 #include "gtest/gtest.h"
 
+#include "aos/util/math.h"
+
 namespace frc971::vision {
 // For now, just testing extracting camera number from channel name
 TEST(VisionUtilsTest, CameraNumberFromChannel) {
@@ -12,4 +15,114 @@
   ASSERT_EQ(CameraNumberFromChannel("/orin1/camera1").value(), 1);
   ASSERT_EQ(CameraNumberFromChannel("/orin1"), std::nullopt);
 }
+
+namespace {
+constexpr double kToleranceRadians = 0.05;
+// Conversions between euler angles and quaternion result in slightly-off
+// doubles
+constexpr double kOrientationEqTolerance = 1e-10;
+}  // namespace
+
+// Angles normalized by aos::math::NormalizeAngle()
+#define EXPECT_NORMALIZED_ANGLES_NEAR(theta1, theta2, tolerance)           \
+  {                                                                        \
+    double delta = std::abs(aos::math::DiffAngle(theta1, theta2));         \
+    /* Have to check delta - 2pi for the case that one angle is very */    \
+    /* close to -pi, and the other is very close to +pi */                 \
+    EXPECT_TRUE(delta < tolerance || std::abs(aos::math::DiffAngle(        \
+                                         delta, 2.0 * M_PI)) < tolerance); \
+  }
+
+#define EXPECT_POSE_NEAR(pose1, pose2)                                     \
+  {                                                                        \
+    for (size_t i = 0; i < 3; i++) {                                       \
+      EXPECT_NEAR(pose1.p(i), pose2.p(i), kToleranceMeters);               \
+    }                                                                      \
+    auto rpy_1 = PoseUtils::QuaternionToEulerAngles(pose1.q);              \
+    auto rpy_2 = PoseUtils::QuaternionToEulerAngles(pose2.q);              \
+    for (size_t i = 0; i < 3; i++) {                                       \
+      SCOPED_TRACE(absl::StrFormat("rpy_1(%d) = %f, rpy_2(%d) = %f", i,    \
+                                   rpy_1(i), i, rpy_2(i)));                \
+      EXPECT_NORMALIZED_ANGLES_NEAR(rpy_1(i), rpy_2(i), kToleranceRadians) \
+    }                                                                      \
+  }
+
+#define EXPECT_POSE_EQ(pose1, pose2) \
+  EXPECT_EQ(pose1.p, pose2.p);       \
+  EXPECT_EQ(pose1.q, pose2.q);
+
+#define EXPECT_QUATERNION_NEAR(q1, q2)                                        \
+  EXPECT_NEAR(q1.x(), q2.x(), kOrientationEqTolerance) << q1 << " != " << q2; \
+  EXPECT_NEAR(q1.y(), q2.y(), kOrientationEqTolerance) << q1 << " != " << q2; \
+  EXPECT_NEAR(q1.z(), q2.z(), kOrientationEqTolerance) << q1 << " != " << q2; \
+  EXPECT_NEAR(q1.w(), q2.w(), kOrientationEqTolerance) << q1 << " != " << q2;
+
+// Expects same roll, pitch, and yaw values (not equivalent rotation)
+#define EXPECT_RPY_EQ(rpy_1, rpy_2)                                     \
+  {                                                                     \
+    for (size_t i = 0; i < 3; i++) {                                    \
+      SCOPED_TRACE(absl::StrFormat("rpy_1(%d) = %f, rpy_2(%d) = %f", i, \
+                                   rpy_1(i), i, rpy_2(i)));             \
+      EXPECT_NORMALIZED_ANGLES_NEAR(rpy_1(i), rpy_2(i),                 \
+                                    kOrientationEqTolerance)            \
+    }                                                                   \
+  }
+
+#define EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(roll, pitch, yaw) \
+  {                                                                        \
+    auto rpy = Eigen::Vector3d(roll, pitch, yaw);                          \
+    auto converted_rpy = PoseUtils::QuaternionToEulerAngles(               \
+        PoseUtils::EulerAnglesToQuaternion(rpy));                          \
+    EXPECT_RPY_EQ(converted_rpy, rpy);                                     \
+  }
+
+// Both confidence matrixes should have the same dimensions and be square
+#define EXPECT_CONFIDENCE_GT(confidence1, confidence2) \
+  {                                                    \
+    ASSERT_EQ(confidence1.rows(), confidence2.rows()); \
+    ASSERT_EQ(confidence1.rows(), confidence1.cols()); \
+    ASSERT_EQ(confidence2.rows(), confidence2.cols()); \
+    for (size_t i = 0; i < confidence1.rows(); i++) {  \
+      EXPECT_GT(confidence1(i, i), confidence2(i, i)); \
+    }                                                  \
+  }
+
+TEST(PoseUtilsTest, EulerAnglesAndQuaternionConversions) {
+  // Make sure that the conversions are consistent back and forth.
+  // These angles shouldn't get changed to a different, equivalent roll pitch
+  // yaw.
+  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, M_PI);
+  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, -M_PI);
+  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, M_PI_2);
+  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, -M_PI_2);
+  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, 0.0);
+  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, M_PI_4, 0.0);
+  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, -M_PI_4, 0.0);
+  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, -M_PI_4, M_PI_4);
+  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(M_PI_4, -M_PI_4, M_PI_4);
+  EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(-M_PI_2, -M_PI_4, M_PI_4);
+
+  // Now, do a sweep of roll, pitch, and yaws in the normalized
+  // range.
+  // - roll: (-pi/2, pi/2)
+  // - pitch: (-pi/2, pi/2)
+  // - yaw: [-pi, pi)
+  constexpr double kThetaMaxRoll = M_PI_2 - kToleranceRadians;
+  constexpr double kThetaMaxPitch = M_PI_2 - kToleranceRadians;
+  constexpr double kThetaMaxYaw = M_PI;
+  constexpr double kDeltaTheta = M_PI / 16;
+
+  for (double roll = -kThetaMaxRoll; roll < kThetaMaxRoll;
+       roll += kDeltaTheta) {
+    for (double pitch = -kThetaMaxPitch; pitch < kThetaMaxPitch;
+         pitch += kDeltaTheta) {
+      for (double yaw = -kThetaMaxYaw; yaw < kThetaMaxYaw; yaw += kDeltaTheta) {
+        SCOPED_TRACE(
+            absl::StrFormat("roll: %f, pitch: %f, yaw: %f", roll, pitch, yaw));
+        EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(roll, pitch, yaw);
+      }
+    }
+  }
+}
+
 }  // namespace frc971::vision
diff --git a/y2023/control_loops/superstructure/arm/arm_design.cc b/y2023/control_loops/superstructure/arm/arm_design.cc
index 07439d6..4f7c629 100644
--- a/y2023/control_loops/superstructure/arm/arm_design.cc
+++ b/y2023/control_loops/superstructure/arm/arm_design.cc
@@ -106,13 +106,13 @@
     const auto x_blocked = X.block<4, 1>(0, 0);
 
     const ::Eigen::Matrix<double, 4, 4> final_A =
-        ::frc971::control_loops::NumericalJacobianX<4, 2>(
+        ::frc971::control_loops::NumericalJacobianX<4, 2, double>(
             [dynamics](const auto &x_blocked, const auto &U, double kDt) {
               return dynamics.UnboundedDiscreteDynamics(x_blocked, U, kDt);
             },
             x_blocked, U, 0.00505);
     const ::Eigen::Matrix<double, 4, 2> final_B =
-        ::frc971::control_loops::NumericalJacobianU<4, 2>(
+        ::frc971::control_loops::NumericalJacobianU<4, 2, double>(
             [dynamics](const auto &x_blocked, const auto &U, double kDt) {
               return dynamics.UnboundedDiscreteDynamics(x_blocked, U, kDt);
             },
diff --git a/y2023/control_loops/superstructure/arm/trajectory.cc b/y2023/control_loops/superstructure/arm/trajectory.cc
index f7ae5f9..4ad4424 100644
--- a/y2023/control_loops/superstructure/arm/trajectory.cc
+++ b/y2023/control_loops/superstructure/arm/trajectory.cc
@@ -736,14 +736,14 @@
   const auto x_blocked = arm_X.block<4, 1>(0, 0);
 
   const ::Eigen::Matrix<double, 4, 4> final_A =
-      ::frc971::control_loops::NumericalJacobianX<4, 2>(
+      ::frc971::control_loops::NumericalJacobianX<4, 2, double>(
           [this](const auto &x_blocked, const auto &U, double dt) {
             return this->dynamics_->UnboundedDiscreteDynamics(x_blocked, U, dt);
           },
           x_blocked, arm_U, 0.00505);
 
   const ::Eigen::Matrix<double, 4, 2> final_B =
-      ::frc971::control_loops::NumericalJacobianU<4, 2>(
+      ::frc971::control_loops::NumericalJacobianU<4, 2, double>(
           [this](const auto &x_blocked, const auto &U, double dt) {
             return this->dynamics_->UnboundedDiscreteDynamics(x_blocked, U, dt);
           },
diff --git a/y2023/vision/april_detection_test.cc b/y2023/vision/april_detection_test.cc
index 9f4f8b7..7e50b09 100644
--- a/y2023/vision/april_detection_test.cc
+++ b/y2023/vision/april_detection_test.cc
@@ -11,6 +11,7 @@
 #include "frc971/constants/constants_sender_lib.h"
 #include "frc971/vision/target_mapper.h"
 #include "frc971/vision/vision_generated.h"
+#include "frc971/vision/vision_util_lib.h"
 #include "y2023/constants/constants_generated.h"
 #include "y2023/constants/constants_list_generated.h"
 #include "y2023/vision/aprilrobotics.h"
@@ -61,7 +62,7 @@
     ASSERT_EQ(april_pose_fetcher_->target_poses()->size(), 1);
 
     frc971::vision::TargetMapper::TargetPose target_pose =
-        frc971::vision::PoseUtils::TargetPoseFromFbs(
+        frc971::vision::TargetMapper::TargetPoseFromFbs(
             *april_pose_fetcher_->target_poses()->Get(0));
 
     ASSERT_EQ(april_pose_fetcher_->target_poses()->Get(0)->id(), 8);
diff --git a/y2023/vision/calibrate_multi_cameras.cc b/y2023/vision/calibrate_multi_cameras.cc
index 8032c764..5c29797 100644
--- a/y2023/vision/calibrate_multi_cameras.cc
+++ b/y2023/vision/calibrate_multi_cameras.cc
@@ -13,6 +13,7 @@
 #include "frc971/vision/charuco_lib.h"
 #include "frc971/vision/extrinsics_calibration.h"
 #include "frc971/vision/target_mapper.h"
+#include "frc971/vision/vision_util_lib.h"
 #include "frc971/vision/visualize_robot.h"
 // clang-format off
 // OpenCV eigen files must be included after Eigen includes
@@ -362,7 +363,7 @@
     }
 
     const TargetMapper::TargetPose target_pose =
-        PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
+        TargetMapper::TargetPoseFromFbs(*target_pose_fbs);
 
     target_poses.emplace_back(target_pose);
 
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index f33a83a..325ad90 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -15,6 +15,7 @@
 #include "frc971/control_loops/pose.h"
 #include "frc971/vision/calibration_generated.h"
 #include "frc971/vision/target_mapper.h"
+#include "frc971/vision/vision_util_lib.h"
 #include "frc971/vision/visualize_robot.h"
 #include "y2023/constants/simulated_constants_sender.h"
 #include "y2023/vision/aprilrobotics.h"
@@ -239,7 +240,7 @@
     }
 
     const TargetMapper::TargetPose target_pose =
-        PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
+        TargetMapper::TargetPoseFromFbs(*target_pose_fbs);
 
     Eigen::Affine3d H_camera_target =
         Eigen::Translation3d(target_pose.pose.p) * target_pose.pose.q;
diff --git a/y2024/vision/calibrate_multi_cameras.cc b/y2024/vision/calibrate_multi_cameras.cc
index 7a9f412..b0fa33b 100644
--- a/y2024/vision/calibrate_multi_cameras.cc
+++ b/y2024/vision/calibrate_multi_cameras.cc
@@ -40,6 +40,9 @@
           "Throw out target poses with a higher pose error than this");
 ABSL_FLAG(double, max_pose_error_ratio, 0.4,
           "Throw out target poses with a higher pose error ratio than this");
+ABSL_FLAG(bool, robot, false,
+          "If true we're calibrating extrinsics for the robot, use the "
+          "correct node path for the robot.");
 ABSL_FLAG(std::string, output_folder, "/tmp",
           "Directory in which to store the updated calibration files");
 ABSL_FLAG(std::string, target_type, "charuco_diamond",
@@ -49,12 +52,10 @@
 ABSL_FLAG(
     uint64_t, wait_key, 1,
     "Time in ms to wait between images (0 to wait indefinitely until click)");
-ABSL_FLAG(bool, robot, false,
-          "If true we're calibrating extrinsics for the robot, use the "
-          "correct node path for the robot.");
 
 ABSL_DECLARE_FLAG(int32_t, min_target_id);
 ABSL_DECLARE_FLAG(int32_t, max_target_id);
+ABSL_DECLARE_FLAG(double, outlier_std_devs);
 
 // Calibrate extrinsic relationship between cameras using two targets
 // seen jointly between cameras.  Uses two types of information: 1)
@@ -72,8 +73,6 @@
 // from all cameras, not just first camera
 
 namespace y2024::vision {
-using frc971::vision::DataAdapter;
-using frc971::vision::ImageCallback;
 using frc971::vision::PoseUtils;
 using frc971::vision::TargetMap;
 using frc971::vision::TargetMapper;
@@ -83,8 +82,8 @@
 static constexpr double kImagePeriodMs =
     1.0 / 60.0 * 1000.0;  // Image capture period in ms
 
+// Set up our camera naming data
 std::vector<CameraNode> node_list(y2024::vision::CreateNodeList());
-
 std::map<std::string, int> ordering_map(
     y2024::vision::CreateOrderingMap(node_list));
 
@@ -111,57 +110,8 @@
 std::vector<TimestampedCameraDetection> two_board_extrinsics_list;
 VisualizeRobot vis_robot_;
 
-// TODO<jim>: Implement variance calcs
-Eigen::Affine3d ComputeAveragePose(
-    std::vector<Eigen::Vector3d> &translation_list,
-    std::vector<Eigen::Vector4d> &rotation_list,
-    Eigen::Vector3d *translation_variance = nullptr,
-    Eigen::Vector3d *rotation_variance = nullptr) {
-  Eigen::Vector3d avg_translation =
-      std::accumulate(translation_list.begin(), translation_list.end(),
-                      Eigen::Vector3d{0, 0, 0}) /
-      translation_list.size();
-
-  // TODO<Jim>: Use QuaternionMean from quaternion_utils.cc (but this
-  // requires a fixed number of quaternions to be averaged
-  Eigen::Vector4d avg_rotation =
-      std::accumulate(rotation_list.begin(), rotation_list.end(),
-                      Eigen::Vector4d{0, 0, 0, 0}) /
-      rotation_list.size();
-  // Normalize, so it's a valid quaternion
-  avg_rotation = avg_rotation / avg_rotation.norm();
-  Eigen::Quaterniond avg_rotation_q{avg_rotation[0], avg_rotation[1],
-                                    avg_rotation[2], avg_rotation[3]};
-  Eigen::Translation3d translation(avg_translation);
-  Eigen::Affine3d return_pose = translation * avg_rotation_q;
-  if (translation_variance != nullptr) {
-    *translation_variance = Eigen::Vector3d();
-  }
-  if (rotation_variance != nullptr) {
-    *rotation_variance = Eigen::Vector3d();
-  }
-
-  return return_pose;
-}
-
-Eigen::Affine3d ComputeAveragePose(
-    std::vector<Eigen::Affine3d> &pose_list,
-    Eigen::Vector3d *translation_variance = nullptr,
-    Eigen::Vector3d *rotation_variance = nullptr) {
-  std::vector<Eigen::Vector3d> translation_list;
-  std::vector<Eigen::Vector4d> rotation_list;
-
-  for (Eigen::Affine3d pose : pose_list) {
-    translation_list.push_back(pose.translation());
-    Eigen::Quaterniond quat(pose.rotation().matrix());
-    rotation_list.push_back(
-        Eigen::Vector4d(quat.w(), quat.x(), quat.y(), quat.z()));
-  }
-
-  return ComputeAveragePose(translation_list, rotation_list,
-                            translation_variance, rotation_variance);
-}
-
+// Helper function to compute average pose when supplied with list
+// of TimestampedCameraDetection's
 Eigen::Affine3d ComputeAveragePose(
     std::vector<TimestampedCameraDetection> &pose_list,
     Eigen::Vector3d *translation_variance = nullptr,
@@ -172,18 +122,95 @@
   for (TimestampedCameraDetection pose : pose_list) {
     translation_list.push_back(pose.H_camera_target.translation());
     Eigen::Quaterniond quat(pose.H_camera_target.rotation().matrix());
+    // NOTE: Eigen initialies as (x,y,z,w) from Vector4d's
     rotation_list.push_back(
-        Eigen::Vector4d(quat.w(), quat.x(), quat.y(), quat.z()));
+        Eigen::Vector4d(quat.x(), quat.y(), quat.z(), quat.w()));
   }
-  return ComputeAveragePose(translation_list, rotation_list,
-                            translation_variance, rotation_variance);
+  return frc971::vision::ComputeAveragePose(
+      translation_list, rotation_list, translation_variance, rotation_variance);
 }
 
+// Do outlier rejection.  Given a list of poses, compute the
+// mean and standard deviation, and throw out those more than
+// FLAGS_outlier_std_devs standard deviations away from the mean.
+// Repeat for the desired number of iterations or until we don't throw
+// out any more outliers
+void RemoveOutliers(std::vector<TimestampedCameraDetection> &pose_list,
+                    int num_iterations = 1) {
+  for (int i = 1; i <= num_iterations; i++) {
+    Eigen::Vector3d translation_variance, rotation_variance;
+    Eigen::Affine3d avg_pose = ComputeAveragePose(
+        pose_list, &translation_variance, &rotation_variance);
+
+    size_t original_size = pose_list.size();
+    // Create a lambda to identify the outliers to be removed
+    auto remove_lambda = [translation_variance, rotation_variance,
+                          avg_pose](const TimestampedCameraDetection &pose) {
+      Eigen::Affine3d H_delta = avg_pose * pose.H_camera_target.inverse();
+      // Compute the z-score for each dimension, and use the max to
+      // decide on outliers.  This is similar to the Mahalanobis
+      // distance (scale by inverse of the covariance matrix), but we're
+      // treating each dimension independently
+      Eigen::Matrix3d translation_sigma = translation_variance.asDiagonal();
+      Eigen::Matrix3d rotation_sigma = rotation_variance.asDiagonal();
+      Eigen::Vector3d delta_rotation =
+          PoseUtils::RotationMatrixToEulerAngles(H_delta.rotation().matrix());
+      double max_translation_z_score = std::sqrt(
+          H_delta.translation()
+              .cwiseProduct(translation_sigma.inverse() * H_delta.translation())
+              .maxCoeff());
+      double max_rotation_z_score = std::sqrt(static_cast<double>(
+          (delta_rotation.array() *
+           (rotation_sigma.inverse() * delta_rotation).array())
+              .maxCoeff()));
+      double z_score = std::max(max_translation_z_score, max_rotation_z_score);
+      // Remove observations that vary significantly from the mean
+      if (z_score > absl::GetFlag(FLAGS_outlier_std_devs)) {
+        VLOG(1) << "Removing outlier with z_score " << z_score
+                << " relative to std dev = "
+                << absl::GetFlag(FLAGS_outlier_std_devs);
+        return true;
+      }
+      return false;
+    };
+    pose_list.erase(
+        std::remove_if(pose_list.begin(), pose_list.end(), remove_lambda),
+        pose_list.end());
+
+    VLOG(1) << "Iteration #" << i << ": removed "
+            << (original_size - pose_list.size())
+            << " outlier constraints out of " << original_size
+            << " total\nStd Dev's are: "
+            << translation_variance.array().sqrt().transpose() << "m and "
+            << rotation_variance.array().sqrt().transpose() * 180.0 / M_PI
+            << "deg";
+    if (original_size - pose_list.size() == 0) {
+      VLOG(1) << "At step " << i
+              << ", ending outlier rejection early due to convergence at "
+              << pose_list.size() << " elements.\nStd Dev's are: "
+              << translation_variance.array().sqrt().transpose() << "m and "
+              << rotation_variance.array().sqrt().transpose() * 180.0 / M_PI
+              << "deg";
+      break;
+    }
+  }
+}
+
+static std::map<std::string, aos::distributed_clock::time_point>
+    last_eofs_debug;
+int display_count = 1;
+
+// Take in list of poses from a camera observation and add to running list
+// One of two options:
+// 1) We see two boards in one view-- store this to get an estimate of
+// the offset between the two boards
+// 2) We see just one board-- save this and try to pair it with a previous
+// observation from another camera
 void HandlePoses(cv::Mat rgb_image,
                  std::vector<TargetMapper::TargetPose> target_poses,
                  aos::distributed_clock::time_point distributed_eof,
                  std::string camera_name) {
-  // This is used to transform points for visualization
+  // This (H_world_board) is used to transform points for visualization
   // Assumes targets are aligned with x->right, y->up, z->out of board
   Eigen::Affine3d H_world_board;
   H_world_board = Eigen::Translation3d::Identity() *
@@ -229,6 +256,15 @@
     // Store this observation of the transform between two boards
     two_board_extrinsics_list.push_back(boardA_boardB);
 
+    // Also, store one of the observations, so we can potentially
+    // match against the next single-target observation that comes in
+    TimestampedCameraDetection new_observation{
+        .time = distributed_eof,
+        .H_camera_target = H_camera_boardA,
+        .camera_name = camera_name,
+        .board_id = target_poses[from_index].id};
+    last_observation = new_observation;
+
     if (absl::GetFlag(FLAGS_visualize)) {
       vis_robot_.DrawFrameAxes(
           H_world_board,
@@ -252,8 +288,9 @@
         .board_id = target_poses[0].id};
 
     // Only take two observations if they're within half an image cycle of
-    // each other (i.e., as close in time as possible) And, if two consecutive
-    // observations are from the same camera, just replace with the newest one
+    // each other (i.e., as close in time as possible).  And, if two
+    // consecutive observations are from the same camera, just replace with
+    // the newest one
     if ((new_observation.camera_name != last_observation.camera_name) &&
         (std::abs((distributed_eof - last_observation.time).count()) <
          kImagePeriodMs / 2.0 * 1000000.0)) {
@@ -271,44 +308,51 @@
       detection_list.push_back(new_pair);
 
       // This bit is just for visualization and checking purposes-- use the
-      // last two-board observation to figure out the current estimate
-      // between the two cameras
+      // first two-board observation to figure out the current estimate
+      // between the two cameras (this could be incorrect, but it keeps it
+      // constant)
       if (absl::GetFlag(FLAGS_visualize) &&
           two_board_extrinsics_list.size() > 0) {
         draw_vis = true;
-        TimestampedCameraDetection &last_two_board_ext =
-            two_board_extrinsics_list.back();
-        Eigen::Affine3d &H_boardA_boardB = last_two_board_ext.H_camera_target;
-        int boardA_boardB_id = last_two_board_ext.board_id;
+        TimestampedCameraDetection &first_two_board_ext =
+            two_board_extrinsics_list.front();
+        Eigen::Affine3d &H_boardA_boardB = first_two_board_ext.H_camera_target;
+        int boardA_id = first_two_board_ext.board_id;
 
         TimestampedCameraDetection camera1_boardA = new_pair.first;
-        TimestampedCameraDetection camera2_boardB = new_pair.second;
-        // If camera1_boardA doesn't point to the first target, then swap
-        // these two
-        if (camera1_boardA.board_id != boardA_boardB_id) {
-          camera1_boardA = new_pair.second;
-          camera2_boardB = new_pair.first;
+        TimestampedCameraDetection camera2_boardA = new_pair.second;
+        Eigen::Affine3d H_camera1_boardA = camera1_boardA.H_camera_target;
+        Eigen::Affine3d H_camera2_boardA = camera2_boardA.H_camera_target;
+        // If camera1_boardA doesn't currently point to boardA, then fix it
+        if (camera1_boardA.board_id != boardA_id) {
+          H_camera1_boardA = H_camera1_boardA * H_boardA_boardB.inverse();
+        }
+        // If camera2_boardA doesn't currently point to boardA, then fix it
+        if (camera2_boardA.board_id != boardA_id) {
+          H_camera2_boardA = H_camera2_boardA * H_boardA_boardB.inverse();
         }
         VLOG(1) << "Camera " << camera1_boardA.camera_name << " seeing board "
                 << camera1_boardA.board_id << " and camera "
-                << camera2_boardB.camera_name << " seeing board "
-                << camera2_boardB.board_id;
+                << camera2_boardA.camera_name << " seeing board "
+                << camera2_boardA.board_id;
 
-        vis_robot_.DrawRobotOutline(
-            H_world_board * camera1_boardA.H_camera_target.inverse(),
-            camera1_boardA.camera_name,
-            kOrinColors.at(camera1_boardA.camera_name));
-        vis_robot_.DrawRobotOutline(
-            H_world_board * H_boardA_boardB *
-                camera2_boardB.H_camera_target.inverse(),
-            camera2_boardB.camera_name,
-            kOrinColors.at(camera2_boardB.camera_name));
+        // Draw the two poses of the cameras, and the locations of the
+        // boards We use "Board A" as the origin (with everything relative
+        // to H_world_board)
+        vis_robot_.DrawRobotOutline(H_world_board * H_camera1_boardA.inverse(),
+                                    camera1_boardA.camera_name,
+                                    kOrinColors.at(camera1_boardA.camera_name));
+        vis_robot_.DrawRobotOutline(H_world_board * H_camera2_boardA.inverse(),
+                                    camera2_boardA.camera_name,
+                                    kOrinColors.at(camera2_boardA.camera_name));
         vis_robot_.DrawFrameAxes(
             H_world_board,
-            std::string("Board ") + std::to_string(last_two_board_ext.board_id),
+            std::string("Board ") +
+                std::to_string(first_two_board_ext.board_id),
             cv::Scalar(0, 255, 0));
         vis_robot_.DrawFrameAxes(H_world_board * H_boardA_boardB, "Board B",
                                  cv::Scalar(255, 0, 0));
+
         VLOG(1) << "Storing observation between " << new_pair.first.camera_name
                 << ", target " << new_pair.first.board_id << " and "
                 << new_pair.second.camera_name << ", target "
@@ -341,6 +385,10 @@
     }
 
     if (draw_vis) {
+      cv::putText(vis_robot_.image_,
+                  "Display #" + std::to_string(display_count++),
+                  cv::Point(600, 10), cv::FONT_HERSHEY_PLAIN, 1.0,
+                  cv::Scalar(255, 255, 255));
       cv::imshow("Overhead View", vis_robot_.image_);
       cv::waitKey(absl::GetFlag(FLAGS_wait_key));
       vis_robot_.ClearImage();
@@ -355,6 +403,14 @@
   // Create empty RGB image in this case
   cv::Mat rgb_image;
   std::vector<TargetMapper::TargetPose> target_poses;
+  VLOG(1) << ": Diff since last image from " << camera_name << " is "
+          << (distributed_eof - last_eofs_debug.at(camera_name)).count() /
+                 1000000.0
+          << "ms";
+
+  if (last_eofs_debug.find(camera_name) == last_eofs_debug.end()) {
+    last_eofs_debug[camera_name] = distributed_eof;
+  }
 
   for (const auto *target_pose_fbs : *map.target_poses()) {
     // Skip detections with invalid ids
@@ -384,7 +440,7 @@
     }
 
     const TargetMapper::TargetPose target_pose =
-        PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
+        TargetMapper::TargetPoseFromFbs(*target_pose_fbs);
 
     target_poses.emplace_back(target_pose);
 
@@ -393,7 +449,13 @@
     VLOG(1) << camera_name << " saw target " << target_pose.id
             << " from TargetMap at timestamp " << distributed_eof
             << " with pose = " << H_camera_target.matrix();
+    LOG(INFO) << "pose info for target " << target_pose_fbs->id()
+              << ": \nconfidence: " << target_pose_fbs->confidence()
+              << ", pose_error: " << target_pose_fbs->pose_error()
+              << ", pose_error_ratio: " << target_pose_fbs->pose_error_ratio()
+              << ", dist_factor: " << target_pose_fbs->distortion_factor();
   }
+  last_eofs_debug[camera_name] = distributed_eof;
   HandlePoses(rgb_image, target_poses, distributed_eof, camera_name);
 }
 
@@ -432,12 +494,59 @@
   HandlePoses(rgb_image, target_poses, distributed_eof, camera_name);
 }
 
+void WriteExtrinsicFile(Eigen::Affine3d extrinsic, CameraNode camera_node,
+                        const calibration::CameraCalibration *original_cal) {
+  // Write out this extrinsic to a file
+  flatbuffers::FlatBufferBuilder fbb;
+  flatbuffers::Offset<flatbuffers::Vector<float>> data_offset =
+      fbb.CreateVector(frc971::vision::MatrixToVector(extrinsic.matrix()));
+  calibration::TransformationMatrix::Builder matrix_builder(fbb);
+  matrix_builder.add_data(data_offset);
+  flatbuffers::Offset<calibration::TransformationMatrix>
+      extrinsic_calibration_offset = matrix_builder.Finish();
+
+  calibration::CameraCalibration::Builder calibration_builder(fbb);
+  calibration_builder.add_fixed_extrinsics(extrinsic_calibration_offset);
+  const aos::realtime_clock::time_point realtime_now =
+      aos::realtime_clock::now();
+  calibration_builder.add_calibration_timestamp(
+      realtime_now.time_since_epoch().count());
+  fbb.Finish(calibration_builder.Finish());
+  aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+      solved_extrinsics = fbb.Release();
+
+  aos::FlatbufferDetachedBuffer<frc971::vision::calibration::CameraCalibration>
+      cal_copy = aos::RecursiveCopyFlatBuffer(original_cal);
+  cal_copy.mutable_message()->clear_fixed_extrinsics();
+  cal_copy.mutable_message()->clear_calibration_timestamp();
+  aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+      merged_calibration = aos::MergeFlatBuffers(&cal_copy.message(),
+                                                 &solved_extrinsics.message());
+
+  std::stringstream time_ss;
+  time_ss << realtime_now;
+
+  const std::string calibration_filename = frc971::vision::CalibrationFilename(
+      absl::GetFlag(FLAGS_output_folder), camera_node.node_name,
+      absl::GetFlag(FLAGS_team_number), camera_node.camera_number,
+      cal_copy.message().camera_id()->data(), time_ss.str());
+  LOG(INFO) << calibration_filename << " -> "
+            << aos::FlatbufferToJson(merged_calibration, {.multi_line = true});
+
+  aos::util::WriteStringToFileOrDie(
+      calibration_filename,
+      aos::FlatbufferToJson(merged_calibration, {.multi_line = true}));
+}
+
 void ExtrinsicsMain(int argc, char *argv[]) {
   vis_robot_ = VisualizeRobot(cv::Size(1000, 1000));
   vis_robot_.ClearImage();
   const double kFocalLength = 1000.0;
   const int kImageWidth = 1000;
   vis_robot_.SetDefaultViewpoint(kImageWidth, kFocalLength);
+  LOG(INFO) << "COPYTHIS, count, camera_name, target_id, timestamp, mag_T, "
+               "mag_R_deg, "
+               "confidence, pose_error, pose_error_ratio, distortion_factor";
 
   std::optional<aos::FlatbufferDetachedBuffer<aos::Configuration>> config =
       (absl::GetFlag(FLAGS_config).empty()
@@ -467,7 +576,6 @@
 
   std::vector<std::unique_ptr<aos::EventLoop>> detection_event_loops;
   std::vector<frc971::vision::CharucoExtractor *> charuco_extractors;
-  std::vector<frc971::vision::ImageCallback *> image_callbacks;
   std::vector<Eigen::Affine3d> default_extrinsics;
 
   for (const CameraNode &camera_node : node_list) {
@@ -514,17 +622,50 @@
         });
     VLOG(1) << "Created watcher for using the detection event loop for "
             << camera_node.camera_name();
+
+    // Display images, if they exist
+    std::string camera_name = camera_node.camera_name();
+    detection_event_loops.back()->MakeWatcher(
+        camera_name.c_str(),
+        [camera_name](const frc971::vision::CameraImage &image) {
+          cv::Mat image_color_mat(cv::Size(image.cols(), image.rows()), CV_8UC2,
+                                  (void *)image.data()->data());
+          cv::Mat bgr_image(cv::Size(image.cols(), image.rows()), CV_8UC3);
+          cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);
+          cv::resize(bgr_image, bgr_image, cv::Size(500, 500));
+          cv::imshow(camera_name.c_str(), bgr_image);
+          cv::waitKey(1);
+        });
   }
 
   reader.event_loop_factory()->Run();
 
-  // Do quick check to see what averaged two-board pose for
-  // each camera is individually
   CHECK_GT(two_board_extrinsics_list.size(), 0u)
       << "Must have at least one view of both boards";
   int base_target_id = two_board_extrinsics_list[0].board_id;
   VLOG(1) << "Base id for two_board_extrinsics_list is " << base_target_id;
 
+  int pre_outlier_num = two_board_extrinsics_list.size();
+  RemoveOutliers(two_board_extrinsics_list);
+
+  LOG(INFO) << "Started with " << pre_outlier_num
+            << " observations.  After OUTLIER rejection, "
+            << two_board_extrinsics_list.size() << " observations remaining";
+  Eigen::Affine3d H_boardA_boardB_avg =
+      ComputeAveragePose(two_board_extrinsics_list);
+  LOG(INFO) << "Estimate of two board pose using all nodes with "
+            << two_board_extrinsics_list.size() << " observations is:\n"
+            << H_boardA_boardB_avg.matrix() << "\nOr translation of "
+            << H_boardA_boardB_avg.translation().transpose()
+            << " (m) and rotation (r,p,y) of "
+            << PoseUtils::RotationMatrixToEulerAngles(
+                   H_boardA_boardB_avg.rotation().matrix())
+                       .transpose() *
+                   180.0 / M_PI
+            << " (deg)";
+
+  // Do quick check to see what averaged two-board pose for
+  // each camera is individually, and compare with overall average
   for (auto camera_node : node_list) {
     std::vector<TimestampedCameraDetection> pose_list;
     for (auto ext : two_board_extrinsics_list) {
@@ -534,32 +675,74 @@
         pose_list.push_back(ext);
       }
     }
-    Eigen::Affine3d avg_pose_from_camera = ComputeAveragePose(pose_list);
-    VLOG(1) << "Estimate from " << camera_node.camera_name() << " with "
-            << pose_list.size() << " observations is:\n"
-            << avg_pose_from_camera.matrix();
+    CHECK(pose_list.size() > 0)
+        << "Didn't get any two_board extrinsics for camera "
+        << camera_node.camera_name();
+    Eigen::Vector3d translation_variance, rotation_variance;
+    Eigen::Affine3d avg_pose_from_camera = ComputeAveragePose(
+        pose_list, &translation_variance, &rotation_variance);
+
+    Eigen::Vector3d translation_std_dev = translation_variance.array().sqrt();
+    LOG(INFO) << camera_node.camera_name() << " has average pose from "
+              << pose_list.size() << " views of two targets of \n"
+              << avg_pose_from_camera.matrix()
+              << "\nTranslation standard deviation is "
+              << translation_std_dev.transpose();
+    double stdev_norm = translation_std_dev.norm();
+    double threshold = 0.03;  // 3 cm threshold on translation variation
+    if (stdev_norm > threshold) {
+      LOG(INFO) << "WARNING: |STD_DEV| is " << stdev_norm * 100 << " > "
+                << threshold * 100 << " cm!!!!\nStd dev vector (in m) is "
+                << translation_std_dev.transpose();
+    }
+
+    Eigen::Vector3d rotation_std_dev = rotation_variance.array().sqrt();
+    LOG(INFO) << camera_node.camera_name()
+              << " with rotational standard deviation of: "
+              << rotation_std_dev.transpose() << " (radians)";
+    double rot_stdev_norm = rotation_std_dev.norm();
+    double rot_threshold = 3 * M_PI / 180.0;  // Warn if more than 3 degrees
+    if (rot_stdev_norm > rot_threshold) {
+      LOG(INFO) << "WARNING: ROTATIONAL STD DEV is "
+                << rot_stdev_norm * 180.0 / M_PI << " > "
+                << rot_threshold * 180.0 / M_PI
+                << " degrees!!!!\nStd dev vector (in deg) is "
+                << (rotation_std_dev * 180.0 / M_PI).transpose();
+    }
+    // Check if a particular camera deviates significantly from the overall
+    // average Any of these factors could indicate a problem with that camera
+    Eigen::Affine3d delta_from_overall =
+        H_boardA_boardB_avg * avg_pose_from_camera.inverse();
+    LOG(INFO) << camera_node.camera_name()
+              << " had estimate different from pooled average of\n"
+              << "|dT| = " << delta_from_overall.translation().norm()
+              << "m  and |dR| = "
+              << (PoseUtils::RotationMatrixToEulerAngles(
+                      delta_from_overall.rotation().matrix()) *
+                  180.0 / M_PI)
+                     .norm()
+              << " deg";
   }
-  Eigen::Affine3d H_boardA_boardB_avg =
-      ComputeAveragePose(two_board_extrinsics_list);
-  // TODO: Should probably do some outlier rejection
-  VLOG(1) << "Estimate of two board pose using all nodes with "
-          << two_board_extrinsics_list.size() << " observations is:\n"
-          << H_boardA_boardB_avg.matrix() << "\n";
 
   // Next, compute the relative camera poses
   LOG(INFO) << "Got " << detection_list.size() << " extrinsic observations";
-  std::vector<Eigen::Affine3d> H_camera1_camera2_list;
+  std::vector<TimestampedCameraDetection> H_camera1_camera2_list;
   std::vector<Eigen::Affine3d> updated_extrinsics;
   // Use the first node's extrinsics as our base, and fix from there
   updated_extrinsics.push_back(default_extrinsics[0]);
   LOG(INFO) << "Default extrinsic for camera " << node_list.at(0).camera_name()
-            << " is " << default_extrinsics[0].matrix();
+            << " is\n"
+            << default_extrinsics[0].matrix();
   for (uint i = 0; i < node_list.size() - 1; i++) {
     H_camera1_camera2_list.clear();
     // Go through the list, and find successive pairs of
-    // cameras We'll be calculating and writing the second
-    // of the pair (the i+1'th camera)
+    // cameras. We'll be calculating and writing the second
+    // of the pair's (the i+1'th camera) extrinsics
     for (auto [pose1, pose2] : detection_list) {
+      // Note that this assumes our poses are ordered by the node_list
+      CHECK(!((pose1.camera_name == node_list.at(i + 1).camera_name()) &&
+              (pose2.camera_name == node_list.at(i).camera_name())))
+          << "The camera ordering on our detections is incorrect!";
       if ((pose1.camera_name == node_list.at(i).camera_name()) &&
           (pose2.camera_name == node_list.at(i + 1).camera_name())) {
         Eigen::Affine3d H_camera1_boardA = pose1.H_camera_target;
@@ -584,7 +767,12 @@
         // Compute camera1->camera2 map
         Eigen::Affine3d H_camera1_camera2 =
             H_camera1_boardA * H_camera2_boardA.inverse();
-        H_camera1_camera2_list.push_back(H_camera1_camera2);
+        // Set the list up as TimestampedCameraDetection's to use the outlier
+        // removal function
+        TimestampedCameraDetection camera1_camera2{
+            .H_camera_target = H_camera1_camera2,
+            .camera_name = node_list.at(i + 1).camera_name()};
+        H_camera1_camera2_list.push_back(camera1_camera2);
         VLOG(1) << "Map from camera " << pose1.camera_name << " and tag "
                 << pose1.board_id << " with observation: \n"
                 << pose1.H_camera_target.matrix() << "\n to camera "
@@ -619,6 +807,17 @@
                 << H_camera1_camera2_list.size()
                 << " observations, and the average pose is:\n"
                 << H_camera1_camera2_avg.matrix();
+
+      RemoveOutliers(H_camera1_camera2_list);
+
+      H_camera1_camera2_avg = ComputeAveragePose(H_camera1_camera2_list);
+      LOG(INFO) << "After outlier rejection, from "
+                << node_list.at(i).camera_name() << " to "
+                << node_list.at(i + 1).camera_name() << " found "
+                << H_camera1_camera2_list.size()
+                << " observations, and the average pose is:\n"
+                << H_camera1_camera2_avg.matrix();
+
       Eigen::Affine3d H_camera1_camera2_default =
           default_extrinsics[i].inverse() * default_extrinsics[i + 1];
       LOG(INFO) << "Compare this to that from default values:\n"
@@ -634,6 +833,7 @@
           << Eigen::AngleAxisd(H_camera1_camera2_diff.rotation()).angle() *
                  180.0 / M_PI
           << " degrees)";
+
       // Next extrinsic is just previous one * avg_delta_pose
       Eigen::Affine3d next_extrinsic =
           updated_extrinsics.back() * H_camera1_camera2_avg;
@@ -645,51 +845,8 @@
                 << node_list.at(i + 1).camera_name() << " is \n"
                 << next_extrinsic.matrix();
 
-      // Write out this extrinsic to a file
-      flatbuffers::FlatBufferBuilder fbb;
-      flatbuffers::Offset<flatbuffers::Vector<float>> data_offset =
-          fbb.CreateVector(
-              frc971::vision::MatrixToVector(next_extrinsic.matrix()));
-      calibration::TransformationMatrix::Builder matrix_builder(fbb);
-      matrix_builder.add_data(data_offset);
-      flatbuffers::Offset<calibration::TransformationMatrix>
-          extrinsic_calibration_offset = matrix_builder.Finish();
-
-      calibration::CameraCalibration::Builder calibration_builder(fbb);
-      calibration_builder.add_fixed_extrinsics(extrinsic_calibration_offset);
-      const aos::realtime_clock::time_point realtime_now =
-          aos::realtime_clock::now();
-      calibration_builder.add_calibration_timestamp(
-          realtime_now.time_since_epoch().count());
-      fbb.Finish(calibration_builder.Finish());
-      aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
-          solved_extrinsics = fbb.Release();
-
-      aos::FlatbufferDetachedBuffer<
-          frc971::vision::calibration::CameraCalibration>
-          cal_copy = aos::RecursiveCopyFlatBuffer(calibration_list[i + 1]);
-      cal_copy.mutable_message()->clear_fixed_extrinsics();
-      cal_copy.mutable_message()->clear_calibration_timestamp();
-      aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
-          merged_calibration = aos::MergeFlatBuffers(
-              &cal_copy.message(), &solved_extrinsics.message());
-
-      std::stringstream time_ss;
-      time_ss << realtime_now;
-
-      CameraNode &camera_node = node_list[i + 1];
-      const std::string calibration_filename =
-          frc971::vision::CalibrationFilename(
-              absl::GetFlag(FLAGS_output_folder), camera_node.node_name,
-              absl::GetFlag(FLAGS_team_number), camera_node.camera_number,
-              cal_copy.message().camera_id()->data(), time_ss.str());
-      LOG(INFO) << calibration_filename << " -> "
-                << aos::FlatbufferToJson(merged_calibration,
-                                         {.multi_line = true});
-
-      aos::util::WriteStringToFileOrDie(
-          calibration_filename,
-          aos::FlatbufferToJson(merged_calibration, {.multi_line = true}));
+      WriteExtrinsicFile(next_extrinsic, node_list[i + 1],
+                         calibration_list[i + 1]);
 
       if (absl::GetFlag(FLAGS_visualize)) {
         // Draw each of the updated extrinsic camera locations
@@ -712,9 +869,8 @@
   }
 
   // Cleanup
-  for (uint i = 0; i < image_callbacks.size(); i++) {
+  for (uint i = 0; i < charuco_extractors.size(); i++) {
     delete charuco_extractors[i];
-    delete image_callbacks[i];
   }
 }
 }  // namespace y2024::vision
diff --git a/y2024/vision/target_mapping.cc b/y2024/vision/target_mapping.cc
index f5fa34c..d9b067a 100644
--- a/y2024/vision/target_mapping.cc
+++ b/y2024/vision/target_mapping.cc
@@ -266,7 +266,7 @@
     }
 
     const TargetMapper::TargetPose target_pose =
-        PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
+        TargetMapper::TargetPoseFromFbs(*target_pose_fbs);
 
     Eigen::Affine3d H_camera_target =
         Eigen::Translation3d(target_pose.pose.p) * target_pose.pose.q;
diff --git a/y2024/vision/viewer.cc b/y2024/vision/viewer.cc
index c72c08e..c741445 100644
--- a/y2024/vision/viewer.cc
+++ b/y2024/vision/viewer.cc
@@ -86,7 +86,9 @@
 
   frc971::constants::ConstantsFetcher<y2024::Constants> constants_fetcher(
       &event_loop);
-  CHECK(absl::GetFlag(FLAGS_channel).length() == 8);
+  CHECK(absl::GetFlag(FLAGS_channel).length() == 8)
+      << " channel should be of the form '/cameraN' for viewing images from "
+         "camera N";
   int camera_id = std::stoi(absl::GetFlag(FLAGS_channel).substr(7, 1));
   const auto *calibration_data = FindCameraCalibration(
       constants_fetcher.constants(), event_loop.node()->name()->string_view(),
diff --git a/y2024_bot3/constants/9971.json b/y2024_bot3/constants/9971.json
index 5583b44..4eb1acf 100644
--- a/y2024_bot3/constants/9971.json
+++ b/y2024_bot3/constants/9971.json
@@ -1,3 +1,5 @@
+{% from 'y2024_bot3/constants/common.jinja2' import arm_zero %}
+
 {
   "cameras": [
     {
@@ -13,6 +15,21 @@
       "calibration": {% include 'y2024_bot3/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json' %}
     }
   ],
-  "robot": {},
+  "robot": {
+    "arm_constants": {
+      {% set _ = arm_zero.update(
+          {
+              "measured_absolute_position" : 0.0992895926495078
+          }
+      ) %}
+      "zeroing_constants": {{ arm_zero | tojson(indent=2)}},
+      "potentiometer_offset": {{ 0 }},
+      "arm_positions": {
+        "intake": 0,
+        "idle": 1,
+        "amp": 2
+      }
+    }
+  },
   {% include 'y2024_bot3/constants/common.json' %}
 }
diff --git a/y2024_bot3/constants/BUILD b/y2024_bot3/constants/BUILD
index 1ce69a0..323bd49 100644
--- a/y2024_bot3/constants/BUILD
+++ b/y2024_bot3/constants/BUILD
@@ -26,6 +26,7 @@
         "common.jinja2",
         "common.json",
         "//y2024_bot3/constants/calib_files",
+        "//y2024_bot3/control_loops/superstructure/arm:arm_json",
         "//y2024_bot3/vision/maps",
     ],
     parameters = {},
@@ -40,6 +41,7 @@
         "common.jinja2",
         "common.json",
         "//y2024_bot3/constants/calib_files",
+        "//y2024_bot3/control_loops/superstructure/arm:arm_json",
         "//y2024_bot3/vision/maps",
     ],
     parameters = {},
diff --git a/y2024_bot3/constants/common.jinja2 b/y2024_bot3/constants/common.jinja2
index 9b7af82..809f8c8 100644
--- a/y2024_bot3/constants/common.jinja2
+++ b/y2024_bot3/constants/common.jinja2
@@ -1,3 +1,16 @@
 {% set pi = 3.14159265 %}
 
 {%set zeroing_sample_size = 200 %}
+
+{# Arm #}
+{% set arm_encoder_ratio = (4.0 / 1.0) %}
+
+{%
+set arm_zero = {
+    "average_filter_size": zeroing_sample_size,
+    "one_revolution_distance": pi * 2.0 * arm_encoder_ratio,
+    "zeroing_threshold": 0.0005,
+    "moving_buffer_size": 20,
+    "allowable_encoder_error": 0.9
+}
+%}
diff --git a/y2024_bot3/constants/common.json b/y2024_bot3/constants/common.json
index e7b70b4..d5abec0 100644
--- a/y2024_bot3/constants/common.json
+++ b/y2024_bot3/constants/common.json
@@ -1,2 +1,21 @@
 "common": {
+  "arm": {
+    "zeroing_voltage": 3.0,
+    "operating_voltage": 12.0,
+    "zeroing_profile_params": {
+      "max_velocity": 0.5,
+      "max_acceleration": 3.0
+    },
+    "default_profile_params":{
+      "max_velocity": 2.0,
+      "max_acceleration": 10.0
+    },
+    "range": {
+        "lower_hard": -50,
+        "upper_hard": 50,
+        "lower": -50,
+        "upper": 50
+    },
+    "loop": {% include 'y2024_bot3/control_loops/superstructure/arm/integral_arm_plant.json' %}
+  }
 }
diff --git a/y2024_bot3/constants/constants.fbs b/y2024_bot3/constants/constants.fbs
index 0c7a792..99999bd 100644
--- a/y2024_bot3/constants/constants.fbs
+++ b/y2024_bot3/constants/constants.fbs
@@ -10,12 +10,26 @@
   calibration:frc971.vision.calibration.CameraCalibration (id: 0);
 }
 
+table ArmPositions {
+  intake:double (id: 0);
+  idle:double (id: 1);
+  amp:double (id: 2);
+}
+
+table PotAndAbsEncoderConstants {
+  zeroing_constants:frc971.zeroing.PotAndAbsoluteEncoderZeroingConstants (id: 0);
+  potentiometer_offset:double (id: 1);
+  arm_positions:ArmPositions (id: 2);
+}
+
 table RobotConstants {
+  arm_constants:PotAndAbsEncoderConstants (id: 0);
 }
 
 // Common table for constants unrelated to the robot
 table Common {
   target_map:frc971.vision.TargetMap (id: 0);
+  arm:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemCommonParams (id: 1);
 }
 
 table Constants {
diff --git a/y2024_bot3/constants/test_data/test_team.json b/y2024_bot3/constants/test_data/test_team.json
index deacef4..06286e7 100644
--- a/y2024_bot3/constants/test_data/test_team.json
+++ b/y2024_bot3/constants/test_data/test_team.json
@@ -1,4 +1,21 @@
+{% from 'y2024_bot3/constants/common.jinja2' import arm_zero %}
+
 {
-  "robot": {},
+  "robot": {
+    "arm_constants": {
+      {% set _ = arm_zero.update(
+          {
+              "measured_absolute_position" : 0.0992895926495078
+          }
+      ) %}
+      "zeroing_constants": {{ arm_zero | tojson(indent=2)}},
+      "potentiometer_offset": {{ 0 }},
+      "arm_positions": {
+        "intake": 0,
+        "idle": 1,
+        "amp": 2
+      }
+    }
+  },
   {% include 'y2024_bot3/constants/common.json' %}
 }
diff --git a/y2024_bot3/control_loops/python/BUILD b/y2024_bot3/control_loops/python/BUILD
index 9ec6b57..853719d 100644
--- a/y2024_bot3/control_loops/python/BUILD
+++ b/y2024_bot3/control_loops/python/BUILD
@@ -6,3 +6,19 @@
     visibility = ["//visibility:public"],
     deps = ["//y2024_bot3/control_loops:python_init"],
 )
+
+py_binary(
+    name = "arm",
+    srcs = [
+        "arm.py",
+    ],
+    legacy_create_init = False,
+    target_compatible_with = ["@platforms//cpu:x86_64"],
+    deps = [
+        ":python_init",
+        "//frc971/control_loops/python:controls",
+        "//frc971/control_loops/python:linear_system",
+        "@pip//glog",
+        "@pip//python_gflags",
+    ],
+)
diff --git a/y2024_bot3/control_loops/python/arm.py b/y2024_bot3/control_loops/python/arm.py
new file mode 100644
index 0000000..ef02be8
--- /dev/null
+++ b/y2024_bot3/control_loops/python/arm.py
@@ -0,0 +1,61 @@
+#!/usr/bin/python3
+
+from aos.util.trapezoid_profile import TrapezoidProfile
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import linear_system
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+    gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+    pass
+
+gflags.DEFINE_bool('hybrid', False, 'If true, make it hybrid.')
+
+kArm = linear_system.LinearSystemParams(
+    name='Arm',
+    motor=control_loop.KrakenFOC(),
+    G=(14. / 60.) * (32. / 48.),
+    radius=36 * 0.005 / numpy.pi / 2.0,
+    mass=5.0,
+    q_pos=0.20,
+    q_vel=80.0,
+    kalman_q_pos=0.12,
+    kalman_q_vel=2.0,
+    kalman_q_voltage=8.0,
+    kalman_r_position=0.05,
+    dt=0.005,
+)
+
+
+def main(argv):
+    if FLAGS.plot:
+        R = numpy.matrix([[0.4], [0.0]])
+        linear_system.PlotKick(kArm, R)
+        linear_system.PlotMotion(kArm,
+                                 R,
+                                 max_velocity=2.0,
+                                 max_acceleration=15.0)
+        return
+
+    # Write the generated constants out to a file.
+    if len(argv) != 7:
+        glog.fatal(
+            'Expected .h file name and .cc file name for the arm pivot and integral arm pivot.'
+        )
+    else:
+        namespaces = ['y2024_bot3', 'control_loops', 'superstructure', 'arm']
+        linear_system.WriteLinearSystem(kArm, argv[1:4], argv[4:7], namespaces)
+
+
+if __name__ == '__main__':
+    argv = FLAGS(sys.argv)
+    glog.init()
+    sys.exit(main(argv))
diff --git a/y2024_bot3/control_loops/superstructure/BUILD b/y2024_bot3/control_loops/superstructure/BUILD
index 8d909bf..bcac518 100644
--- a/y2024_bot3/control_loops/superstructure/BUILD
+++ b/y2024_bot3/control_loops/superstructure/BUILD
@@ -142,6 +142,7 @@
         "//frc971/control_loops:position_sensor_sim",
         "//frc971/control_loops:subsystem_simulator",
         "//frc971/control_loops:team_number_test_environment",
+        "//y2024_bot3/control_loops/superstructure/arm:arm_plants",
     ],
 )
 
diff --git a/y2024_bot3/control_loops/superstructure/arm/BUILD b/y2024_bot3/control_loops/superstructure/arm/BUILD
new file mode 100644
index 0000000..d6d2fec
--- /dev/null
+++ b/y2024_bot3/control_loops/superstructure/arm/BUILD
@@ -0,0 +1,42 @@
+package(default_visibility = ["//y2024_bot3:__subpackages__"])
+
+genrule(
+    name = "genrule_arm",
+    outs = [
+        "arm_plant.h",
+        "arm_plant.cc",
+        "arm_plant.json",
+        "integral_arm_plant.h",
+        "integral_arm_plant.cc",
+        "integral_arm_plant.json",
+    ],
+    cmd = "$(location //y2024_bot3/control_loops/python:arm) $(OUTS)",
+    target_compatible_with = ["@platforms//os:linux"],
+    tools = [
+        "//y2024_bot3/control_loops/python:arm",
+    ],
+)
+
+cc_library(
+    name = "arm_plants",
+    srcs = [
+        "arm_plant.cc",
+        "integral_arm_plant.cc",
+    ],
+    hdrs = [
+        "arm_plant.h",
+        "integral_arm_plant.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//frc971/control_loops:hybrid_state_feedback_loop",
+        "//frc971/control_loops:state_feedback_loop",
+    ],
+)
+
+filegroup(
+    name = "arm_json",
+    srcs = ["integral_arm_plant.json"],
+    visibility = ["//visibility:public"],
+)
diff --git a/y2024_bot3/control_loops/superstructure/superstructure.cc b/y2024_bot3/control_loops/superstructure/superstructure.cc
index c18da15..2c510ad 100644
--- a/y2024_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2024_bot3/control_loops/superstructure/superstructure.cc
@@ -26,7 +26,9 @@
       constants_fetcher_(event_loop),
       robot_constants_(&constants_fetcher_.constants()),
       joystick_state_fetcher_(
-          event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
+          event_loop->MakeFetcher<aos::JoystickState>("/aos")),
+      arm_(robot_constants_->common()->arm(),
+           robot_constants_->robot()->arm_constants()->zeroing_constants()) {
   event_loop->SetRuntimeRealtimePriority(30);
 }
 
@@ -38,33 +40,71 @@
                                   const Position *position,
                                   aos::Sender<Output>::Builder *output,
                                   aos::Sender<Status>::Builder *status) {
-  (void)position;
-
   if (WasReset()) {
     AOS_LOG(ERROR, "WPILib reset, restarting\n");
   }
 
   OutputT output_struct;
 
-  if (unsafe_goal != nullptr) {
-  }
-
   if (joystick_state_fetcher_.Fetch() &&
       joystick_state_fetcher_->has_alliance()) {
     alliance_ = joystick_state_fetcher_->alliance();
   }
 
+  aos::FlatbufferFixedAllocatorArray<
+      frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
+      arm_goal_buffer;
+
+  ArmStatus arm_status = ArmStatus::IDLE;
+  double arm_position =
+      robot_constants_->robot()->arm_constants()->arm_positions()->idle();
+  if (unsafe_goal != nullptr) {
+    switch (unsafe_goal->arm_position()) {
+      case PivotGoal::INTAKE:
+        arm_status = ArmStatus::INTAKE;
+        arm_position = robot_constants_->robot()
+                           ->arm_constants()
+                           ->arm_positions()
+                           ->intake();
+        break;
+      case PivotGoal::AMP:
+        arm_status = ArmStatus::AMP;
+        arm_position =
+            robot_constants_->robot()->arm_constants()->arm_positions()->amp();
+        break;
+      default:
+        arm_position =
+            robot_constants_->robot()->arm_constants()->arm_positions()->idle();
+    }
+  }
+
+  arm_goal_buffer.Finish(
+      frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+          *arm_goal_buffer.fbb(), arm_position));
+
+  const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+      *arm_goal = &arm_goal_buffer.message();
+
+  // static_zeroing_single_dof_profiled_subsystem.h
+  const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+      arm_offset =
+          arm_.Iterate(arm_goal, position->arm(),
+                       output != nullptr ? &output_struct.arm_voltage : nullptr,
+                       status->fbb());
+
   if (output) {
     output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
   }
 
   Status::Builder status_builder = status->MakeBuilder<Status>();
 
-  const bool zeroed = true;
-  const bool estopped = false;
+  const bool zeroed = arm_.zeroed();
+  const bool estopped = arm_.estopped();
 
   status_builder.add_zeroed(zeroed);
   status_builder.add_estopped(estopped);
+  status_builder.add_arm(arm_offset);
+  status_builder.add_arm_status(arm_status);
 
   (void)status->Send(status_builder.Finish());
 }
diff --git a/y2024_bot3/control_loops/superstructure/superstructure.h b/y2024_bot3/control_loops/superstructure/superstructure.h
index 6362bc0..b7499c2 100644
--- a/y2024_bot3/control_loops/superstructure/superstructure.h
+++ b/y2024_bot3/control_loops/superstructure/superstructure.h
@@ -35,6 +35,8 @@
 
   double robot_velocity() const;
 
+  inline const PotAndAbsoluteEncoderSubsystem &arm() const { return arm_; }
+
  protected:
   virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
                             aos::Sender<Output>::Builder *output,
@@ -47,6 +49,8 @@
 
   aos::Alliance alliance_ = aos::Alliance::kInvalid;
 
+  PotAndAbsoluteEncoderSubsystem arm_;
+
   DISALLOW_COPY_AND_ASSIGN(Superstructure);
 };
 
diff --git a/y2024_bot3/control_loops/superstructure/superstructure_can_position.fbs b/y2024_bot3/control_loops/superstructure/superstructure_can_position.fbs
index 15a0565..d34cd9b 100644
--- a/y2024_bot3/control_loops/superstructure/superstructure_can_position.fbs
+++ b/y2024_bot3/control_loops/superstructure/superstructure_can_position.fbs
@@ -11,6 +11,12 @@
     // The ctre::phoenix::StatusCode of the measurement
     // Should be OK = 0
     status:int (id: 1);
+
+    // CAN Position of the roller falcon
+    intake_roller:frc971.control_loops.CANTalonFX (id: 2);
+
+    // CAN Position of the arm pivot falcon
+    arm_pivot:frc971.control_loops.CANTalonFX (id: 3);
 }
 
 root_type CANPosition;
diff --git a/y2024_bot3/control_loops/superstructure/superstructure_goal.fbs b/y2024_bot3/control_loops/superstructure/superstructure_goal.fbs
index 947f740..66ecfdf 100644
--- a/y2024_bot3/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2024_bot3/control_loops/superstructure/superstructure_goal.fbs
@@ -2,6 +2,33 @@
 
 namespace y2024_bot3.control_loops.superstructure;
 
-table Goal {
+// Represents goal for the intake rollers
+// INTAKE will turn on the rollers to intake the note.
+// SCORE will turn on the rollers to shoot the note.
+// SPIT will turn on the rollers (in reverse) to spit out the note.
+enum IntakeGoal : ubyte {
+    NONE = 0,
+    INTAKE = 1,
+    SCORE = 2,
+    SPIT = 3,
 }
+
+// Represents goal for the intake arm
+// IDLE will place the arm at the resting position.
+// INTAKE will place the arm at the intake position.
+// AMP will place the arm at the amp scoring position.
+enum PivotGoal: ubyte {
+    IDLE = 0,
+    INTAKE = 1,
+    AMP = 2,
+}
+
+table Goal {
+    // Intake roller goal
+    roller_goal: IntakeGoal = NONE (id: 0);
+
+    // Arm position goal
+    arm_position: PivotGoal (id: 1);
+}
+
 root_type Goal;
diff --git a/y2024_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2024_bot3/control_loops/superstructure/superstructure_lib_test.cc
index fe26af2..d9f684c 100644
--- a/y2024_bot3/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -12,6 +12,7 @@
 #include "frc971/control_loops/team_number_test_environment.h"
 #include "frc971/zeroing/absolute_encoder.h"
 #include "y2024_bot3/constants/simulated_constants_sender.h"
+#include "y2024_bot3/control_loops/superstructure/arm/arm_plant.h"
 #include "y2024_bot3/control_loops/superstructure/superstructure.h"
 #include "y2024_bot3/control_loops/superstructure/superstructure_can_position_generated.h"
 
@@ -56,10 +57,28 @@
         superstructure_status_fetcher_(
             event_loop_->MakeFetcher<Status>("/superstructure")),
         superstructure_output_fetcher_(
-            event_loop_->MakeFetcher<Output>("/superstructure")) {
-    (void)dt_;
-    (void)simulated_robot_constants;
+            event_loop_->MakeFetcher<Output>("/superstructure")),
+        arm_(new CappedTestPlant(arm::MakeArmPlant()),
+             PositionSensorSimulator(simulated_robot_constants->robot()
+                                         ->arm_constants()
+                                         ->zeroing_constants()
+                                         ->one_revolution_distance()),
+             {.subsystem_params = {simulated_robot_constants->common()->arm(),
+                                   simulated_robot_constants->robot()
+                                       ->arm_constants()
+                                       ->zeroing_constants()},
+              .potentiometer_offset = simulated_robot_constants->robot()
+                                          ->arm_constants()
+                                          ->potentiometer_offset()},
+             frc971::constants::Range::FromFlatbuffer(
+                 simulated_robot_constants->common()->arm()->range()),
+             simulated_robot_constants->robot()
+                 ->arm_constants()
+                 ->zeroing_constants()
+                 ->measured_absolute_position(),
+             dt_)
 
+  {
     phased_loop_handle_ = event_loop_->AddPhasedLoop(
         [this](int) {
           // Skip this the first time.
@@ -78,8 +97,15 @@
     ::aos::Sender<Position>::Builder builder =
         superstructure_position_sender_.MakeBuilder();
 
+    frc971::PotAndAbsolutePosition::Builder arm_builder =
+        builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+    flatbuffers::Offset<frc971::PotAndAbsolutePosition> arm_offset =
+        arm_.encoder()->GetSensorValues(&arm_builder);
+
     Position::Builder position_builder = builder.MakeBuilder<Position>();
 
+    position_builder.add_arm(arm_offset);
+
     CHECK_EQ(builder.Send(position_builder.Finish()),
              aos::RawSender::Error::kOk);
   }
@@ -94,6 +120,8 @@
   ::aos::Fetcher<Status> superstructure_status_fetcher_;
   ::aos::Fetcher<Output> superstructure_output_fetcher_;
 
+  PotAndAbsoluteEncoderSimulator arm_;
+
   bool first_ = true;
 };
 
@@ -154,6 +182,29 @@
         << ": No status";
     ASSERT_TRUE(superstructure_output_fetcher_.get() != nullptr)
         << ": No output";
+
+    double set_point;
+    auto arm_positions =
+        simulated_robot_constants_->robot()->arm_constants()->arm_positions();
+
+    switch (superstructure_goal_fetcher_->arm_position()) {
+      case PivotGoal::IDLE:
+        set_point = arm_positions->idle();
+        break;
+      case PivotGoal::INTAKE:
+        set_point = arm_positions->intake();
+        break;
+      case PivotGoal::AMP:
+        set_point = arm_positions->amp();
+        break;
+      default:
+        LOG(FATAL) << "PivotGoal is not IDLE, INTAKE, or AMP";
+    }
+
+    EXPECT_NEAR(
+        set_point,
+        superstructure_status_fetcher_->arm()->unprofiled_goal_position(),
+        0.03);
   }
 
   void CheckIfZeroed() {
@@ -211,7 +262,6 @@
 
 // Tests that the superstructure does nothing when the goal is to remain
 // still.
-
 TEST_F(SuperstructureTest, DoesNothing) {
   SetEnabled(true);
   WaitUntilZeroed();
@@ -240,4 +290,50 @@
   CheckIfZeroed();
 }
 
+TEST_F(SuperstructureTest, ArmPivotMovement) {
+  SetEnabled(true);
+  WaitUntilZeroed();
+
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_arm_position(PivotGoal::INTAKE);
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+  RunFor(chrono::seconds(20));
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  EXPECT_EQ(superstructure_status_fetcher_->arm_status(), ArmStatus::INTAKE);
+
+  VerifyNearGoal();
+
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_arm_position(PivotGoal::AMP);
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+  RunFor(chrono::seconds(10));
+
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  EXPECT_EQ(superstructure_status_fetcher_->arm_status(), ArmStatus::AMP);
+
+  VerifyNearGoal();
+
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_arm_position(PivotGoal::IDLE);
+
+    ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+  }
+  RunFor(chrono::seconds(10));
+
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  EXPECT_EQ(superstructure_status_fetcher_->arm_status(), ArmStatus::IDLE);
+
+  VerifyNearGoal();
+}
+
 }  // namespace y2024_bot3::control_loops::superstructure::testing
diff --git a/y2024_bot3/control_loops/superstructure/superstructure_output.fbs b/y2024_bot3/control_loops/superstructure/superstructure_output.fbs
index 28799de..faaa21e 100644
--- a/y2024_bot3/control_loops/superstructure/superstructure_output.fbs
+++ b/y2024_bot3/control_loops/superstructure/superstructure_output.fbs
@@ -1,6 +1,11 @@
 namespace y2024_bot3.control_loops.superstructure;
 
 table Output {
+    // Roller voltage, positive is for intake and scoring
+    roller_voltage: double (id: 0);
+
+    // Arm voltage, positive is moving arm up
+    arm_voltage: double (id: 1);
 }
 
 root_type Output;
diff --git a/y2024_bot3/control_loops/superstructure/superstructure_position.fbs b/y2024_bot3/control_loops/superstructure/superstructure_position.fbs
index f0553a5..6f2614b 100644
--- a/y2024_bot3/control_loops/superstructure/superstructure_position.fbs
+++ b/y2024_bot3/control_loops/superstructure/superstructure_position.fbs
@@ -4,6 +4,11 @@
 namespace y2024_bot3.control_loops.superstructure;
 
 table Position {
+    // Zero for the intake position value is horizontal, positive is up.
+    arm:frc971.PotAndAbsolutePosition (id: 0);
+
+    // Beambreak for the intake, 1 means note is present.
+    intake_beambreak:bool (id: 1);
 }
 
 root_type Position;
diff --git a/y2024_bot3/control_loops/superstructure/superstructure_status.fbs b/y2024_bot3/control_loops/superstructure/superstructure_status.fbs
index ca5876e..407af5f 100644
--- a/y2024_bot3/control_loops/superstructure/superstructure_status.fbs
+++ b/y2024_bot3/control_loops/superstructure/superstructure_status.fbs
@@ -3,12 +3,36 @@
 
 namespace y2024_bot3.control_loops.superstructure;
 
-table Status {
-  // All subsystems know their location.
-  zeroed:bool (id: 0);
+// Equivalent to IntakeGoal enum in goal flatbuffer
+enum IntakeRollerStatus : ubyte {
+    NONE = 0,
+    INTAKE = 1,
+    SCORE = 2,
+    SPIT = 3,
+}
 
-  // If true, we have aborted. This is the or of all subsystem estops.
-  estopped:bool (id: 1);
+// Equivalent to PivotGoal enum in goal flatbuffer
+enum ArmStatus: ubyte {
+    IDLE = 0,
+    INTAKE = 1,
+    AMP = 2,
+}
+
+table Status {
+    // All subsystems know their location.
+    zeroed:bool (id: 0);
+
+    // If true, we have aborted. This is the or of all subsystem estops.
+    estopped:bool (id: 1);
+
+    // Estimated Angles + Velocities of the Intake
+    arm:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 2);
+
+    // Tells us what IntakeGoal is
+    intake_roller_status: IntakeRollerStatus (id: 3);
+
+    // Tells us what PivotGoal is
+    arm_status: ArmStatus (id: 4);
 }
 
 root_type Status;
diff --git a/y2024_swerve/vision/target_mapping.cc b/y2024_swerve/vision/target_mapping.cc
index b8b1f62..dd33f11 100644
--- a/y2024_swerve/vision/target_mapping.cc
+++ b/y2024_swerve/vision/target_mapping.cc
@@ -268,7 +268,7 @@
     }
 
     const TargetMapper::TargetPose target_pose =
-        PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
+        TargetMapper::TargetPoseFromFbs(*target_pose_fbs);
 
     Eigen::Affine3d H_camera_target =
         Eigen::Translation3d(target_pose.pose.p) * target_pose.pose.q;