diff --git a/scripts/copy_images_in_range.py b/scripts/copy_images_in_range.py
new file mode 100755
index 00000000..68b43222
--- /dev/null
+++ b/scripts/copy_images_in_range.py
@@ -0,0 +1,115 @@
+#!/usr/bin/env python3
+from __future__ import annotations
+
+import argparse
+import os
+import re
+import shutil
+import sys
+from pathlib import Path
+
+SUPPORTED_EXTENSIONS = (".png", ".jpg", ".jpeg", ".bmp", ".gif", ".tiff")
+CAMERA_DIRS = ("main_bot_left", "main_bot_right")
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(
+        description=(
+            "Copy timestamped images from main_bot_left/main_bot_right into a "
+            "new folder, keeping only images inside an inclusive timestamp range."
+        )
+    )
+    parser.add_argument("source", help="Source folder containing camera folders")
+    parser.add_argument("destination", help="Destination folder to copy into")
+    parser.add_argument("start_time", type=float, help="Minimum timestamp to copy")
+    parser.add_argument("end_time", type=float, help="Maximum timestamp to copy")
+    parser.add_argument(
+        "--flat",
+        action="store_true",
+        help="Copy directly into destination instead of preserving camera folders",
+    )
+    return parser.parse_args()
+
+
+def extract_timestamp(filename: str) -> float:
+    match = re.search(r"\d+(\.\d+)?", filename)
+    if not match:
+        raise ValueError(f"No timestamp found in filename: {filename}")
+    return float(match.group())
+
+
+def iter_image_paths(folder: Path):
+    for path in sorted(folder.iterdir()):
+        if path.is_file() and path.name.lower().endswith(SUPPORTED_EXTENSIONS):
+            yield path
+
+
+def copy_camera_folder(
+    source_dir: Path,
+    destination_dir: Path,
+    start_time: float,
+    end_time: float,
+) -> tuple[int, int]:
+    destination_dir.mkdir(parents=True, exist_ok=True)
+    copied = 0
+    skipped = 0
+
+    for image_path in iter_image_paths(source_dir):
+        try:
+            timestamp = extract_timestamp(image_path.name)
+        except ValueError:
+            skipped += 1
+            continue
+
+        if start_time <= timestamp <= end_time:
+            shutil.copy2(image_path, destination_dir / image_path.name)
+            copied += 1
+        else:
+            skipped += 1
+
+    return copied, skipped
+
+
+def main() -> int:
+    args = parse_args()
+    source = Path(args.source).expanduser()
+    destination = Path(args.destination).expanduser()
+
+    if args.start_time > args.end_time:
+        print("Error: start_time must be <= end_time", file=sys.stderr)
+        return 1
+
+    if not source.is_dir():
+        print(f"Error: source folder not found: {source}", file=sys.stderr)
+        return 1
+
+    camera_dirs = [source / camera_dir for camera_dir in CAMERA_DIRS]
+    if not all(camera_dir.is_dir() for camera_dir in camera_dirs):
+        missing = [str(camera_dir) for camera_dir in camera_dirs if not camera_dir.is_dir()]
+        print(
+            "Error: source must contain main_bot_left and main_bot_right. "
+            f"Missing: {', '.join(missing)}",
+            file=sys.stderr,
+        )
+        return 1
+
+    total_copied = 0
+    total_skipped = 0
+    for camera_dir in camera_dirs:
+        output_dir = destination if args.flat else destination / camera_dir.name
+        copied, skipped = copy_camera_folder(
+            camera_dir,
+            output_dir,
+            args.start_time,
+            args.end_time,
+        )
+        total_copied += copied
+        total_skipped += skipped
+        print(f"{camera_dir.name}: copied {copied}, skipped {skipped}")
+
+    print(f"Done. Copied {total_copied} images into {destination}")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())
diff --git a/scripts/img_opener.py b/scripts/img_opener.py
index 250b06d6..b2511ac9 100644
--- a/scripts/img_opener.py
+++ b/scripts/img_opener.py
@@ -1,10 +1,12 @@
 import os
 import sys
 import re
+from bisect import bisect_left
 from PIL import Image, ImageTk
 import tkinter as tk
 
 SUPPORTED_EXTENSIONS = (".png", ".jpg", ".jpeg", ".bmp", ".gif", ".tiff")
+CAMERA_DIRS = ("main_bot_left", "main_bot_right")
 
 
 def extract_timestamp(filename):
@@ -20,24 +22,31 @@ def __init__(self, folder, start_time=0.0):
         self.root = tk.Tk()
         self.root.title("Image Viewer (Press any key to advance)")
 
-        # Collect images
-        items = []
-        for f in os.listdir(folder):
-            if f.lower().endswith(SUPPORTED_EXTENSIONS):
-                try:
-                    ts = extract_timestamp(f)
-                    items.append((ts, os.path.join(folder, f)))
-                except ValueError:
-                    pass
+        folder = resolve_folder(folder)
+        camera_folders = [os.path.join(folder, cam) for cam in CAMERA_DIRS]
+        if all(os.path.isdir(camera_folder) for camera_folder in camera_folders):
+            self.load_stereo_images(camera_folders, start_time)
+        else:
+            self.load_single_folder(folder, start_time)
+
+        self.index = 0
+
+        self.label = tk.Label(self.root)
+        self.label.pack()
+
+        self.root.bind("<Key>", self.next_image)
+
+        self.show_image()
+        self.root.mainloop()
+
+    def load_single_folder(self, folder, start_time):
+        self.mode = "single"
+        items = collect_images(folder)
 
         if not items:
             print("No valid timestamped images found.")
             sys.exit(1)
 
-        # Sort numerically
-        items.sort(key=lambda x: x[0])
-
-        # --- FILTER BY START TIME ---
         items = [item for item in items if item[0] >= start_time]
 
         if not items:
@@ -46,22 +55,52 @@ def __init__(self, folder, start_time=0.0):
 
         self.timestamps = [x[0] for x in items]
         self.image_paths = [x[1] for x in items]
-
-        # --- NORMALIZE TO FIRST DISPLAYED FRAME ---
         self.t0 = self.timestamps[0]
         self.norm_timestamps = [t - self.t0 for t in self.timestamps]
 
-        self.index = 0
+    def load_stereo_images(self, camera_folders, start_time):
+        self.mode = "stereo"
+        left_items = collect_images(camera_folders[0])
+        right_items = collect_images(camera_folders[1])
 
-        self.label = tk.Label(self.root)
-        self.label.pack()
+        if not left_items or not right_items:
+            print("Both main_bot_left and main_bot_right need valid timestamped images.")
+            sys.exit(1)
 
-        self.root.bind("<Key>", self.next_image)
+        left_items = [item for item in left_items if item[0] >= start_time]
+        right_items = [item for item in right_items if item[0] >= start_time]
 
-        self.show_image()
-        self.root.mainloop()
+        if not left_items or not right_items:
+            print(f"No images found after start_time={start_time}")
+            sys.exit(1)
+
+        all_timestamps = sorted(set(t for t, _ in left_items + right_items))
+
+        left_timestamps = [x[0] for x in left_items]
+        right_timestamps = [x[0] for x in right_items]
+        pairs = []
+        last_paths = None
+        for timestamp in all_timestamps:
+            left = nearest_item(left_items, left_timestamps, timestamp)
+            right = nearest_item(right_items, right_timestamps, timestamp)
+            paths = (left[1], right[1])
+            if paths == last_paths:
+                continue
+            pairs.append((timestamp, left, right))
+            last_paths = paths
+
+        self.timestamps = [x[0] for x in pairs]
+        self.image_pairs = [(x[1], x[2]) for x in pairs]
+        self.t0 = self.timestamps[0]
+        self.norm_timestamps = [t - self.t0 for t in self.timestamps]
 
     def show_image(self):
+        if self.mode == "stereo":
+            self.show_stereo_image()
+        else:
+            self.show_single_image()
+
+    def show_single_image(self):
         path = self.image_paths[self.index]
         img = Image.open(path)
 
@@ -79,9 +118,28 @@ def show_image(self):
             f"({self.index+1}/{len(self.image_paths)})"
         )
 
+    def show_stereo_image(self):
+        (left_ts, left_path), (right_ts, right_path) = self.image_pairs[self.index]
+
+        screen_w = self.root.winfo_screenwidth()
+        screen_h = self.root.winfo_screenheight()
+        img = make_side_by_side(left_path, right_path, screen_w, screen_h)
+
+        self.tk_image = ImageTk.PhotoImage(img)
+        self.label.config(image=self.tk_image)
+
+        norm_time = self.norm_timestamps[self.index]
+        frame_time = self.timestamps[self.index]
+        delta_ms = abs(left_ts - right_ts) * 1000
+        self.root.title(
+            f"left {os.path.basename(left_path)} | right {os.path.basename(right_path)} | "
+            f"t = {norm_time:.3f}s ({frame_time:.3f}) | dt = {delta_ms:.1f}ms "
+            f"({self.index+1}/{len(self.image_pairs)})"
+        )
+
     def next_image(self, event=None):
         self.index += 1
-        if self.index >= len(self.image_paths):
+        if self.index >= len(self.timestamps):
             print("Reached end of images.")
             self.root.quit()
             return
@@ -89,9 +147,64 @@ def next_image(self, event=None):
         self.show_image()
 
 
+def resolve_folder(folder):
+    if os.path.isdir(folder):
+        return folder
+
+    scripts_parent_path = os.path.join(os.path.dirname(__file__), "..", folder)
+    if os.path.isdir(scripts_parent_path):
+        return scripts_parent_path
+
+    print(f"Folder not found: {folder}")
+    sys.exit(1)
+
+
+def collect_images(folder):
+    items = []
+    for f in os.listdir(folder):
+        if f.lower().endswith(SUPPORTED_EXTENSIONS):
+            try:
+                ts = extract_timestamp(f)
+                items.append((ts, os.path.join(folder, f)))
+            except ValueError:
+                pass
+
+    items.sort(key=lambda x: x[0])
+    return items
+
+
+def nearest_item(items, timestamps, timestamp):
+    index = bisect_left(timestamps, timestamp)
+    if index == 0:
+        return items[0]
+    if index == len(items):
+        return items[-1]
+
+    before = items[index - 1]
+    after = items[index]
+    if abs(before[0] - timestamp) <= abs(after[0] - timestamp):
+        return before
+    return after
+
+
+def make_side_by_side(left_path, right_path, screen_w, screen_h):
+    max_each_w = max(1, screen_w // 2)
+    left_img = Image.open(left_path).convert("RGB")
+    right_img = Image.open(right_path).convert("RGB")
+    left_img.thumbnail((max_each_w, screen_h))
+    right_img.thumbnail((max_each_w, screen_h))
+
+    width = left_img.width + right_img.width
+    height = max(left_img.height, right_img.height)
+    combined = Image.new("RGB", (width, height), "black")
+    combined.paste(left_img, (0, (height - left_img.height) // 2))
+    combined.paste(right_img, (left_img.width, (height - right_img.height) // 2))
+    return combined
+
+
 if __name__ == "__main__":
     if len(sys.argv) not in (2, 3):
-        print("Usage: python viewer.py <image_folder> [start_time]")
+        print("Usage: python img_opener.py <image_folder> [start_time]")
         sys.exit(1)
 
     folder = sys.argv[1]
diff --git a/src/camera/disk_camera.cc b/src/camera/disk_camera.cc
index eee12f85..54cad904 100644
--- a/src/camera/disk_camera.cc
+++ b/src/camera/disk_camera.cc
@@ -1,21 +1,82 @@
 #include "disk_camera.h"
+#include <condition_variable>
 #include <optional>
 #include <utility>
+#include "absl/flags/flag.h"
 #include "absl/log/check.h"
 #include "src/camera/camera_constants.h"
 #include "src/camera/camera_source.h"
 
+ABSL_FLAG(bool, deterministic_disk_camera, true,  // NOLINT
+          "Make DiskCamera replay synchronize deterministic DiskCamera "
+          "instances by frame instead of sleeping between timestamps.");
+
+namespace {
+std::mutex deterministic_barrier_mutex;
+std::condition_variable deterministic_barrier_cv;
+size_t deterministic_barrier_active = 0;
+size_t deterministic_barrier_arrived = 0;
+size_t deterministic_barrier_generation = 0;
+
+auto RegisterDeterministicDiskCamera() -> void {
+  std::lock_guard<std::mutex> lock(deterministic_barrier_mutex);
+  deterministic_barrier_active++;
+}
+
+auto ReleaseDeterministicBarrierIfReady(std::unique_lock<std::mutex>& lock)
+    -> void {
+  if (deterministic_barrier_active == 0 ||
+      deterministic_barrier_arrived < deterministic_barrier_active) {
+    return;
+  }
+  deterministic_barrier_arrived = 0;
+  deterministic_barrier_generation++;
+  lock.unlock();
+  deterministic_barrier_cv.notify_all();
+}
+
+auto DeregisterDeterministicDiskCamera(bool& registered) -> void {
+  if (!registered) {
+    return;
+  }
+  std::unique_lock<std::mutex> lock(deterministic_barrier_mutex);
+  registered = false;
+  deterministic_barrier_active--;
+  ReleaseDeterministicBarrierIfReady(lock);
+}
+
+auto WaitForDeterministicDiskCameras() -> void {
+  std::unique_lock<std::mutex> lock(deterministic_barrier_mutex);
+  const size_t generation = deterministic_barrier_generation;
+  deterministic_barrier_arrived++;
+  if (deterministic_barrier_arrived >= deterministic_barrier_active) {
+    ReleaseDeterministicBarrierIfReady(lock);
+    return;
+  }
+  deterministic_barrier_cv.wait(lock, [generation] {
+    return generation != deterministic_barrier_generation ||
+           deterministic_barrier_active == 0;
+  });
+}
+}  // namespace
+
 namespace camera {
 
 DiskCamera::DiskCamera(std::string image_folder_path,
                        std::optional<camera_constant_t> camera_constant,
                        double speed, std::optional<double> start,
-                       std::optional<double> end)
+                       std::optional<double> end, bool deterministic)
     : speed_(speed),
+      deterministic_(deterministic ||
+                     absl::GetFlag(FLAGS_deterministic_disk_camera)),
+      deterministic_registered_(deterministic_),
       start_(start),
       end_(end),
       camera_constant_(std::move(camera_constant)),
       image_folder_path_(std::move(image_folder_path)) {
+  if (deterministic_registered_) {
+    RegisterDeterministicDiskCamera();
+  }
   CHECK(!start_.has_value() || !end_.has_value() ||
         end_.value() > start_.value());
   for (auto& entry : std::filesystem::directory_iterator(image_folder_path_)) {
@@ -55,8 +116,14 @@ DiskCamera::DiskCamera(std::string image_folder_path,
   }
   image_paths_ = std::move(normalized);
 }
+
+DiskCamera::~DiskCamera() {
+  DeregisterDeterministicDiskCamera(deterministic_registered_);
+}
+
 auto DiskCamera::GetFrame() -> timestamped_frame_t {
   if (image_paths_.empty()) {
+    DeregisterDeterministicDiskCamera(deterministic_registered_);
     std::cout << "Finished reading all frames from DiskCamera. Folder path: "
               << image_folder_path_ << std::endl;
     return {.invalid = true};
@@ -69,7 +136,12 @@ auto DiskCamera::GetFrame() -> timestamped_frame_t {
       .timestamp = recorded_ts + start_.value_or(0)};
   image_paths_.pop();
 
-  if (!image_paths_.empty()) {
+  if (deterministic_) {
+    WaitForDeterministicDiskCameras();
+    if (image_paths_.empty()) {
+      DeregisterDeterministicDiskCamera(deterministic_registered_);
+    }
+  } else if (!image_paths_.empty()) {
     double delta = image_paths_.top().timestamp - recorded_ts;
     std::this_thread::sleep_for(std::chrono::duration<double>(delta / speed_));
   }
diff --git a/src/camera/disk_camera.h b/src/camera/disk_camera.h
index 08d98d2d..9899a2ac 100644
--- a/src/camera/disk_camera.h
+++ b/src/camera/disk_camera.h
@@ -28,7 +28,9 @@ class DiskCamera : public ICamera {
   DiskCamera(std::string image_folder_path,
              std::optional<camera_constant_t> camera_constant = std::nullopt,
              double speed = 1.0, std::optional<double> start = std::nullopt,
-             std::optional<double> end = std::nullopt);
+             std::optional<double> end = std::nullopt,
+             bool deterministic = false);
+  ~DiskCamera() override;
   auto GetFrame() -> timestamped_frame_t override;
   auto Restart() -> void override;
   auto IsDone() -> bool override { return image_paths_.empty(); }
@@ -36,6 +38,8 @@ class DiskCamera : public ICamera {
 
  private:
   const double speed_;
+  const bool deterministic_;
+  bool deterministic_registered_;
   const std::optional<double> start_, end_;
   std::optional<camera_constant_t> camera_constant_;
   std::string image_folder_path_;
diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc
index 2f9ff596..3e45fdf5 100644
--- a/src/localization/joint_solver.cc
+++ b/src/localization/joint_solver.cc
@@ -1,17 +1,21 @@
 #include "src/localization/joint_solver.h"
 #include <opencv2/calib3d.hpp>
+#include <unsupported/Eigen/MatrixFunctions>
 #include <utility>
+#include "absl/flags/flag.h"
 #include "src/utils/camera_utils.h"
 #include "src/utils/constants_from_json.h"
 #include "src/utils/transform.h"
 
-namespace localization {
+ABSL_FLAG(int, max_epochs, 10000,                       // NOLINT
+          "Max iterative epochs the solver will use");  // NOLINT
+ABSL_FLAG(double, max_acceptable_error, 1e5,             // NOLINT
+          "Maximum accepted final reprojection loss");   // NOLINT
+ABSL_FLAG(double, lambda_scalar,                        // NOLINT
+          2.0,                                          // NOLINT
+          "Levenberg-Marquardt damping adjustment");    // NOLINT
 
-using data_point_t = struct DataPoint {
-  Eigen::Vector2d undistorted_point;
-  std::string name;
-  Eigen::Vector4d field_to_tag_corner_homogenous;
-};
+namespace localization {
 
 constexpr auto sq(double num) -> double {
   return num * num;
@@ -20,9 +24,9 @@ constexpr auto sq(double num) -> double {
 using frc::AprilTagFieldLayout;
 
 JointSolver::JointSolver(
-    const std::vector<camera::camera_constant_t>& camera_constants_,
-    const AprilTagFieldLayout& layout)
-    : robot_to_field_(Eigen::Matrix4d::Identity()) {
+    const std::vector<camera::camera_constant_t>& camera_constants_)
+    : robot_to_field_(Eigen::Matrix4d::Identity()),
+      backup_solver_(camera_constants_) {
   // clang-format off
   const Eigen::Matrix4d rotate_yaw_cv = (Eigen::Matrix4d() <<
       -1, 0, 0, 0,
@@ -30,7 +34,7 @@ JointSolver::JointSolver(
       0, 0, -1, 0,
       0, 0, 0, 1).finished();
   // clang-format on
-  for (const frc::AprilTag& tag : layout.GetTags()) {
+  for (const frc::AprilTag& tag : kapriltag_layout.GetTags()) {
     Eigen::Matrix4d field_to_tag = tag.pose.ToMatrix();
     utils::ChangeBasis(field_to_tag, utils::WPI_TO_CV);
     std::array<Eigen::Vector4d, 4> field_relative_corners;
@@ -48,168 +52,211 @@ JointSolver::JointSolver(
     const auto camera_matrix =
         utils::CameraMatrixFromJson<Eigen::Matrix3d>(intrinsics_json);
     const Eigen::Matrix<double, 3, 4> image_to_camera = camera_matrix * pi;
-    const Eigen::Matrix4d camera_to_robot =
+    Eigen::Matrix4d camera_to_robot =
         utils::ExtrinsicsJsonToCameraToRobot(
             utils::ReadExtrinsics(camera_constant.extrinsics_path.value()))
             .ToMatrix();
+    utils::ChangeBasis(camera_to_robot, utils::WPI_TO_CV);
     const Eigen::Matrix<double, 3, 4> image_to_robot =
         image_to_camera * camera_to_robot;
-    camera_matrices_.insert(
-        {camera_constant.name,
-         {.image_to_robot = image_to_robot,
-          .distortion_coefficients =
-              utils::DistortionCoefficientsFromJson<cv::Mat>(intrinsics_json),
-          .camera_matrix = utils::EigenToCvMat(camera_matrix)}});
+    camera_matrices_.push_back(
+        {.image_to_robot = image_to_robot,
+         .distortion_coefficients =
+             utils::DistortionCoefficientsFromJson<cv::Mat>(intrinsics_json),
+         .camera_matrix = utils::EigenToCvMat(camera_matrix)});
+  }
+}
+
+void JointSolver::SetStartPosition(const std::optional<frc::Pose3d>& pose) {
+  if (pose.has_value()) {
+    robot_to_field_ = pose->ToMatrix().inverse();
+    utils::ChangeBasis(robot_to_field_, utils::WPI_TO_CV);
+  } else {
+    should_reset_ = true;
+  }
+}
+
+void JointSolver::ComputeResidual(
+    const std::vector<data_point_t>& data_points,
+    const Eigen::Matrix4d& robot_to_field, Eigen::VectorXd& residual,
+    Eigen::MatrixXd* d_residual_d_twist_jacobian) {
+  for (size_t i = 0; i < data_points.size(); i++) {
+    const Eigen::Matrix<double, 3, 4>& image_to_robot =
+        camera_matrices_[data_points[i].source_index].image_to_robot;
+
+    const Eigen::Vector4d robot_relative_corner =
+        robot_to_field * data_points[i].field_to_tag_corner_homogeneous_cv;
+
+    Eigen::Vector3d projection = image_to_robot * robot_relative_corner;
+    const double lambda = projection(2);
+    projection /= lambda;
+
+    const double x_residual =
+        data_points[i].undistorted_point.x() - projection.x();
+    const double y_residual =
+        data_points[i].undistorted_point.y() - projection.y();
+    residual(2 * i) = x_residual;
+    residual(2 * i + 1) = y_residual;
+    if (d_residual_d_twist_jacobian == nullptr) {
+      continue;
+    }
+
+    // clang-format off
+      const Eigen::Matrix<double, 2, 3> d_residual_i_d_projection_i = 
+        (Eigen::MatrixXd(2, 3) <<
+          -1/lambda, 0, projection.x()/lambda,
+          0, -1/lambda, projection.y()/lambda).finished();
+    // clang-format on
+
+    const Eigen::Matrix3d& d_projection_i_d_robot_relative_object_point =
+        image_to_robot.block<3, 3>(0, 0);
+
+    Eigen::Matrix<double, 3, 6> d_robot_relative_object_point_d_d_twist =
+        Eigen::MatrixXd(3, 6);
+    d_robot_relative_object_point_d_d_twist.block<3, 3>(0, 0) =
+        -utils::CrossProduct(robot_relative_corner.block<3, 1>(0, 0));
+    d_robot_relative_object_point_d_d_twist.block<3, 3>(0, 3) =
+        Eigen::Matrix3d::Identity();
+
+    d_residual_d_twist_jacobian->block<2, 6>(2 * i, 0) =
+        d_residual_i_d_projection_i *
+        d_projection_i_d_robot_relative_object_point *
+        d_robot_relative_object_point_d_d_twist;
   }
 }
 
 auto JointSolver::EstimatePosition(
-    const std::map<std::string, std::vector<tag_detection_t>>&
-        all_cam_detections,
-    const frc::Pose3d& starting_pose) -> position_estimate_t {
-  if (all_cam_detections.empty()) {
-    return {};
+    std::vector<std::vector<tag_detection_t>>& detection_batches,
+    bool reject_far_tags) -> std::optional<position_estimate_t> {
+  const int max_epochs = absl::GetFlag(FLAGS_max_epochs);
+  const double lambda_scalar = absl::GetFlag(FLAGS_lambda_scalar);
+  const double max_acceptable_error =
+      absl::GetFlag(FLAGS_max_acceptable_error);
+  if (detection_batches.empty()) {
+    return std::nullopt;
   }
-  robot_to_field_ = starting_pose.ToMatrix().inverse();
-  utils::ChangeBasis(robot_to_field_, utils::WPI_TO_CV);
+  double avg_timestamp = 0;
   std::vector<data_point_t> data_points;
-  for (const auto& pair : all_cam_detections) {
-    const CameraMatrices& camera_mats = camera_matrices_.at(pair.first);
-    for (const tag_detection_t& detection : pair.second) {
+  for (size_t source_index = 0; source_index < detection_batches.size();
+       source_index++) {
+    const CameraMatrices& camera_mats = camera_matrices_[source_index];
+    for (const tag_detection_t& detection : detection_batches[source_index]) {
+      avg_timestamp += detection.timestamp;
       std::vector<cv::Point2d> undistorted_corners;
       cv::undistortImagePoints(detection.corners, undistorted_corners,
                                camera_mats.camera_matrix,
                                camera_mats.distortion_coefficients);
-      for (size_t i = 0; i < undistorted_corners.size(); i++) {
+      for (size_t corner_index = 0; corner_index < undistorted_corners.size();
+           corner_index++) {
         Eigen::Vector2d undistorted_image_point;
-        undistorted_image_point << undistorted_corners[i].x,
-            undistorted_corners[i].y;
+        undistorted_image_point << undistorted_corners[corner_index].x,
+            undistorted_corners[corner_index].y;
         const data_point_t datapoint = {
             .undistorted_point = undistorted_image_point,
-            .name = pair.first,
-            .field_to_tag_corner_homogenous =
-                tag_corners_[detection.tag_id].value()[i]};
+            .source_index = source_index,
+            .field_to_tag_corner_homogeneous_cv =
+                tag_corners_[detection.tag_id].value()[corner_index]};
         data_points.push_back(datapoint);
       }
     }
   }
 
-  double loss = std::numeric_limits<double>::infinity();
-  int counter = 0;
-
-  const double step = 1e-7;
-
-  utils::TransformValues translation_and_rotation =
-      utils::ExtractTranslationAndRotation(robot_to_field_);
-  utils::TransformDecomposition decomposed_robot_to_field;
-
-  while (loss > kacceptable_reprojection_error && counter < 1000000) {
-    bool printed = false;
-    for (const data_point_t& data_point : data_points) {
-      decomposed_robot_to_field = utils::SeparateTranslationAndRotationMatrices(
-          translation_and_rotation);
-      const Eigen::Matrix<double, 3, 4>& image_to_robot =
-          camera_matrices_.at(data_point.name).image_to_robot;
-
-      const Eigen::Vector4d Rx_activation =
-          decomposed_robot_to_field.Rx *
-          data_point.field_to_tag_corner_homogenous;
-      const Eigen::Vector4d Ry_activation =
-          decomposed_robot_to_field.Ry * Rx_activation;
-      const Eigen::Vector4d Rz_activation =
-          decomposed_robot_to_field.Rz * Ry_activation;
-
-      Eigen::Vector3d projection = image_to_robot *
-                                   decomposed_robot_to_field.translation *
-                                   Rz_activation;
-      const double lambda = projection(2);
-      projection /= lambda;
-
-      const Eigen::Vector2d projection_error(
-          projection.x() - data_point.undistorted_point.x(),
-          projection.y() - data_point.undistorted_point.y());
-      loss = 0.5 * projection_error.squaredNorm();
-
-      if (counter % 10000 == 0 && !printed) {
-        printed = true;
-        std::cout << "loss: " << loss << std::endl;
+  if (data_points.empty()) {
+    return std::nullopt;
+  }
+
+  std::optional<position_estimate_t> backup_estimate_ =
+      backup_solver_.EstimatePosition(detection_batches);
+  if (!backup_estimate_.has_value()) {
+    return std::nullopt;
+  }
+  bool joint_solve_failure = false;
+  utils::PrintPose3d(backup_estimate_->pose);
+  SetStartPosition(std::make_optional<frc::Pose3d>(backup_estimate_->pose));
+
+  avg_timestamp /= data_points.size() / 4;
+
+  Eigen::VectorXd residual(2 * data_points.size());
+  Eigen::MatrixXd d_residual_d_twist_jacobian(2 * data_points.size(), 6);
+  double current_error = std::numeric_limits<double>::infinity();
+
+  for (int epoch = 0; epoch < max_epochs; epoch++) {
+    ComputeResidual(data_points, robot_to_field_, residual,
+                    &d_residual_d_twist_jacobian);
+    current_error = residual.cwiseSquare().sum();
+    if (debug && epoch % 10 == 0) {
+      LOG(INFO) << "Current error: " << current_error;
+    }
+    const Eigen::Vector<double, 6> b =
+        -d_residual_d_twist_jacobian.transpose() * residual;
+    const Eigen::Matrix<double, 6, 6> hessian =
+        d_residual_d_twist_jacobian.transpose() * d_residual_d_twist_jacobian;
+    double lambda = 1;
+    Eigen::Matrix<double, 6, 6> hessian_diag =
+        hessian.diagonal().asDiagonal().toDenseMatrix();
+    Eigen::Matrix4d candidate_robot_to_field;
+    while (true) {
+      const Eigen::Vector<double, 6> partial_twist =
+          (hessian + lambda * hessian_diag).ldlt().solve(b);
+      Eigen::Matrix4d twist_expanded = Eigen::Matrix4d::Zero();
+      twist_expanded.block<3, 3>(0, 0) =
+          utils::CrossProduct(partial_twist.block<3, 1>(0, 0));
+      twist_expanded.block<3, 1>(0, 3) = partial_twist.block<3, 1>(3, 0);
+      candidate_robot_to_field = twist_expanded.exp() * robot_to_field_;
+      ComputeResidual(data_points, candidate_robot_to_field, residual);
+      double candidate_error = residual.cwiseSquare().sum();
+      if (candidate_error > current_error) {
+        lambda *= lambda_scalar;
+        if (lambda > kmaximum_lambda) {
+          joint_solve_failure = true;
+        }
+      } else {
+        lambda /= lambda_scalar;
+        robot_to_field_ = candidate_robot_to_field;
+        break;
       }
+    }
+  }
 
-      const Eigen::Vector3d d_projection(
-          projection_error.x() / lambda, projection_error.y() / lambda,
-          -(projection_error.x() * projection.x() +
-            projection_error.y() * projection.y()) /
-              lambda);
-
-      Eigen::Vector4d accumulated_gradient =
-          image_to_robot.transpose() * d_projection;
-
-      const Eigen::Matrix4d d_translation =
-          accumulated_gradient * Rz_activation.transpose();
-
-      accumulated_gradient = decomposed_robot_to_field.translation.transpose() *
-                             accumulated_gradient;
-      const Eigen::Matrix4d d_Rz =
-          accumulated_gradient * Ry_activation.transpose();
-
-      accumulated_gradient =
-          decomposed_robot_to_field.Rz.transpose() * accumulated_gradient;
-      const Eigen::Matrix4d d_Ry =
-          accumulated_gradient * Rx_activation.transpose();
-
-      accumulated_gradient =
-          decomposed_robot_to_field.Ry.transpose() * accumulated_gradient;
-      const Eigen::Matrix4d d_Rx =
-          accumulated_gradient *
-          data_point.field_to_tag_corner_homogenous.transpose();
-
-      // clang-format off
-      const Eigen::Matrix4d d_Rz_d_rz =
-          (Eigen::Matrix4d() << 
-        -sin(translation_and_rotation.rz), -cos(translation_and_rotation.rz), 0, 0,
-        cos(translation_and_rotation.rz), -sin(translation_and_rotation.rz), 0, 0, 
-        0, 0, 0, 0,
-        0, 0, 0, 0).finished();
-      const Eigen::Matrix4d d_Ry_d_ry =
-          (Eigen::Matrix4d() << 
-        -sin(translation_and_rotation.ry), 0, cos(translation_and_rotation.ry), 0,
-        0, 0, 0, 0,
-        -cos(translation_and_rotation.ry), 0, -sin(translation_and_rotation.ry), 0,
-        0, 0, 0, 0).finished();
-      const Eigen::Matrix4d d_Rx_d_rx =
-        (Eigen::Matrix4d() << 
-        0, 0, 0, 0, 
-        0, -sin(translation_and_rotation.rx), -cos(translation_and_rotation.rx), 0,
-        0, cos(translation_and_rotation.rx), -sin(translation_and_rotation.rx), 0,
-        0, 0, 0, 0).finished();
-      // clang-format on
-
-      translation_and_rotation.rx -=
-          step * (d_Rx_d_rx.transpose() * d_Rx).trace();
-      translation_and_rotation.ry -=
-          step * (d_Ry_d_ry.transpose() * d_Ry).trace();
-      translation_and_rotation.rz -=
-          step * (d_Rz_d_rz.transpose() * d_Rz).trace();
-
-      translation_and_rotation.x -= step * d_translation(0, 3);
-      translation_and_rotation.y -= step * d_translation(1, 3);
-      translation_and_rotation.z -= step * d_translation(2, 3);
+  if (joint_solve_failure || current_error > max_acceptable_error) {
+    if (debug) {
+      LOG(INFO) << "Joint solve failure";
     }
-    counter++;
+    return backup_estimate_;
   }
 
-  Eigen::Matrix4d field_to_robot =
-      (decomposed_robot_to_field.translation * decomposed_robot_to_field.Rz *
-       decomposed_robot_to_field.Ry * decomposed_robot_to_field.Rx)
-          .inverse();
+  if (debug)
+    LOG(INFO) << "Robot to field:\n" << robot_to_field_;
 
-  utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot),
-                                   "Field to robot cv");
+  Eigen::Matrix4d field_to_robot = robot_to_field_.inverse();
+  field_to_robot(3, 0) = 0;
+  field_to_robot(3, 1) = 0;
+  field_to_robot(3, 2) = 0;
+  field_to_robot(3, 3) = 1;
+
+  if (debug) {
+    LOG(INFO) << "Field to robot cv:\n" << field_to_robot;
+    utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot),
+                                     "Field to robot cv");
+  }
   utils::ChangeBasis(field_to_robot, utils::CV_TO_WPI);
-  utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot),
-                                   "Field to robot wpi");
+  if (debug) {
+    LOG(INFO) << "Field to robot wpi:\n" << field_to_robot;
+    utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot),
+                                     "Field to robot wpi");
+  }
+  const frc::Pose3d iteratively_solved_pose(field_to_robot);
+
+  if (debug) {
+    LOG(INFO) << "Pose for ts: " << avg_timestamp << ":\t";
+  }
+  utils::PrintPose3d(iteratively_solved_pose);
 
-  return {.pose = frc::Pose3d(field_to_robot), .variance = 0, .timestamp = 0};
+  return std::make_optional<position_estimate_t>(
+      {.pose = iteratively_solved_pose,
+       .timestamp = avg_timestamp,
+       .num_tags = static_cast<int>(data_points.size() / 4),
+       .loss = current_error});
 }
 
 }  // namespace localization
diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h
index 7dfb91f1..950b13bf 100644
--- a/src/localization/joint_solver.h
+++ b/src/localization/joint_solver.h
@@ -3,31 +3,46 @@
 #include "nlohmann/json.hpp"
 #include "src/camera/camera_constants.h"
 #include "src/localization/position_solver.h"
-#include "src/localization/square_solver.h"
+#include "src/localization/unambiguous_estimator.h"
 
 using json = nlohmann::json;
 
 namespace localization {
+using data_point_t = struct DataPoint {
+  Eigen::Vector2d undistorted_point;
+  size_t source_index;
+  Eigen::Vector4d field_to_tag_corner_homogeneous_cv;
+};
 struct CameraMatrices {
   Eigen::Matrix<double, 3, 4> image_to_robot;
   cv::Mat distortion_coefficients;
   cv::Mat camera_matrix;
 };
 static constexpr int kmax_tags = 50;
-class JointSolver {
+class JointSolver : public IJointPositionSolver {
  public:
-  JointSolver(const std::vector<camera::camera_constant_t>& camera_constants_,
-              const frc::AprilTagFieldLayout& layout = kapriltag_layout);
+  JointSolver(const std::vector<camera::camera_constant_t>& camera_constants_);
   auto EstimatePosition(
-      const std::map<std::string, std::vector<tag_detection_t>>&
-          all_cam_detections,
-      const frc::Pose3d& starting_pose) -> position_estimate_t;
-  Eigen::Matrix4d robot_to_field_;
+      std::vector<std::vector<tag_detection_t>>& detection_batches,
+      bool reject_far_tags = true)
+      -> std::optional<position_estimate_t> override;
+  void SetStartPosition(const std::optional<frc::Pose3d>& pose);
+  void ComputeResidual(const std::vector<data_point_t>& data_points,
+                       const Eigen::Matrix4d& robot_to_field,
+                       Eigen::VectorXd& residual,
+                       Eigen::MatrixXd* d_residual_d_twist_jacobian = nullptr);
 
  private:
   static constexpr double kacceptable_reprojection_error = 0.005;
-  std::map<std::string, CameraMatrices> camera_matrices_;
+  static constexpr double kmaximum_lambda = 1e10;
+  static constexpr double kmax_acceptable_error = 1e5;
+  static constexpr double kmin_acceptable_error = 2;
+  std::vector<CameraMatrices> camera_matrices_;
   std::array<std::optional<std::array<Eigen::Vector4d, 4>>, kmax_tags>
       tag_corners_;
+  Eigen::Matrix4d robot_to_field_;
+  UnambiguousEstimator backup_solver_;
+  bool should_reset_ = false;
+  const bool debug = false;
 };
 }  // namespace localization
diff --git a/src/localization/networktable_sender.cc b/src/localization/networktable_sender.cc
index 37d2a8c4..80931c83 100644
--- a/src/localization/networktable_sender.cc
+++ b/src/localization/networktable_sender.cc
@@ -1,6 +1,7 @@
 #include "networktable_sender.h"
 #include "frc/DataLogManager.h"
 #include "src/localization/position.h"
+#include "src/utils/log_path.h"
 
 namespace localization {
 
@@ -56,7 +57,7 @@ NetworkTableSender::NetworkTableSender(const std::string& camera_name,
 
   if (sim) {
     std::error_code ec;
-    log_.emplace("/bos/logs/sim.wpilog", ec);
+    log_.emplace("/bos/logs/" + utils::GetSimLogName(), ec);
     if (ec) {
       std::cerr << "Failed to open log: " << ec.message() << '\n';
       std::exit(0);
diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc
index 128248cc..cfddf080 100644
--- a/src/pathing/simulator.cc
+++ b/src/pathing/simulator.cc
@@ -7,10 +7,15 @@
 #include <nlohmann/json.hpp>
 #include <system_error>
 #include <vector>
+#include "absl/flags/parse.h"
 #include "src/pathing/pathing.h"
+#include "src/utils/log_path.h"
 
-auto main() -> int {
-  wpi::log::DataLogBackgroundWriter log{"/root/bos/logs", "sim.wpilog"};
+auto main(int argc, char** argv) -> int {
+  absl::ParseCommandLine(argc, argv);
+
+  wpi::log::DataLogBackgroundWriter log{"/root/bos/logs",
+                                        utils::GetSimLogName()};
 
   wpi::log::StructLogEntry<frc::Pose2d> poseLog(log, "/sim/Pose2d");
   wpi::log::DoubleLogEntry accelXLog(log, "/sim/AccelX");
diff --git a/src/test/integration_test/localization_test2.cc b/src/test/integration_test/localization_test2.cc
index 250603c0..f174ebc3 100644
--- a/src/test/integration_test/localization_test2.cc
+++ b/src/test/integration_test/localization_test2.cc
@@ -14,6 +14,7 @@
 #include "src/camera/camera_constants.h"
 #include "src/camera/camera_source.h"
 #include "src/camera/disk_camera.h"
+#include "src/localization/joint_solver.h"
 #include "src/localization/multi_tag_solver.h"
 #include "src/localization/networktable_sender.h"
 #include "src/localization/opencv_apriltag_detector.h"
@@ -129,7 +130,7 @@ auto main(int argc, char** argv) -> int {
     LOG(INFO) << "Created camera source";
     localization::RunJointLocalization(
         stop_token, detector_source,
-        std::make_unique<localization::UnambiguousEstimator>(camera_constants),
+        std::make_unique<localization::JointSolver>(camera_constants),
         std::make_unique<localization::NetworkTableSender>("Joint", false,
                                                            true));
   });
diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc
index 0bc15d77..88f21c04 100644
--- a/src/test/unit_test/joint_solve_test.cc
+++ b/src/test/unit_test/joint_solve_test.cc
@@ -1,41 +1,150 @@
 #include <gtest/gtest.h>
+#include <random>
 #include "src/localization/joint_solver.h"
+#include "src/localization/opencv_apriltag_detector.h"
 #include "src/localization/square_solver.h"
+#include "src/test/unit_test/unit_test_utils.h"
 #include "src/utils/camera_utils.h"
 #include "src/utils/constants_from_json.h"
 #include "src/utils/transform.h"
 
-std::vector<localization::tag_detection_t> detections(
-    {{.tag_id = 31,
-      .corners = {cv::Point2d(100.0, 200.0), cv::Point2d(200.0, 200.0),
-                  cv::Point2d(200.0, 100.0), cv::Point2d(100.0, 100.0)},
-      .timestamp = 0.0,
-      .confidence = 0.0}});
+class JointSolverTest : public ::testing::Test {
+ protected:
+  camera::camera_constants_t camera_constants = camera::GetCameraConstants();
 
-TEST(JointSolveTest, EstimatePosition) {  // NOLINT
-  camera::camera_constant_t camera_constant =
-      camera::GetCameraConstants().at("dev_orin");
-  localization::JointSolver joint_solver({camera_constant});
-  localization::SquareSolver square_solver(camera_constant);
+  std::vector<camera::CameraConstant> joint_solve_cameras =
+      std::vector<camera::CameraConstant>{
+          camera_constants.at("main_bot_left"),
+          camera_constants.at("main_bot_right")};
+  localization::SquareSolver left_square_solver;
+  localization::SquareSolver right_square_solver;
+  localization::JointSolver joint_solver;
+  localization::OpenCVAprilTagDetector left_detector;
+  localization::OpenCVAprilTagDetector right_detector;
+  JointSolverTest()
+      : left_square_solver(camera_constants.at("main_bot_left")),
+        right_square_solver(camera_constants.at("main_bot_right")),
+        joint_solver(joint_solve_cameras),
+        left_detector(utils::ReadIntrinsics(
+            camera_constants.at("main_bot_left").intrinsics_path.value())),
+        right_detector(utils::ReadIntrinsics(
+            camera_constants.at("main_bot_right").intrinsics_path.value())) {}
+};
 
-  auto square_solver_solution = square_solver.EstimatePosition(detections)[0];
-  utils::PrintTransformationMatrix(
-      utils::EigenToCvMat(square_solver_solution.pose.ToMatrix()));
-  std::cout << square_solver_solution.pose.ToMatrix() << std::endl;
-
-  frc::Transform3d noise(
-      frc::Translation3d(units::meter_t{0.067}, units::meter_t{0.1},
-                         units::meter_t{-0.1}),
-      {});
+TEST_F(JointSolverTest, MaintainsValidEstimateRealImageYawOnly) {  // NOLINT
+  const cv::Mat left_image =
+      cv::imread("/bos/bos-logs/short-pmatch2/main_bot_left/3.267974.jpg");
+  const cv::Mat right_image =
+      cv::imread("/bos/bos-logs/short-pmatch2/main_bot_right/3.271297.jpg");
+  camera::timestamped_frame_t left_frame{.frame = left_image, .timestamp = 0.0};
+  camera::timestamped_frame_t right_frame{.frame = right_image,
+                                          .timestamp = 0.0};
+  const std::vector<localization::tag_detection_t> left_detections =
+      left_detector.GetTagDetections(left_frame);
+  const std::vector<localization::tag_detection_t> right_detections =
+      right_detector.GetTagDetections(right_frame);
+  CHECK_NE(left_detections.size() + right_detections.size(), 0);
+  const std::vector<localization::position_estimate_t>
+      left_square_solver_solutions =
+          left_square_solver.EstimatePosition(left_detections, false);
+  const std::optional<localization::position_estimate_t>
+      left_square_solver_solution =
+          left_square_solver_solutions.size() == 0
+              ? std::nullopt
+              : std::make_optional<localization::position_estimate_t>(
+                    left_square_solver_solutions[0]);
+  const std::vector<localization::position_estimate_t>
+      right_square_solver_solutions =
+          right_square_solver.EstimatePosition(right_detections, false);
+  const std::optional<localization::position_estimate_t>
+      right_square_solver_solution =
+          right_square_solver_solutions.size() == 0
+              ? std::nullopt
+              : std::make_optional<localization::position_estimate_t>(
+                    right_square_solver_solutions[0]);
+  std::vector<std::vector<localization::tag_detection_t>> associated_detections{
+      left_detections, right_detections};
+  const frc::Transform3d joint_solve_input_noise(
+      frc::Translation3d(units::meter_t{0.4}, units::meter_t{0.4},
+                         units::meter_t{0.0}),
+      frc::Rotation3d(units::degree_t{0}, units::degree_t{0},
+                      units::degree_t{5}));
+  joint_solver.SetStartPosition(
+      (left_square_solver_solution.has_value() ? left_square_solver_solution
+                                               : right_square_solver_solution)
+          ->pose.TransformBy(joint_solve_input_noise));
+  const localization::position_estimate_t joint_solver_solution =
+      joint_solver.EstimatePosition(associated_detections, false).value();
+  if (left_square_solver_solution) {
+    std::cout << "sq left: " << left_square_solver_solution.value();
+  }
+  if (right_square_solver_solution) {
+    std::cout << "\nsq right: " << right_square_solver_solution.value();
+  }
+  std::cout << "\njoint: " << joint_solver_solution << std::endl;
+  std::cout << "loss: " << joint_solver_solution.loss << std::endl;
+  std::cout << "numtags: " << joint_solver_solution.num_tags << std::endl;
+}
 
-  square_solver_solution.pose = square_solver_solution.pose.TransformBy(noise);
+TEST_F(JointSolverTest, DoubleTrouble) {  // NOLINT
+  const cv::Mat left_image =
+      cv::imread("/bos/bos-logs/short-pmatch2/main_bot_left/3.267974.jpg");
+  const cv::Mat right_image =
+      cv::imread("/bos/bos-logs/short-pmatch2/main_bot_right/3.271297.jpg");
+  camera::timestamped_frame_t left_frame{.frame = left_image, .timestamp = 0.0};
+  camera::timestamped_frame_t right_frame{.frame = right_image,
+                                          .timestamp = 0.0};
+  const std::vector<localization::tag_detection_t> left_detections =
+      left_detector.GetTagDetections(left_frame);
+  const std::vector<localization::tag_detection_t> right_detections =
+      right_detector.GetTagDetections(right_frame);
+  CHECK_NE(left_detections.size() + right_detections.size(), 0);
+  const std::vector<localization::position_estimate_t>
+      left_square_solver_solutions =
+          left_square_solver.EstimatePosition(left_detections, false);
+  const std::optional<localization::position_estimate_t>
+      left_square_solver_solution =
+          left_square_solver_solutions.size() == 0
+              ? std::nullopt
+              : std::make_optional<localization::position_estimate_t>(
+                    left_square_solver_solutions[0]);
+  const std::vector<localization::position_estimate_t>
+      right_square_solver_solutions =
+          right_square_solver.EstimatePosition(right_detections, false);
+  const std::optional<localization::position_estimate_t>
+      right_square_solver_solution =
+          right_square_solver_solutions.size() == 0
+              ? std::nullopt
+              : std::make_optional<localization::position_estimate_t>(
+                    right_square_solver_solutions[0]);
+  std::vector<std::vector<localization::tag_detection_t>> associated_detections{
+      left_detections, right_detections};
+  const frc::Pose3d start_pose(
+      frc::Translation3d(units::meter_t{6.062}, units::meter_t{0.15},
+                         units::meter_t{-0.065}),
+      frc::Rotation3d(units::degree_t{3.221067}, units::degree_t{3.6748235},
+                      units::degree_t{176.1127104}));
 
-  std::map<std::string, std::vector<localization::tag_detection_t>>
-      associated_detections;
-  associated_detections.insert({camera_constant.name, detections});
-  auto joint_solver_solution = joint_solver.EstimatePosition(
-      associated_detections, square_solver_solution.pose);
-  std::cout << "Joint solver solution: " << joint_solver_solution << std::endl;
-  std::cout << "Square solver solution: " << square_solver_solution
-            << std::endl;
+  const frc::Pose3d correction_noise(
+      frc::Translation3d(units::meter_t{6.062}, units::meter_t{0.15},
+                         units::meter_t{0.06}),
+      frc::Rotation3d(units::degree_t{3.221067}, units::degree_t{3.6748235},
+                      units::degree_t{176.1127104}));
+  joint_solver.SetStartPosition(start_pose);
+  const localization::position_estimate_t joint_solver_solution =
+      joint_solver.EstimatePosition(associated_detections, false).value();
+  const localization::position_estimate_t new_solution =
+      joint_solver.EstimatePosition(associated_detections, false).value();
+  if (left_square_solver_solution) {
+    std::cout << "sq left: " << left_square_solver_solution.value();
+  }
+  if (right_square_solver_solution) {
+    std::cout << "\nsq right: " << right_square_solver_solution.value();
+  }
+  std::cout << "\njoint: " << joint_solver_solution << std::endl;
+  std::cout << "loss: " << joint_solver_solution.loss << std::endl;
+  std::cout << "numtags: " << joint_solver_solution.num_tags << std::endl;
+  std::cout << "\njoint new: " << new_solution << std::endl;
+  std::cout << "loss: " << new_solution.loss << std::endl;
+  std::cout << "numtags: " << new_solution.num_tags << std::endl;
 }
diff --git a/src/test/unit_test/matrix.cc b/src/test/unit_test/matrix.cc
index fce9f629..9b661d4a 100644
--- a/src/test/unit_test/matrix.cc
+++ b/src/test/unit_test/matrix.cc
@@ -30,20 +30,3 @@ auto TransformationMatrix(const double rx, const double ry, const double rz,
 
   return A;
 }
-
-TEST(JointSolveTest, EstimatePosition) {  // NOLINT
-  const Eigen::Matrix4d transform =
-      TransformationMatrix(rad(35), rad(23), rad(58), 1, 2, 3);
-  const utils::TransformValues decomp =
-      utils::ExtractTranslationAndRotation(transform);
-  std::cout << decomp.x << " " << decomp.y << " " << decomp.z << " "
-            << decomp.rx << " " << decomp.ry << " " << decomp.rz << " "
-            << std::endl;
-  const utils::TransformDecomposition decomped =
-      utils::SeparateTranslationAndRotationMatrices(decomp);
-  utils::PrintTransformationMatrix(utils::EigenToCvMat(transform), "original");
-  utils::PrintTransformationMatrix(
-      utils::EigenToCvMat(decomped.translation * decomped.Rz * decomped.Ry *
-                          decomped.Rx),
-      "new");
-}
diff --git a/src/utils/log.cc b/src/utils/log.cc
index 3354eb4b..55bcf936 100644
--- a/src/utils/log.cc
+++ b/src/utils/log.cc
@@ -26,6 +26,10 @@ void PrintPose3d(const frc::Pose3d& pose) {
             << "°" << std::endl;
 }
 
+auto GetSimLogName() -> std::string {
+  return "joint.wpilog";
+}
+
 void PrintPose2d(const frc::Pose2d& pose) {
   // Extract translation (in meters)
   double x = pose.X().value();
diff --git a/src/utils/log.h b/src/utils/log.h
index 54048040..4a6c7309 100644
--- a/src/utils/log.h
+++ b/src/utils/log.h
@@ -10,4 +10,5 @@ void PrintTransform3d(const frc::Transform3d& transform);
 void PrintTransformationMatrix(
     const cv::Mat& mat, const std::optional<std::string>& name = std::nullopt);
 void PrintPose2d(const frc::Pose2d& pose);
+auto GetSimLogName() -> std::string;
 }  // namespace utils
diff --git a/src/utils/log_path.cc b/src/utils/log_path.cc
index e69de29b..84d2726e 100644
--- a/src/utils/log_path.cc
+++ b/src/utils/log_path.cc
@@ -0,0 +1,16 @@
+#include "src/utils/log_path.h"
+
+#include <string>
+
+#include "absl/flags/flag.h"
+
+ABSL_FLAG(std::string, sim_log_name, "sim.wpilog",  // NOLINT
+          "Name of the simulation WPILib log file.");
+
+namespace utils {
+
+std::string GetSimLogName() {
+  return absl::GetFlag(FLAGS_sim_log_name);
+}
+
+}  // namespace utils
diff --git a/src/utils/log_path.h b/src/utils/log_path.h
index e69de29b..8b2c749c 100644
--- a/src/utils/log_path.h
+++ b/src/utils/log_path.h
@@ -0,0 +1,9 @@
+#pragma once
+
+#include <string>
+
+namespace utils {
+
+std::string GetSimLogName();
+
+}  // namespace utils
diff --git a/src/utils/transform.cc b/src/utils/transform.cc
index 36116045..854480df 100644
--- a/src/utils/transform.cc
+++ b/src/utils/transform.cc
@@ -68,58 +68,4 @@ auto HomogenizePoint3d(cv::Point3d point) -> cv::Mat {
 template cv::Mat utils::EigenToCvMat<Eigen::Matrix<double, 4, 4>>(
     const Eigen::MatrixBase<Eigen::Matrix<double, 4, 4>>&);
 
-auto ExtractTranslationAndRotation(const Eigen::Matrix4d& transform_mat)
-    -> TransformValues {
-  double x = transform_mat(0, 3);
-  double y = transform_mat(1, 3);
-  double z = transform_mat(2, 3);
-
-  const Eigen::Matrix3d& R = transform_mat.block<3, 3>(0, 0);
-
-  double sy = std::hypot(R(0, 0), R(1, 0));
-
-  bool singular = sy < 1e-6;
-
-  double roll, pitch, yaw;
-
-  if (!singular) {
-    roll = std::atan2(R(2, 1), R(2, 2));
-    pitch = std::atan2(-R(2, 0), sy);
-    yaw = std::atan2(R(1, 0), R(0, 0));
-  } else {
-    // Gimbal lock
-    roll = std::atan2(-R(1, 2), R(1, 1));
-    pitch = std::atan2(-R(2, 0), sy);
-    yaw = 0.0;
-  }
-
-  return {x, y, z, roll, pitch, yaw};
-}
-
-auto SeparateTranslationAndRotationMatrices(
-    const TransformValues& decomposition) -> TransformDecomposition {
-  // clang-format off
-  const Eigen::Matrix4d Rx = (Eigen::Matrix4d() << 
-      1, 0, 0, 0,
-      0, cos(decomposition.rx), -sin(decomposition.rx), 0,
-      0, sin(decomposition.rx), cos(decomposition.rx), 0,
-      0, 0, 0, 1).finished();
-  const Eigen::Matrix4d Ry = (Eigen::Matrix4d() << 
-      cos(decomposition.ry), 0, sin(decomposition.ry), 0,
-      0, 1, 0, 0,
-      -sin(decomposition.ry), 0, cos(decomposition.ry), 0,
-      0, 0, 0, 1).finished();
-  const Eigen::Matrix4d Rz = (Eigen::Matrix4d() << 
-      cos(decomposition.rz), -sin(decomposition.rz), 0, 0,
-      sin(decomposition.rz), cos(decomposition.rz), 0, 0,
-      0, 0, 1, 0,
-      0, 0, 0, 1).finished();
-
-  // clang-format on
-  Eigen::Matrix4d translation = Eigen::Matrix4d::Identity();
-  translation(0, 3) = decomposition.x;
-  translation(1, 3) = decomposition.y;
-  translation(2, 3) = decomposition.z;
-  return {translation, Rx, Ry, Rz};
-}
 }  // namespace utils
diff --git a/src/utils/transform.h b/src/utils/transform.h
index d573be1d..ec5f81ed 100644
--- a/src/utils/transform.h
+++ b/src/utils/transform.h
@@ -62,28 +62,15 @@ auto inline Homogenize(const Eigen::Vector3d point) -> Eigen::Vector4d {
   return homogenized_point;
 }
 
-struct TransformValues {
-  double x;
-  double y;
-  double z;
-  double rx;
-  double ry;
-  double rz;
-};
-
-struct TransformDecomposition {
-  Eigen::Matrix4d translation;
-  Eigen::Matrix4d Rx;
-  Eigen::Matrix4d Ry;
-  Eigen::Matrix4d Rz;
-};
-
-auto ExtractTranslationAndRotation(const Eigen::Matrix4d& transform_mat)
-    -> TransformValues;
-
-auto SeparateTranslationAndRotationMatrices(const TransformValues& combined)
-    -> TransformDecomposition;
-
+auto inline CrossProduct(const Eigen::Vector3d corner) -> Eigen::Matrix3d {
+  // clang-format off
+  return (Eigen::Matrix3d() << 
+      0, -corner.z(), corner.y(),
+      corner.z(), 0, -corner.x(),
+      -corner.y(), corner.x(), 0)
+      .finished();
+  // clang-format on
+}
 inline auto PoseOffField(frc::Pose3d pose) -> bool {
   constexpr double kerror_margin = 0.2;
   return pose.X().value() < 0 - kerror_margin ||