From 3328a8a106377e38639713dd96d9d0b2be1bc11c Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sat, 20 Jun 2026 10:31:09 +0200
Subject: [PATCH 01/45] Initial commit of Camera-IMU calibration algo

---
 python/PiFinder/develop/README.md             |   4 +
 .../develop/camera_imu_alignment/README.md    | 221 ++++++++++
 .../imu_extrinsic_calibration.py              | 387 ++++++++++++++++++
 3 files changed, 612 insertions(+)
 create mode 100644 python/PiFinder/develop/README.md
 create mode 100644 python/PiFinder/develop/camera_imu_alignment/README.md
 create mode 100644 python/PiFinder/develop/camera_imu_alignment/imu_extrinsic_calibration.py

diff --git a/python/PiFinder/develop/README.md b/python/PiFinder/develop/README.md
new file mode 100644
index 000000000..ccab79156
--- /dev/null
+++ b/python/PiFinder/develop/README.md
@@ -0,0 +1,4 @@
+# `develop\` directory
+
+The `develop\` directory is for experimental code that is not used by the main
+program flow.
diff --git a/python/PiFinder/develop/camera_imu_alignment/README.md b/python/PiFinder/develop/camera_imu_alignment/README.md
new file mode 100644
index 000000000..9e6600214
--- /dev/null
+++ b/python/PiFinder/develop/camera_imu_alignment/README.md
@@ -0,0 +1,221 @@
+# Camera-IMU alignment (extrinsic calibration)
+
+To track the pointing using IMU dead-reckoning, we need to know the relative
+orientation or alignment between the camera and IMU. The development code here
+estimates the alignment.
+
+The alignment error will introduce a "jump" when the IMU dead-reckoning hands
+off to the camera solve which is (probably) approximately proportional to the
+product of the camera-IMU alignment error and the angle moved under
+dead-reckoning (in radians).
+
+See the header comments in `imu_extrinsic_calibration.py` for explanation of
+the algorithm.
+
+## Previous studies
+
+In a previous study, we used recorded telemetry data to estimate the camera-IMU
+alignment. The extrinsic calibration estimated an adjustment over the nominal
+orientation by 1.6 degrees with an uncertainty of ±0.5 degrees. This was based
+on around 1 minute of data which gave 9 samples (after outlier removal). 
+
+With 4 minutes of data, we might be able to get it down to something around
+±0.1 degrees but this might be unrealistic because it needs continual movement
+and (based on simulations) the main source of error doesn't look like
+nicely-behaved random noise but something else. The difference between the
+moment of camera exposure and the IMU measurement could be just one issue. 
+
+When compared to using the improved alignment with the nominal alignment, the
+improvement isn't that big. It cuts the angular jump by around a half, which is
+what we'd expect given the uncertainty. 
+
+Simulations with realistic noise gave much better results. This suggests that
+the accuracy of the real results may be limited by one or more of the following
+following potential root causes:
+
+1. The alignment algorithm needs to be fed with pairs of start/end samples
+with paired camera solves and IMU measurements. Outliers could introduce errors
+so better selection criteria may be needed to filter out outliers.
+2. The telemetry recording used the BNO055 IMU in fusion mode. This is known to 
+be noisy so better filtering and outlier rejections may be needed.
+3. The BNO055 is an older IMU and it may be that its poorer accuracy propagates
+to alignment inaccuracies. It is possible that a more modern IMU could give better
+alignment results. 
+4. The camera and IMU samples are assumed to be from the same time instance.
+Relative delays could introduce errors. Filtering of the IMU could also
+introduce delays.
+
+## What still needs to be done
+
+The study showed that the camera-IMU alsignment could be estimated to ±0.5
+degrees. This is good enough to replace the nominal alignments that need to be
+set in configurations.
+
+A rough alignment feature could be built based on the algorithm in this 
+directory and the sample code below.
+
+To improve the alignment accuracy to reduce the "jumps", the potential root
+causes listed above may need to be investigated.
+
+## Sample code from the Jupyter notebooks
+
+The following is a sample code from the Jupyter notebooks that was used to
+analyse the data from telemetry. It could form the basis of an implementation
+in PiFinder.
+
+
+```python
+from dataclasses import dataclass
+from enum import Enum
+import quaternion
+import numpy as np
+from pathlib import Path
+import json
+
+from astro_coords import RaDecRoll
+import quaternion_transforms as qt
+
+
+@dataclass
+class ImuData:
+    quat: quaternion.quaternion | None = None
+    gyro: list  | None = None
+    accel: list | None = None
+    
+
+@dataclass
+class SolveData:
+    camera_ra_dec_roll: RaDecRoll | None = None
+    timestamp_exposure_end: float | None = None  # seconds, from time.time()
+    imu_quat: quaternion.quaternion | None = None  # Quaternion at exposure end
+
+
+class MeasurementType(Enum):
+    CAMERA = 1
+    IMU = 2
+
+
+@dataclass
+class Sample:
+    timestamp: float | None = None  # seconds, from time.time()
+    measurement_type: MeasurementType | None = None
+    data: SolveData | ImuData | None = None
+ 
+    def set(self, timestamp: float, measurement_type: MeasurementType, data):
+        self.timestamp = timestamp
+        self.measurement_type = measurement_type
+        self.data = data
+
+    def get(self):
+        return self.timestamp, self.measurement_type, self.data
+
+
+def read_samples_from_telemetry(path: Path, n_max_samples: int | None = None) -> list[Sample]:
+    """
+    Reads samples from a telemetry file and returns a list of Sample objects.
+    Each line in the telemetry file is expected to be a JSON object with the following format:
+    {
+        "t": timestamp (float, seconds from time.time()),
+        "e": event type (string, either "imu" or "solve"),
+        "q": [w, x, y, z] (quaternion for IMU measurements),
+        "ra": right ascension (float, degrees),
+        "dec": declination (float, degrees),
+        "roll": roll angle (float, degrees)
+    }
+    """
+    samples = []
+    counter = 0
+    with open(path, 'r') as f:
+        for line in f:
+            d = json.loads(line)
+            #print(d)  # For debugging (print the raw data from the telemetry file)
+            if d["e"] == "imu":
+                q = quaternion.quaternion(*d["q"])
+                imu_data = ImuData(quat=q, gyro=d["gyro"], accel=d['accel'])
+                samples.append(Sample(timestamp=d["t"], measurement_type=MeasurementType.IMU, data=imu_data))
+            elif d["e"] == "solve":
+                ra_dec_roll = RaDecRoll(ra=d["cam_ra"], dec=d["cam_dec"], roll=d["cam_roll"], deg=True)
+                solve_data = SolveData(camera_ra_dec_roll=ra_dec_roll, 
+                                       timestamp_exposure_end=d["lss"], imu_quat=quaternion.quaternion(*d["iq"]))
+                samples.append(Sample(timestamp=d["t"], measurement_type=MeasurementType.CAMERA, data=solve_data))
+            else:
+                continue  # Skip unknown measurement types
+            
+            counter += 1
+            #print(samples[-1])  # For debugging (print the stored sample)
+            if n_max_samples is not None:
+                if counter >= n_max_samples:
+                    break
+
+    return samples
+
+def get_ang_diffs(last_camera_sample: Sample, camera_sample: Sample):
+    ang_diff_cam = qt.get_quat_angular_diff(
+        last_camera_sample.data.camera_ra_dec_roll.as_quaternion(), 
+        camera_sample.data.camera_ra_dec_roll.as_quaternion())
+    ang_diff_imu = qt.get_quat_angular_diff(
+         last_camera_sample.data.imu_quat, 
+         camera_sample.data.imu_quat)                
+
+    return ang_diff_cam, ang_diff_imu
+
+def pair_camera_imu_samples(samples: list[Sample],
+                            max_time_diff=0.1,  # [s] Maximum time difference between IMU and platesolve
+                            min_angle_diff=np.deg2rad(5),  # Reject if angle from prev. sample is less than this
+                            verbose=False
+                            ):
+    """
+    Pair up solved data (RaDecRoll) with the previous IMU sample. The time
+    difference between the IMU and camera must be small and the angular
+    movement between sequential pairs must be large enough.
+    """
+    paired_samples = []  # The result that will be returned
+    quarantined_samples = []  # Samples that were too close in angle but could be used later
+    prev_imu_idx = None
+    for idx, samp in enumerate(samples):
+        if samp.measurement_type is MeasurementType.IMU:
+            prev_imu_idx = idx
+            continue
+        elif samp.measurement_type is MeasurementType.CAMERA and prev_imu_idx is not None:
+            if not samp.data.camera_ra_dec_roll.valid:
+                continue  # Skip if camera sample is not valid
+            
+            # Skip if IMU sample is after the camera sample or large time difference:
+            #imu_sample = samples[prev_imu_idx]
+            #time_diff = samp.timestamp - imu_sample.timestamp
+            #print(f"{(time_diff)*1000:.1f} ms between IMU and camera sample")
+            #if (time_diff < 0) or (time_diff > max_time_diff):
+            #    continue
+            
+            if not paired_samples:
+                paired_samples.append(samp)
+                continue
+
+            # See if we can use the oldest quarantined sample
+            # TODO: Also add the time difference criterion to reject old samples
+            if quarantined_samples:
+                last_camera_sample = paired_samples[-1]
+                q_samp = quarantined_samples[0]
+                ang_diff_cam, ang_diff_imu = get_ang_diffs(last_camera_sample, q_samp)
+                if abs(ang_diff_imu) >= min_angle_diff and abs(ang_diff_cam) >= min_angle_diff:
+                    # Use the quarantined sample
+                    paired_samples.append(q_samp)
+                    quarantined_samples = quarantined_samples[1:]
+ 
+            # Save pairs of data if the angular difference since the previous sample is large enough
+            # Note: We could re-use these by another pairing
+            if paired_samples:
+                # Skip if angular diff too small (won't be able to solve)
+                last_camera_sample = paired_samples[-1]                
+                ang_diff_cam, ang_diff_imu = get_ang_diffs(last_camera_sample, samp)
+                #print(f"Angular difference since last sample: {np.rad2deg(ang_diff):.1f} deg")
+                if abs(ang_diff_imu) < min_angle_diff and abs(ang_diff_cam) < min_angle_diff or ang_diff_imu < min_angle_diff or ang_diff_cam < min_angle_diff:
+                    quarantined_samples.append(samp)
+                    continue
+                else:
+                    paired_samples.append(samp)
+
+            assert "Shouldn't get here"
+
+    return paired_samples
+```
\ No newline at end of file
diff --git a/python/PiFinder/develop/camera_imu_alignment/imu_extrinsic_calibration.py b/python/PiFinder/develop/camera_imu_alignment/imu_extrinsic_calibration.py
new file mode 100644
index 000000000..8c3b08641
--- /dev/null
+++ b/python/PiFinder/develop/camera_imu_alignment/imu_extrinsic_calibration.py
@@ -0,0 +1,387 @@
+"""
+Alignment of the IMU-camera axes (extrinsic calibration)
+
+For dead-reckoning with the IMU, we need the rotation between the IMU and
+camera axes. This is done by the quaternion q_cam2imu and its inverse
+q_imu2cam.
+
+The goal of this module is to estimate q_cam2imu. We can do this using pairs of
+camera and IMU orientation quaternions measured simultaneously.
+
+Required measurements
+---------------------
+
+The measurements we have are:
+
+* q_eq2cam: Quaternion rotation of the camera center relative to the equatorial
+  frame. 
+* q_x2imu: The rotation of the IMU relative to some arbibtrary reference frame
+  X.
+
+The camera and IMU measurements are paired and assumed to be simultaneous.
+  
+Algorithm:
+----------
+
+We can express the rotation between successive timesteps for the camera and
+IMU:
+
+dq_cam = q_eq2cam[k-1].conjugate() * q_eq2cam[k] dq_imu =
+q_x2imu[k-1].conjugate() * q_x2imu[k]
+
+where * is the quaternion multiplication and .conjugate() is the quaternion
+conjugate, which is equivalent to the inverse for a unit quaternion. We can
+relate the changes in orientation of the camera and IMU by
+
+dq_cam * q_cam2imu = q_cam2imu * dq_imu
+
+This is the quaternion version of the hand-eye calibration problem (better
+known  in the matrix form: AX = XB).
+
+We will solve for q_cam2imu by defining the error quaternion:
+
+q_err = (dq_cam * q_cam2imu) * (q_cam2imu * dq_imu).conjugate()
+
+In the ideal case, q_err will converge to the identity quaternion (1, 0, 0, 0)
+at the solution. Quaternions are defined by 4 parameters with one constraint.
+We will map the quaternion to a 3-parameter rotation vector, which can be
+solved more efficiently and simply. The rotation vector is the product of the
+unit vector around the axis of rotation (u) and the rotation (theta):
+
+e = theta * u = log(q_err)
+
+The optimization algorith will minimize the two-norm of the error rotation
+vector for k = 1..N measurements:
+
+sum(||e[k]||^2)
+
+
+Assumptions & limitations
+-------------------------
+
+1. Small rotation angles for dq_cam and dq_imu could cause numerical problems
+   so successive samples should be selected so that the angles are sufficiently
+   large.
+2. The IMU will drift over time so the time between the samples used to
+   calculate dq_imu should be short enough for drift to be negligible.
+3. The camera and IMU samples should be taken simultaneously. If the camera
+   moves during exposure, this will introduce an error. Error could be reduced
+   by used samples when the camera movement is reasonably stationary.
+4. In practice, the plate solver will have worse error in roll than RA and Dec.
+   This is not accounted for.
+5. Ideally, the camera/IMU should be rotated around all three axes but on a
+   mount, the rotation will likely be around two axes. This may result in a 
+   larger uncertainty for the rotation/alignment about some axes.
+"""
+
+import numpy as np
+import quaternion  # Note: numpy-quaternion convention: quaternion(w, x, y, z)
+from scipy.optimize import least_squares
+import time
+from typing import Union
+
+import sandbox.pointing_model.quaternion_transforms as qt
+
+list_of_quats = list[quaternion.quaternion]
+
+
+def ensure_quat_continuity(q_list: list_of_quats) -> list_of_quats:
+    """
+    Ensures that consecutive quaternions in the list have consistent signs (due
+    to the double coverage property of quaternions where q and -q represent
+    same rotation).
+    """
+    q_list_out = [q_list[0]]
+    for q in q_list[1:]:
+        q_prev = quaternion.as_float_array(q_list_out[-1])
+        q_curr = quaternion.as_float_array(q)
+
+        if np.dot(q_prev, q_curr) < 0:
+            q = -q
+        q_list_out.append(q)
+
+    return q_list_out
+
+
+def build_relative_rotations(q_list: list_of_quats, step=1) -> list_of_quats:
+    """
+    Calculate the relative rotation between successive quaternions:
+    dq[k] = q[k].conjugate() * q[k+step]
+    """
+    dq = []
+    for k in range(len(q_list) - step):
+        q_rel = q_list[k].conjugate() * q_list[k + step]
+        dq.append(q_rel)
+
+    return dq
+
+
+def reject_small_rotations(dq_cam: list_of_quats, 
+                           dq_imu: list_of_quats,
+                           min_rotation=np.deg2rad(1.0),  # Reject rotations below this [radians]
+                           ) -> tuple[list_of_quats, list_of_quats]:
+    """
+    Reject small rotations
+    """
+    keep_dq_cam = []
+    keep_dq_imu = []
+    for qc, qi in zip(dq_cam, dq_imu):
+        angle_cam = np.linalg.norm(quaternion.as_rotation_vector(qc))
+        angle_imu = np.linalg.norm(quaternion.as_rotation_vector(qi))
+
+        if angle_cam >= min_rotation or angle_imu >= min_rotation:
+            keep_dq_cam.append(qc)
+            keep_dq_imu.append(qi)
+
+    return keep_dq_cam, keep_dq_imu
+
+
+def residual_rotation_vector(x,  # (3,) Trial solution (q as rotation vector) 
+                             dq_cam: list_of_quats,  # List of relative camera rotation quaternions
+                             dq_imu: list_of_quats  # List of relative IMU rotation quaternions
+                             ) -> np.ndarray:
+    """
+    Calculate the esiduals at the trial solution x for least squares
+    optimization.
+
+    For solving q_cam2imu in the quaternion form of the hand-eye problem: 
+    dq_cam * q_cam2imu = q_cam2imu * dq_imu
+    """
+    # Convert trial solution (rotation vector) to quaternion
+    q_cam2imu = quaternion.from_rotation_vector(x)
+
+    n_meas = len(dq_cam)
+    residuals = np.zeros(3 * n_meas)
+    for ii, (qc, qi) in enumerate(zip(dq_cam, dq_imu)):
+        q_left = qc * q_cam2imu
+        q_right = q_cam2imu * qi
+
+        # Error quaternion
+        q_err = q_left * q_right.conjugate()
+
+        # Convert to rotation vector (Lie algebra logarithm map)
+        residuals[(3 * ii):(3 * ii + 3)] = quaternion.as_rotation_vector(q_err)
+
+    return np.array(residuals)
+
+
+N_UNKNOWN = 3
+
+def calibrate_camera_imu(
+        q_cam: list_of_quats,  # Camera orientations
+        q_imu: list_of_quats,  # IMU orientations at same moments
+        step: int = 1,  # Skip successive measurements
+        min_rotation=np.deg2rad(1.0),  # Reject rotations below this [radians]
+        x0: Union[np.ndarray, list] = np.zeros(N_UNKNOWN),  # Initial guess
+        residual_threshold = 0.01,  # Reject samples with residual > resid_threshold in first pass
+        verbose=True
+        ):
+    """
+    Estimate q_cam2imu from pairs of simultaneous camera and IMU measurements
+    q_cam and q_imu (as quaternions).
+
+    RETURNS:
+    q_cam2imu: [quaternion.quaternion] Camera-to-IMU rotation estimate
+    sigma_total: [rad] Total rotaion uncertainty
+    condition_number: < 10 excellent, < 100 acceptable, <1E4 weak observability
+
+    TODO: 
+    - Add checks to fail gracefully if there aren't enough data points.
+    - Remove outliers
+    """
+    t_start = time.time()
+
+    # Enforce quaternion continuity
+    q_cam = ensure_quat_continuity(q_cam)
+    q_imu = ensure_quat_continuity(q_imu)
+
+    # Calculate relative rotations between successive quaternions
+    dq_cam = build_relative_rotations(q_cam, step)
+    dq_imu = build_relative_rotations(q_imu, step)
+
+    # Reject rotation angle < min_rotation
+    reject_small_rotations(dq_cam, dq_imu, min_rotation=min_rotation)
+    # TODO: Convert print() to logging
+    print(f"{len(q_cam)} measurements. Using {len(dq_cam)} pairs for camera-IMU calibration.")
+    # Calculate angular differences for logging
+    d_thetas = []
+    for ii, dq in enumerate(dq_cam):
+        if ii > 0:
+            d_thetas.append(qt.get_quat_angular_diff(prev_dq, dq))
+        prev_dq = dq
+    print(f"Angular rotations: {np.rad2deg(np.min(np.abs(d_thetas))):.2f} to " 
+          f"{np.rad2deg(np.max(np.abs(d_thetas))):.2f} deg. "
+          f"Median: {np.rad2deg(np.median(np.abs(d_thetas))):.2f} deg.")
+
+    # Solve for x by non-linear least squares (Levenberg-Marquardt)
+    # TODO: Tune LM params
+    # TODO: Calculate the Jacobians analytically? Current numerical Jacobians is probably fast enough?
+    result = least_squares(residual_rotation_vector, x0, method='lm', 
+                           args=(dq_cam, dq_imu))
+    # TODO: Investigate using robust loss functions?
+    #result = least_squares(residual_rotation_vector, x0, loss='cauchy', 
+    #                       args=(dq_cam, dq_imu))
+
+    # Re-run least-squares with outliers removed
+    if residual_threshold is not None:
+        # NOTE: Each quaternion measurement is converted to rotation vectors with 3 values
+        resid_reshaped = result.fun.reshape(-1, 3)  # Each row is a sample
+        msk_accept = np.all(np.abs(resid_reshaped) < residual_threshold, axis=1)
+        dq_cam_accept = np.array(dq_cam)[msk_accept]
+        dq_imu_accept = np.array(dq_imu)[msk_accept]
+        if verbose:
+            print(f"Accepted {np.sum(msk_accept)}/{resid_reshaped.shape[0]} samples.")
+        # Run least-squares again (using previous solution as the initial guess)
+        result = least_squares(residual_rotation_vector, result.x, 
+                               args=(dq_cam_accept, dq_imu_accept))
+
+    # Convert estimate from rotatino vector to quaternion
+    q_cam2imu = quaternion.from_rotation_vector(result.x)
+    t_compute = time.time() - t_start
+    
+    if verbose:
+        print(f"Estimated q_cam2imu: q_cam2imu={q_cam2imu}, compute time = {t_compute:.3f}s ",
+            f"Func evaluations: {result.nfev}, Cost = {result.cost:.4g}, ", 
+            f"Success: {result.success}, {result.message}")
+
+    # Diagnostics
+    sigma_total, condition_number = _solution_diagnostics(result)
+    residuals = result.fun
+
+    return q_cam2imu, sigma_total, residuals, condition_number
+
+
+def _solution_diagnostics(result):
+    """ 
+    Calculate the diagnostics of the least-squares solution. The input, 
+    `result` is the output from scipy.optimize.least_squares.
+    
+    Condition number: < 10 excellent, < 100 acceptable, <1E4 weak observability
+    """
+    t_start = time.time()
+
+    # Estimate the uncertainty of the solution
+    residuals = result.fun
+    dof = len(residuals) - len(result.x)  # Degrees-of-freedom = Number of meas - Number of params
+    residuals_var = np.sum(residuals**2) / dof  # Estimate of residual variance
+
+    # Using 'backslash' rather than inv(): Faster but could be unstable?
+    #JTJ = result.jac.T @ result.jac  # Hessian approx from the Jacobians
+    #cov_x = residuals_var * np.linalg.solve(JTJ, np.eye(JTJ.shape[0]))  
+    
+    # Estimate the uncertainty at the solution using SVD: More robust
+    U, s, Vt = np.linalg.svd(result.jac, full_matrices=False)
+    cov_x = residuals_var * (Vt.T / s**2) @ Vt
+    condition_number = s[0] / s[-1]
+    sigma_total = np.sqrt(np.trace(cov_x))  # [rad] Total rotaion uncertainty
+
+    t_compute = time.time() - t_start
+    print(f"Diagnostics for q_cam2imu: compute time = {t_compute:.3f}s, ",
+          f"Total angular uncertainty = {np.rad2deg(sigma_total):.2} deg, ",
+          f"Condition number = {condition_number:.1g}")
+
+    return sigma_total, condition_number
+
+# ------ Simulation functions for testing & analysis --------------------------
+
+def _q_noise(noise_amp: float):
+    """ Generates random quaternion noise. Noise amp is in radians """
+    noise = np.radians(noise_amp) * np.random.randn(3)
+    return quaternion.from_rotation_vector(noise)
+
+
+def _add_noise_to_quaternion_list(qs: list_of_quats, noise_amp: float):
+    """ Adds noise to a list of quaternions. noise_amp is in radians. """
+    qs_out = []
+    for q in qs:
+        qs_out.append(_q_noise(noise_amp) * q)
+
+    return qs_out
+
+def _random_quaternions(N: int, max_rot=None) -> list_of_quats:
+    """ 
+    Returns a list of N random quaternions. If max_rot is None, the quaternions
+    will be random. If specified, it limits the maximum swing angle from the
+    previous orientation.
+    """
+    qs = []
+    for ii in range(N):
+        axis = np.random.randn(3)
+        axis /= np.linalg.norm(axis)
+
+        if (max_rot is None) or (ii == 0):
+            angle = np.random.uniform(0, np.pi)
+            q = quaternion.from_rotation_vector(axis * angle)
+        else:
+            angle = np.random.uniform(0, max_rot)
+            dq = quaternion.from_rotation_vector(axis * angle)
+            q = qs[-1] * dq
+
+        qs.append(q)
+    
+    return qs
+
+
+def simulate_measurements(q_cam2imu: quaternion.quaternion,  # True q_cam2imu (camera-to-IMU alignment)
+                          N: int = 100,  # Number of samples to simulate
+                          max_rot = None,  # Max rotation from previous orientation
+                          camera_noise_amp: float = np.deg2rad(0.1),  # Camera noise amp in radians
+                          imu_noise_amp: float = np.deg2rad(0.1),  # IMU noise amp in radians
+                          seed=0  # Random seed. None to disable
+                          ):
+    """
+    Simulate camera and IMU measurements
+    """
+    if seed is not None:
+        np.random.seed(seed)
+        
+    # Generate random IMU orientations
+    q_imu_true = _random_quaternions(N, max_rot=max_rot)
+
+    # Generate corresponding camera orientations
+    q_imu2cam = q_cam2imu.conjugate()
+    q_cam_true = []
+    for q in q_imu_true:
+        q_cam_true.append(q * q_imu2cam)
+
+    # Add noise
+    q_cam = _add_noise_to_quaternion_list(q_cam_true, camera_noise_amp)
+    q_imu = _add_noise_to_quaternion_list(q_imu_true, imu_noise_amp)
+
+    return q_cam, q_imu
+
+
+if __name__ == "__main__":
+    """ 
+    The main block simulates pairs of random IMU/camera measurements and solves
+    for the camera-to-IMU alignment (q_cam2imu).
+    """
+
+    # Set the true camera-from-body rotation
+    true_rotvec = np.radians([10, -5, 20])
+    q_cam2imu_true = quaternion.from_rotation_vector(true_rotvec)
+
+    # Simulate measurements:
+    q_cam, q_imu = simulate_measurements(
+        q_cam2imu_true, N=100, camera_noise_amp=np.deg2rad(0.1), 
+        imu_noise_amp=np.deg2rad(0.1), seed=0)
+
+    # Calibrate
+    q_est, sigma_total, condition_number = calibrate_camera_imu(
+        q_cam, q_imu, step=2, min_rotation=np.deg2rad(1.0))
+
+    # Results
+    print("\nTrue q_cam2imu:")
+    print(quaternion.as_float_array(q_cam2imu_true))
+
+    print("\nEstimated q_cam2imu:")
+    print(quaternion.as_float_array(q_est))
+
+    # Error
+    q_error = q_est.conjugate() * q_cam2imu_true
+    error_deg = np.rad2deg(
+        np.linalg.norm(
+            quaternion.as_rotation_vector(q_error)
+        )
+    )
+    print(f"\nCalibration error: {error_deg:.6f} deg")

From d1065b6b7f6026edb0ece00ebb1b95004d70a796 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sat, 20 Jun 2026 10:40:38 +0200
Subject: [PATCH 02/45] Fix & add comments

---
 python/PiFinder/develop/__init__.py                       | 0
 python/PiFinder/develop/camera_imu_alignment/__init__.py  | 0
 .../camera_imu_alignment/imu_extrinsic_calibration.py     | 8 +++++---
 3 files changed, 5 insertions(+), 3 deletions(-)
 create mode 100644 python/PiFinder/develop/__init__.py
 create mode 100644 python/PiFinder/develop/camera_imu_alignment/__init__.py

diff --git a/python/PiFinder/develop/__init__.py b/python/PiFinder/develop/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/python/PiFinder/develop/camera_imu_alignment/__init__.py b/python/PiFinder/develop/camera_imu_alignment/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/python/PiFinder/develop/camera_imu_alignment/imu_extrinsic_calibration.py b/python/PiFinder/develop/camera_imu_alignment/imu_extrinsic_calibration.py
index 8c3b08641..8d815aa00 100644
--- a/python/PiFinder/develop/camera_imu_alignment/imu_extrinsic_calibration.py
+++ b/python/PiFinder/develop/camera_imu_alignment/imu_extrinsic_calibration.py
@@ -80,7 +80,7 @@
 import time
 from typing import Union
 
-import sandbox.pointing_model.quaternion_transforms as qt
+import PiFinder.pointing_model.quaternion_transforms as qt
 
 list_of_quats = list[quaternion.quaternion]
 
@@ -90,6 +90,8 @@ def ensure_quat_continuity(q_list: list_of_quats) -> list_of_quats:
     Ensures that consecutive quaternions in the list have consistent signs (due
     to the double coverage property of quaternions where q and -q represent
     same rotation).
+
+    TODO: Possibly move this to quaternion_transforms?
     """
     q_list_out = [q_list[0]]
     for q in q_list[1:]:
@@ -165,14 +167,14 @@ def residual_rotation_vector(x,  # (3,) Trial solution (q as rotation vector)
     return np.array(residuals)
 
 
-N_UNKNOWN = 3
+N_UNKNOWN_PARAMS = 3  # Number of unknown parameters in the problem to solve
 
 def calibrate_camera_imu(
         q_cam: list_of_quats,  # Camera orientations
         q_imu: list_of_quats,  # IMU orientations at same moments
         step: int = 1,  # Skip successive measurements
         min_rotation=np.deg2rad(1.0),  # Reject rotations below this [radians]
-        x0: Union[np.ndarray, list] = np.zeros(N_UNKNOWN),  # Initial guess
+        x0: Union[np.ndarray, list] = np.zeros(N_UNKNOWN_PARAMS),  # Initial guess
         residual_threshold = 0.01,  # Reject samples with residual > resid_threshold in first pass
         verbose=True
         ):

From 1d436364373d7d77dec214133797a9aa42404d2b Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sun, 2 Aug 2026 09:35:08 +0200
Subject: [PATCH 03/45] Move camera/IMU alignment code to imu/

---
 python/PiFinder/develop/README.md                             | 4 ----
 python/PiFinder/develop/camera_imu_alignment/__init__.py      | 0
 .../PiFinder/{develop/camera_imu_alignment => imu}/README.md  | 0
 python/PiFinder/{develop => imu}/__init__.py                  | 0
 .../camera_imu_alignment => imu}/imu_extrinsic_calibration.py | 0
 5 files changed, 4 deletions(-)
 delete mode 100644 python/PiFinder/develop/README.md
 delete mode 100644 python/PiFinder/develop/camera_imu_alignment/__init__.py
 rename python/PiFinder/{develop/camera_imu_alignment => imu}/README.md (100%)
 rename python/PiFinder/{develop => imu}/__init__.py (100%)
 rename python/PiFinder/{develop/camera_imu_alignment => imu}/imu_extrinsic_calibration.py (100%)

diff --git a/python/PiFinder/develop/README.md b/python/PiFinder/develop/README.md
deleted file mode 100644
index ccab79156..000000000
--- a/python/PiFinder/develop/README.md
+++ /dev/null
@@ -1,4 +0,0 @@
-# `develop\` directory
-
-The `develop\` directory is for experimental code that is not used by the main
-program flow.
diff --git a/python/PiFinder/develop/camera_imu_alignment/__init__.py b/python/PiFinder/develop/camera_imu_alignment/__init__.py
deleted file mode 100644
index e69de29bb..000000000
diff --git a/python/PiFinder/develop/camera_imu_alignment/README.md b/python/PiFinder/imu/README.md
similarity index 100%
rename from python/PiFinder/develop/camera_imu_alignment/README.md
rename to python/PiFinder/imu/README.md
diff --git a/python/PiFinder/develop/__init__.py b/python/PiFinder/imu/__init__.py
similarity index 100%
rename from python/PiFinder/develop/__init__.py
rename to python/PiFinder/imu/__init__.py
diff --git a/python/PiFinder/develop/camera_imu_alignment/imu_extrinsic_calibration.py b/python/PiFinder/imu/imu_extrinsic_calibration.py
similarity index 100%
rename from python/PiFinder/develop/camera_imu_alignment/imu_extrinsic_calibration.py
rename to python/PiFinder/imu/imu_extrinsic_calibration.py

From 9341db08c0d684fb8bb0883ebd4f349ea2711e75 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sun, 2 Aug 2026 10:47:18 +0200
Subject: [PATCH 04/45] Comment where the changes need to be made in integrator

---
 python/PiFinder/integrator.py | 17 +++++++++++++++--
 1 file changed, 15 insertions(+), 2 deletions(-)

diff --git a/python/PiFinder/integrator.py b/python/PiFinder/integrator.py
index a2b2a85ac..5aca7136b 100644
--- a/python/PiFinder/integrator.py
+++ b/python/PiFinder/integrator.py
@@ -146,6 +146,18 @@ def integrator(
                 )
                 estimate = _apply_successful_solve(estimate, solve_result, idr)
                 pointing_updated = True
+
+                # Append plate-solve and IMU states to IMU/camera alignment buffer
+                # TODO: Append the following:
+                # solve_result.last_solve_success (timestamp)
+                # solve_result.camera.as_radecroll() (RaDecRoll type)
+                # solve_result.imu_anchor
+                #
+                # Update idr.q_imu2cam with the new estimate from IMU/camera alignment
+                #
+                # TODO: SuccessfulSolve.last_solve_success is the exposure end time. It's ambiguous...
+                # TODO: Move ImuDeadReckoning._q_imu2cam() to a stand-alone func in imu_dead_reckoning.py with a view to deprecating it 
+
             elif isinstance(solve_result, FailedSolve):
                 telemetry.record_solve(
                     solve_result, predicted=estimate.pointing.aligned.estimate
@@ -250,11 +262,12 @@ def _apply_successful_solve(
     estimate.matched_stars = result.matched_stars
     estimate.matched_catID = result.matched_catID
 
-    # Reseed the dead-reckoner from the new anchor. camera/aligned are
-    # always present on a SuccessfulSolve, so no None-guard is needed.
+    # Reset the dead-reckoning from the plate-solved pointing. camera/aligned
+    # are always present on a SuccessfulSolve, so no None-guard is needed.
     q_anchor = result.imu_anchor
     if q_anchor is None:
         q_anchor = quaternion.quaternion(np.nan)
+
     idr.solve(
         result.camera.as_radecroll(),
         result.aligned.as_radecroll(),

From d9c4c8e4d6f793a8767d2c4f8f6c9d7f9dccc57c Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sun, 2 Aug 2026 10:59:05 +0200
Subject: [PATCH 05/45] Refactor ensure_quat_continuity(): Move to
 quaternion_transforms.py

---
 python/PiFinder/imu/imu_extrinsic_calibration.py  | 14 ++++----------
 .../pointing_model/quaternion_transforms.py       | 15 +++++++++++++++
 2 files changed, 19 insertions(+), 10 deletions(-)

diff --git a/python/PiFinder/imu/imu_extrinsic_calibration.py b/python/PiFinder/imu/imu_extrinsic_calibration.py
index 8d815aa00..b552087fa 100644
--- a/python/PiFinder/imu/imu_extrinsic_calibration.py
+++ b/python/PiFinder/imu/imu_extrinsic_calibration.py
@@ -85,21 +85,15 @@
 list_of_quats = list[quaternion.quaternion]
 
 
-def ensure_quat_continuity(q_list: list_of_quats) -> list_of_quats:
+def ensure_quat_list_continuity(q_list: list_of_quats) -> list_of_quats:
     """
     Ensures that consecutive quaternions in the list have consistent signs (due
     to the double coverage property of quaternions where q and -q represent
     same rotation).
-
-    TODO: Possibly move this to quaternion_transforms?
     """
     q_list_out = [q_list[0]]
     for q in q_list[1:]:
-        q_prev = quaternion.as_float_array(q_list_out[-1])
-        q_curr = quaternion.as_float_array(q)
-
-        if np.dot(q_prev, q_curr) < 0:
-            q = -q
+        q = qt.ensure_quat_continuity(q_list_out[-1], q)
         q_list_out.append(q)
 
     return q_list_out
@@ -194,8 +188,8 @@ def calibrate_camera_imu(
     t_start = time.time()
 
     # Enforce quaternion continuity
-    q_cam = ensure_quat_continuity(q_cam)
-    q_imu = ensure_quat_continuity(q_imu)
+    q_cam = ensure_quat_list_continuity(q_cam)
+    q_imu = ensure_quat_list_continuity(q_imu)
 
     # Calculate relative rotations between successive quaternions
     dq_cam = build_relative_rotations(q_cam, step)
diff --git a/python/PiFinder/pointing_model/quaternion_transforms.py b/python/PiFinder/pointing_model/quaternion_transforms.py
index 7177643b4..bf82654df 100644
--- a/python/PiFinder/pointing_model/quaternion_transforms.py
+++ b/python/PiFinder/pointing_model/quaternion_transforms.py
@@ -62,6 +62,21 @@ def get_quat_angular_diff(
     return d_theta  # In radians
 
 
+def ensure_quat_continuity(q_prev: quaternion.quaternion, 
+                           q_new: quaternion.quaternion) -> quaternion.quaternion:
+    """
+    Ensures that consecutive quaternions to have consistent signs (due
+    to the double coverage property of quaternions where q and -q represent
+    same rotation).
+    """
+    q0 = quaternion.as_float_array(q_prev)
+    q1 = quaternion.as_float_array(q_new)
+
+    if np.dot(q0, q1) < 0:
+        return quaternion.quaternion(-q1)
+    else:
+        return q_new
+
 # ========== Equatorial frame functions ============================
 
 

From 4bea85fd33d3f0ef22d707061b6ce5ec20315a94 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sun, 2 Aug 2026 21:11:07 +0200
Subject: [PATCH 06/45] Rename

---
 ...{imu_extrinsic_calibration.py => solve_imu_alignment.py} | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)
 rename python/PiFinder/imu/{imu_extrinsic_calibration.py => solve_imu_alignment.py} (99%)

diff --git a/python/PiFinder/imu/imu_extrinsic_calibration.py b/python/PiFinder/imu/solve_imu_alignment.py
similarity index 99%
rename from python/PiFinder/imu/imu_extrinsic_calibration.py
rename to python/PiFinder/imu/solve_imu_alignment.py
index b552087fa..ff038c20b 100644
--- a/python/PiFinder/imu/imu_extrinsic_calibration.py
+++ b/python/PiFinder/imu/solve_imu_alignment.py
@@ -73,7 +73,7 @@
    mount, the rotation will likely be around two axes. This may result in a 
    larger uncertainty for the rotation/alignment about some axes.
 """
-
+import logging
 import numpy as np
 import quaternion  # Note: numpy-quaternion convention: quaternion(w, x, y, z)
 from scipy.optimize import least_squares
@@ -84,6 +84,8 @@
 
 list_of_quats = list[quaternion.quaternion]
 
+logger = logging.getLogger("IMU.Integrator")
+
 
 def ensure_quat_list_continuity(q_list: list_of_quats) -> list_of_quats:
     """
@@ -249,7 +251,7 @@ def calibrate_camera_imu(
 
 def _solution_diagnostics(result):
     """ 
-    Calculate the diagnostics of the least-squares solution. The input, 
+    Returns the diagnostics of the least-squares solution. The input, 
     `result` is the output from scipy.optimize.least_squares.
     
     Condition number: < 10 excellent, < 100 acceptable, <1E4 weak observability

From 524e128c4768c537de0b32a9e82e8201521e9b74 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Mon, 3 Aug 2026 17:03:04 +0200
Subject: [PATCH 07/45] Fix RaDecRoll.from_quaternion(): Set valid to True

---
 python/PiFinder/types/coordinates.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/python/PiFinder/types/coordinates.py b/python/PiFinder/types/coordinates.py
index 1ed9da611..62c9ecdfe 100644
--- a/python/PiFinder/types/coordinates.py
+++ b/python/PiFinder/types/coordinates.py
@@ -32,7 +32,7 @@ def __init__(self, ra: float, dec: float, roll: float, deg=False):
     @classmethod
     def from_quaternion(cls, q_eq: quaternion.quaternion):
         ra, dec, roll = q_eq2radec(q_eq)
-        return cls(ra, dec, roll)
+        return cls(ra=ra, dec=dec, roll=roll, valid=True)
 
     def reset(self):
         """Reset to unset state"""

From 62150bc7ff027de23e8bc472be396f558ebf7d63 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Mon, 3 Aug 2026 20:11:44 +0200
Subject: [PATCH 08/45] Commit preprocess_imu_alignment.py

---
 .../PiFinder/imu/preprocess_imu_alignment.py  | 166 ++++++++++++++++++
 1 file changed, 166 insertions(+)
 create mode 100644 python/PiFinder/imu/preprocess_imu_alignment.py

diff --git a/python/PiFinder/imu/preprocess_imu_alignment.py b/python/PiFinder/imu/preprocess_imu_alignment.py
new file mode 100644
index 000000000..2eb9c1821
--- /dev/null
+++ b/python/PiFinder/imu/preprocess_imu_alignment.py
@@ -0,0 +1,166 @@
+"""
+Pre-processing steps for IMU/camera alignment (extrinsic alignment)
+
+Prepares the IMU/camera samples that can be used to solve for the alignment. 
+
+
+"""
+import numpy as np
+import quaternion
+from dataclasses import dataclass
+
+from PiFinder.types.coordinates import RaDecRoll
+from PiFinder.pointing_model import quaternion_transforms as qt
+
+@dataclass
+class CameraImuSample:
+    """
+    """
+    timestamp: float
+    q_cam: quaternion.quaternion
+    q_imu: quaternion.quaternion
+
+
+class SampleBuffer:
+    """
+    Buffer of samples
+    """
+    buffer: list
+    max_buffer_length: int
+
+    def __init__(self, max_buffer_length=10):
+        self.max_buffer_length = max_buffer_length
+        self.reset_buffer()
+
+    def reset_buffer(self):
+        self.buffer = []
+
+    @property
+    def len(self):
+        """Number of samples in buffer"""
+        return len(self.buffer)
+
+    def add_sample(self, sample: CameraImuSample):
+        if len(self.samples) >= self.max_buffer_length:
+            self.samples.pop(0)  # Remove oldest sample from buffer
+        self.buffer.append(sample)
+
+    def pop_sample(self, idx: int):
+        """Remove and return the sample at the given index"""
+        return self.buffer.pop(idx)
+    
+    def remove_samples(self, idx_list: list[int]):
+        """Remove multiple samples by indices"""
+        self.buffer = [self.buffer[i] for i in range(len(self.buffer)) if i not in idx_list]
+
+    def trim_to_max_length(self):
+        if self.len > self.max_buffer_length:
+            self.buffer = self.buffer[-self.max_buffer_length:]
+
+
+class ImuCameraAlignment:
+    """
+    """
+    candidate_buffer: SampleBuffer  # Buffer of camera/IMU samples
+    diff_buffer: SampleBuffer  # Buffer of paired differences in camera/IMU samples
+
+    max_time_diff: float  # [s] Maximum time difference between pairs of samples
+    min_angle_diff: float  # [rad] Pair samples with large enough angle difference
+    max_age: float  # [s] Maximum age of sample compared to current time
+
+    def __init__(self, candidate_buffer_length=10, diff_buffer_length=10,
+                 max_time_diff=10, min_angle_diff=np.deg2rad(5), max_age=1200):
+        self.candidate_buffer = SampleBuffer(max_buffer_length=candidate_buffer_length)
+        self.diff_buffer = SampleBuffer(max_buffer_length=diff_buffer_length)
+
+        self.max_time_diff = max_time_diff
+        self.min_angle_diff = min_angle_diff
+        self.max_age = max_age
+
+    def reset_buffers(self):
+        self.candidate_buffer.reset_buffer()
+        self.diff_buffer.reset_buffer()
+
+    def trim_buffers(self):
+        self.candidate_buffer.trim_to_max_length()
+        self.diff_buffer.trim_to_max_length()
+
+    def add_sample(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
+        """
+        Add to the candidate_buffer the camera solve & corresponding IMU sample
+        from integrator.
+        """
+        if timestamp is None or cam_eq is None or cam_eq.valid is False or q_x2imu is None:
+            return
+        self.candidate_buffer.add_sample(
+            CameraImuSample(timestamp, cam_eq.as_quaternion(), q_x2imu))
+
+    def purge_old_samples(self, current_time: float):
+        """
+        Remove samples from the candidate_buffer that are older than the current
+        time.
+        """
+        allowed_timestamp = current_time - self.max_age  # Purge anything older than this
+        
+        # Purge candidate_buffer:
+        remove_idx_list = [i for i, samp in enumerate(self.candidate_buffer) 
+                           if samp.timestamp < allowed_timestamp]
+        if remove_idx_list:
+            self.candidate_buffer.remove_samples(remove_idx_list)
+
+        # Purge diff_buffer:
+        remove_idx_list = [i for i, (samp1, samp2) in enumerate(self.diff_buffer) 
+                           if samp1.timestamp < allowed_timestamp 
+                           or samp2.timestamp < allowed_timestamp]
+        if remove_idx_list:
+            self.diff_buffer.remove_samples(remove_idx_list)
+
+    def pair_samples(self):
+        """
+        Go through the candidate_buffer from the first sample in the buffer.
+        Pair two sets of camera/IMU samples from the candidate buffer that meet
+        the criteria and remove them from the buffer. Repeat all pairable
+        samples have been removed from the candidate_buffer.
+        """
+        remove_idx_list = []
+        for isamp1, samp1 in enumerate(self.candidate_buffer[:-1]):
+            if isamp1 in remove_idx_list:
+                continue
+
+            for isamp2 in range(isamp1 + 1, self.candidate_buffer.len):
+                if isamp2 in remove_idx_list:
+                    continue
+                samp2 = self.candidate_buffer[isamp2]
+
+                # Check time difference between samples:
+                dt = samp2.timestamp - samp1.timestamp
+                if dt > self.max_time_diff:
+                    continue  # Samples too far apart in time
+                if dt <= 0:
+                    # Duplicate samples or sample1 is newer. Remove sample1
+                    remove_idx_list.append(isamp1)
+                    continue
+                
+                # Check angle difference (from camera solve) between samples:
+                dtheta = qt.get_quat_angular_diff(samp1.q_cam, samp2.q_cam)
+                if np.abs(dtheta) < self.min_angle_diff:
+                    continue  # Samples too close in angle
+
+                # Pair samples and remove from candidate buffer:
+                self.diff_buffer.add_sample((samp1, samp2))
+                remove_idx_list.append(isamp1)
+                remove_idx_list.append(isamp2)
+
+        if remove_idx_list:
+            self.candidate_buffer.remove_samples(remove_idx_list)
+        self.trim_buffers()  # Clean up
+
+    def solve(self, n_pairs=None):
+        """
+        Solve for the alignment between the camera and IMU using the last
+        n_pairs or all available pairs (if None).
+        """
+        if n_pairs is None:
+            n_pairs = self.diff_buffer.len
+        
+

From e83cc3ac59901925f74d2a4e42997cdd2a66d3ed Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Mon, 3 Aug 2026 20:15:52 +0200
Subject: [PATCH 09/45] Rename

---
 .../imu/{preprocess_imu_alignment.py => imu_alignment.py}     | 4 ++++
 .../imu/{solve_imu_alignment.py => imu_alignment_solver.py}   | 2 +-
 2 files changed, 5 insertions(+), 1 deletion(-)
 rename python/PiFinder/imu/{preprocess_imu_alignment.py => imu_alignment.py} (99%)
 rename python/PiFinder/imu/{solve_imu_alignment.py => imu_alignment_solver.py} (99%)

diff --git a/python/PiFinder/imu/preprocess_imu_alignment.py b/python/PiFinder/imu/imu_alignment.py
similarity index 99%
rename from python/PiFinder/imu/preprocess_imu_alignment.py
rename to python/PiFinder/imu/imu_alignment.py
index 2eb9c1821..06799af74 100644
--- a/python/PiFinder/imu/preprocess_imu_alignment.py
+++ b/python/PiFinder/imu/imu_alignment.py
@@ -5,6 +5,7 @@
 
 
 """
+import logging
 import numpy as np
 import quaternion
 from dataclasses import dataclass
@@ -12,6 +13,9 @@
 from PiFinder.types.coordinates import RaDecRoll
 from PiFinder.pointing_model import quaternion_transforms as qt
 
+logger = logging.getLogger("IMU.Align")
+
+
 @dataclass
 class CameraImuSample:
     """
diff --git a/python/PiFinder/imu/solve_imu_alignment.py b/python/PiFinder/imu/imu_alignment_solver.py
similarity index 99%
rename from python/PiFinder/imu/solve_imu_alignment.py
rename to python/PiFinder/imu/imu_alignment_solver.py
index ff038c20b..7f8708664 100644
--- a/python/PiFinder/imu/solve_imu_alignment.py
+++ b/python/PiFinder/imu/imu_alignment_solver.py
@@ -84,7 +84,7 @@
 
 list_of_quats = list[quaternion.quaternion]
 
-logger = logging.getLogger("IMU.Integrator")
+logger = logging.getLogger("IMU.Align")
 
 
 def ensure_quat_list_continuity(q_list: list_of_quats) -> list_of_quats:

From 1091adac6f6a06d75a85f4baa0ede83074bf876b Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Mon, 3 Aug 2026 20:30:22 +0200
Subject: [PATCH 10/45] Transfer functionality to imu_alignment.py and leave
 pure solver functionalities in imu_alignment_solve.py

---
 python/PiFinder/imu/imu_alignment.py        | 74 ++++++++++++++++-
 python/PiFinder/imu/imu_alignment_solver.py | 88 ++-------------------
 2 files changed, 78 insertions(+), 84 deletions(-)

diff --git a/python/PiFinder/imu/imu_alignment.py b/python/PiFinder/imu/imu_alignment.py
index 06799af74..7989fe845 100644
--- a/python/PiFinder/imu/imu_alignment.py
+++ b/python/PiFinder/imu/imu_alignment.py
@@ -1,9 +1,77 @@
 """
-Pre-processing steps for IMU/camera alignment (extrinsic alignment)
+Alignment of the IMU-camera axes (extrinsic calibration)
 
-Prepares the IMU/camera samples that can be used to solve for the alignment. 
+For dead-reckoning with the IMU, we need the rotation between the IMU and
+camera axes. This is done by the quaternion q_cam2imu and its inverse
+q_imu2cam.
 
+The goal of this module is to estimate q_cam2imu. We can do this using pairs of
+camera and IMU orientation quaternions measured simultaneously.
 
+Required measurements
+---------------------
+
+The measurements we have are:
+
+* q_eq2cam: Quaternion rotation of the camera center relative to the equatorial
+  frame. 
+* q_x2imu: The rotation of the IMU relative to some arbibtrary reference frame
+  X.
+
+The camera and IMU measurements are paired and assumed to be simultaneous.
+  
+Algorithm:
+----------
+
+We can express the rotation between successive timesteps for the camera and
+IMU:
+
+dq_cam = q_eq2cam[k-1].conjugate() * q_eq2cam[k] dq_imu =
+q_x2imu[k-1].conjugate() * q_x2imu[k]
+
+where * is the quaternion multiplication and .conjugate() is the quaternion
+conjugate, which is equivalent to the inverse for a unit quaternion. We can
+relate the changes in orientation of the camera and IMU by
+
+dq_cam * q_cam2imu = q_cam2imu * dq_imu
+
+This is the quaternion version of the hand-eye calibration problem (better
+known  in the matrix form: AX = XB).
+
+We will solve for q_cam2imu by defining the error quaternion:
+
+q_err = (dq_cam * q_cam2imu) * (q_cam2imu * dq_imu).conjugate()
+
+In the ideal case, q_err will converge to the identity quaternion (1, 0, 0, 0)
+at the solution. Quaternions are defined by 4 parameters with one constraint.
+We will map the quaternion to a 3-parameter rotation vector, which can be
+solved more efficiently and simply. The rotation vector is the product of the
+unit vector around the axis of rotation (u) and the rotation (theta):
+
+e = theta * u = log(q_err)
+
+The optimization algorith will minimize the two-norm of the error rotation
+vector for k = 1..N measurements:
+
+sum(||e[k]||^2)
+
+
+Assumptions & limitations
+-------------------------
+
+1. Small rotation angles for dq_cam and dq_imu could cause numerical problems
+   so successive samples should be selected so that the angles are sufficiently
+   large.
+2. The IMU will drift over time so the time between the samples used to
+   calculate dq_imu should be short enough for drift to be negligible.
+3. The camera and IMU samples should be taken simultaneously. If the camera
+   moves during exposure, this will introduce an error. Error could be reduced
+   by used samples when the camera movement is reasonably stationary.
+4. In practice, the plate solver will have worse error in roll than RA and Dec.
+   This is not accounted for.
+5. Ideally, the camera/IMU should be rotated around all three axes but on a
+   mount, the rotation will likely be around two axes. This may result in a 
+   larger uncertainty for the rotation/alignment about some axes.
 """
 import logging
 import numpy as np
@@ -13,6 +81,8 @@
 from PiFinder.types.coordinates import RaDecRoll
 from PiFinder.pointing_model import quaternion_transforms as qt
 
+list_of_quats = list[quaternion.quaternion]
+
 logger = logging.getLogger("IMU.Align")
 
 
diff --git a/python/PiFinder/imu/imu_alignment_solver.py b/python/PiFinder/imu/imu_alignment_solver.py
index 7f8708664..0163ef86f 100644
--- a/python/PiFinder/imu/imu_alignment_solver.py
+++ b/python/PiFinder/imu/imu_alignment_solver.py
@@ -1,77 +1,5 @@
 """
-Alignment of the IMU-camera axes (extrinsic calibration)
-
-For dead-reckoning with the IMU, we need the rotation between the IMU and
-camera axes. This is done by the quaternion q_cam2imu and its inverse
-q_imu2cam.
-
-The goal of this module is to estimate q_cam2imu. We can do this using pairs of
-camera and IMU orientation quaternions measured simultaneously.
-
-Required measurements
----------------------
-
-The measurements we have are:
-
-* q_eq2cam: Quaternion rotation of the camera center relative to the equatorial
-  frame. 
-* q_x2imu: The rotation of the IMU relative to some arbibtrary reference frame
-  X.
-
-The camera and IMU measurements are paired and assumed to be simultaneous.
-  
-Algorithm:
-----------
-
-We can express the rotation between successive timesteps for the camera and
-IMU:
-
-dq_cam = q_eq2cam[k-1].conjugate() * q_eq2cam[k] dq_imu =
-q_x2imu[k-1].conjugate() * q_x2imu[k]
-
-where * is the quaternion multiplication and .conjugate() is the quaternion
-conjugate, which is equivalent to the inverse for a unit quaternion. We can
-relate the changes in orientation of the camera and IMU by
-
-dq_cam * q_cam2imu = q_cam2imu * dq_imu
-
-This is the quaternion version of the hand-eye calibration problem (better
-known  in the matrix form: AX = XB).
-
-We will solve for q_cam2imu by defining the error quaternion:
-
-q_err = (dq_cam * q_cam2imu) * (q_cam2imu * dq_imu).conjugate()
-
-In the ideal case, q_err will converge to the identity quaternion (1, 0, 0, 0)
-at the solution. Quaternions are defined by 4 parameters with one constraint.
-We will map the quaternion to a 3-parameter rotation vector, which can be
-solved more efficiently and simply. The rotation vector is the product of the
-unit vector around the axis of rotation (u) and the rotation (theta):
-
-e = theta * u = log(q_err)
-
-The optimization algorith will minimize the two-norm of the error rotation
-vector for k = 1..N measurements:
-
-sum(||e[k]||^2)
-
-
-Assumptions & limitations
--------------------------
-
-1. Small rotation angles for dq_cam and dq_imu could cause numerical problems
-   so successive samples should be selected so that the angles are sufficiently
-   large.
-2. The IMU will drift over time so the time between the samples used to
-   calculate dq_imu should be short enough for drift to be negligible.
-3. The camera and IMU samples should be taken simultaneously. If the camera
-   moves during exposure, this will introduce an error. Error could be reduced
-   by used samples when the camera movement is reasonably stationary.
-4. In practice, the plate solver will have worse error in roll than RA and Dec.
-   This is not accounted for.
-5. Ideally, the camera/IMU should be rotated around all three axes but on a
-   mount, the rotation will likely be around two axes. This may result in a 
-   larger uncertainty for the rotation/alignment about some axes.
+Solver for IMU alignment
 """
 import logging
 import numpy as np
@@ -92,6 +20,7 @@ def ensure_quat_list_continuity(q_list: list_of_quats) -> list_of_quats:
     Ensures that consecutive quaternions in the list have consistent signs (due
     to the double coverage property of quaternions where q and -q represent
     same rotation).
+    TODO: Possibly not needed. If so, remove.
     """
     q_list_out = [q_list[0]]
     for q in q_list[1:]:
@@ -101,17 +30,12 @@ def ensure_quat_list_continuity(q_list: list_of_quats) -> list_of_quats:
     return q_list_out
 
 
-def build_relative_rotations(q_list: list_of_quats, step=1) -> list_of_quats:
+def calculate_relative_rotations(q1_list: list_of_quats, q2_list: list_of_quats) -> list_of_quats:
     """
-    Calculate the relative rotation between successive quaternions:
-    dq[k] = q[k].conjugate() * q[k+step]
+    Calculate the relative rotation between q1_list and the corresponding q2_list:
+    dq[k] = q1[k].conjugate() * q2[k]
     """
-    dq = []
-    for k in range(len(q_list) - step):
-        q_rel = q_list[k].conjugate() * q_list[k + step]
-        dq.append(q_rel)
-
-    return dq
+    return [q1.conjugate() * q2 for q1, q2 in zip(q1_list, q2_list)]
 
 
 def reject_small_rotations(dq_cam: list_of_quats, 

From 386f0718671b3b2bc90f86db94013aada05b081e Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Mon, 3 Aug 2026 21:11:14 +0200
Subject: [PATCH 11/45] Generalise solve funcs

---
 python/PiFinder/imu/imu_alignment_solver.py | 107 +++++---------------
 1 file changed, 28 insertions(+), 79 deletions(-)

diff --git a/python/PiFinder/imu/imu_alignment_solver.py b/python/PiFinder/imu/imu_alignment_solver.py
index 0163ef86f..7a6be1571 100644
--- a/python/PiFinder/imu/imu_alignment_solver.py
+++ b/python/PiFinder/imu/imu_alignment_solver.py
@@ -38,108 +38,59 @@ def calculate_relative_rotations(q1_list: list_of_quats, q2_list: list_of_quats)
     return [q1.conjugate() * q2 for q1, q2 in zip(q1_list, q2_list)]
 
 
-def reject_small_rotations(dq_cam: list_of_quats, 
-                           dq_imu: list_of_quats,
+def reject_small_rotations(dq_list: list_of_quats,
                            min_rotation=np.deg2rad(1.0),  # Reject rotations below this [radians]
-                           ) -> tuple[list_of_quats, list_of_quats]:
+                           ):
     """
     Reject small rotations
     """
-    keep_dq_cam = []
-    keep_dq_imu = []
-    for qc, qi in zip(dq_cam, dq_imu):
-        angle_cam = np.linalg.norm(quaternion.as_rotation_vector(qc))
-        angle_imu = np.linalg.norm(quaternion.as_rotation_vector(qi))
+    pass
 
-        if angle_cam >= min_rotation or angle_imu >= min_rotation:
-            keep_dq_cam.append(qc)
-            keep_dq_imu.append(qi)
-
-    return keep_dq_cam, keep_dq_imu
 
+N_UNKNOWN_PARAMS = 3  # Number of unknown parameters in the problem to solve
 
 def residual_rotation_vector(x,  # (3,) Trial solution (q as rotation vector) 
-                             dq_cam: list_of_quats,  # List of relative camera rotation quaternions
-                             dq_imu: list_of_quats  # List of relative IMU rotation quaternions
+                             q1_list: list_of_quats,  # List of rotation quaternions
+                             q2_list: list_of_quats 
                              ) -> np.ndarray:
     """
+    For solving q_cam2imu in the quaternion form of the hand-eye problem: 
+    q1 * q_12 = q_12 * q2
+
     Calculate the esiduals at the trial solution x for least squares
     optimization.
-
-    For solving q_cam2imu in the quaternion form of the hand-eye problem: 
-    dq_cam * q_cam2imu = q_cam2imu * dq_imu
     """
     # Convert trial solution (rotation vector) to quaternion
-    q_cam2imu = quaternion.from_rotation_vector(x)
+    q_12 = quaternion.from_rotation_vector(x)
 
-    n_meas = len(dq_cam)
+    n_meas = len(q1_list)
     residuals = np.zeros(3 * n_meas)
-    for ii, (qc, qi) in enumerate(zip(dq_cam, dq_imu)):
-        q_left = qc * q_cam2imu
-        q_right = q_cam2imu * qi
-
-        # Error quaternion
-        q_err = q_left * q_right.conjugate()
-
+    for ii, (q1, q2) in enumerate(zip(q1_list, q2_list)):
+        q_err = (q1 * q_12) * (q_12 * q2).conjugate()  # Error quaternion
         # Convert to rotation vector (Lie algebra logarithm map)
         residuals[(3 * ii):(3 * ii + 3)] = quaternion.as_rotation_vector(q_err)
 
     return np.array(residuals)
 
 
-N_UNKNOWN_PARAMS = 3  # Number of unknown parameters in the problem to solve
-
-def calibrate_camera_imu(
-        q_cam: list_of_quats,  # Camera orientations
-        q_imu: list_of_quats,  # IMU orientations at same moments
-        step: int = 1,  # Skip successive measurements
-        min_rotation=np.deg2rad(1.0),  # Reject rotations below this [radians]
+def solve_rotation(
+        q1_list: list_of_quats,  # List of rotation quaternions
+        q2_list: list_of_quats,
         x0: Union[np.ndarray, list] = np.zeros(N_UNKNOWN_PARAMS),  # Initial guess
         residual_threshold = 0.01,  # Reject samples with residual > resid_threshold in first pass
         verbose=True
         ):
     """
-    Estimate q_cam2imu from pairs of simultaneous camera and IMU measurements
-    q_cam and q_imu (as quaternions).
+    Solve the quaternion form of the hand-eye problem
+    dq1 * q_12 = q_12 * dq2
 
-    RETURNS:
-    q_cam2imu: [quaternion.quaternion] Camera-to-IMU rotation estimate
-    sigma_total: [rad] Total rotaion uncertainty
-    condition_number: < 10 excellent, < 100 acceptable, <1E4 weak observability
-
-    TODO: 
-    - Add checks to fail gracefully if there aren't enough data points.
-    - Remove outliers
+    Where q_12 is the unknown rotation that rotates q1 to q2
     """
-    t_start = time.time()
-
-    # Enforce quaternion continuity
-    q_cam = ensure_quat_list_continuity(q_cam)
-    q_imu = ensure_quat_list_continuity(q_imu)
-
-    # Calculate relative rotations between successive quaternions
-    dq_cam = build_relative_rotations(q_cam, step)
-    dq_imu = build_relative_rotations(q_imu, step)
-
-    # Reject rotation angle < min_rotation
-    reject_small_rotations(dq_cam, dq_imu, min_rotation=min_rotation)
-    # TODO: Convert print() to logging
-    print(f"{len(q_cam)} measurements. Using {len(dq_cam)} pairs for camera-IMU calibration.")
-    # Calculate angular differences for logging
-    d_thetas = []
-    for ii, dq in enumerate(dq_cam):
-        if ii > 0:
-            d_thetas.append(qt.get_quat_angular_diff(prev_dq, dq))
-        prev_dq = dq
-    print(f"Angular rotations: {np.rad2deg(np.min(np.abs(d_thetas))):.2f} to " 
-          f"{np.rad2deg(np.max(np.abs(d_thetas))):.2f} deg. "
-          f"Median: {np.rad2deg(np.median(np.abs(d_thetas))):.2f} deg.")
-
     # Solve for x by non-linear least squares (Levenberg-Marquardt)
     # TODO: Tune LM params
     # TODO: Calculate the Jacobians analytically? Current numerical Jacobians is probably fast enough?
     result = least_squares(residual_rotation_vector, x0, method='lm', 
-                           args=(dq_cam, dq_imu))
+                           args=(q1_list, q2_list))
     # TODO: Investigate using robust loss functions?
     #result = least_squares(residual_rotation_vector, x0, loss='cauchy', 
     #                       args=(dq_cam, dq_imu))
@@ -149,28 +100,26 @@ def calibrate_camera_imu(
         # NOTE: Each quaternion measurement is converted to rotation vectors with 3 values
         resid_reshaped = result.fun.reshape(-1, 3)  # Each row is a sample
         msk_accept = np.all(np.abs(resid_reshaped) < residual_threshold, axis=1)
-        dq_cam_accept = np.array(dq_cam)[msk_accept]
-        dq_imu_accept = np.array(dq_imu)[msk_accept]
+        q1_accept = np.array(q1_list)[msk_accept]
+        q2_accept = np.array(q2_list)[msk_accept]
         if verbose:
             print(f"Accepted {np.sum(msk_accept)}/{resid_reshaped.shape[0]} samples.")
         # Run least-squares again (using previous solution as the initial guess)
-        result = least_squares(residual_rotation_vector, result.x, 
-                               args=(dq_cam_accept, dq_imu_accept))
+        result = least_squares(residual_rotation_vector, result.x, args=(q1_accept, q2_accept))
 
-    # Convert estimate from rotatino vector to quaternion
-    q_cam2imu = quaternion.from_rotation_vector(result.x)
-    t_compute = time.time() - t_start
+    # Convert estimate from rotation vector to quaternion
+    q_12 = quaternion.from_rotation_vector(result.x)
     
     if verbose:
-        print(f"Estimated q_cam2imu: q_cam2imu={q_cam2imu}, compute time = {t_compute:.3f}s ",
+        print(f"Estimated q_cam2imu: q_cam2imu={q_12}, ",
             f"Func evaluations: {result.nfev}, Cost = {result.cost:.4g}, ", 
             f"Success: {result.success}, {result.message}")
 
-    # Diagnostics
+    # Diagnostics  TODO: Return these
     sigma_total, condition_number = _solution_diagnostics(result)
     residuals = result.fun
 
-    return q_cam2imu, sigma_total, residuals, condition_number
+    return q_12
 
 
 def _solution_diagnostics(result):

From 98a2220979be7e10b9bbfb9afbd585ce5ea4d0af Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Thu, 6 Aug 2026 22:45:57 +0200
Subject: [PATCH 12/45] Routine to process pairings in loop and solve

---
 python/PiFinder/imu/imu_alignment.py        | 82 ++++++++++++++++++---
 python/PiFinder/imu/imu_alignment_solver.py |  8 +-
 2 files changed, 80 insertions(+), 10 deletions(-)

diff --git a/python/PiFinder/imu/imu_alignment.py b/python/PiFinder/imu/imu_alignment.py
index 7989fe845..6fe749906 100644
--- a/python/PiFinder/imu/imu_alignment.py
+++ b/python/PiFinder/imu/imu_alignment.py
@@ -134,23 +134,32 @@ def trim_to_max_length(self):
 
 class ImuCameraAlignment:
     """
+    Note that max_time_diff should be kept to a few seconds at most to avoid
+    gyro drift over the time between samples.
     """
     candidate_buffer: SampleBuffer  # Buffer of camera/IMU samples
     diff_buffer: SampleBuffer  # Buffer of paired differences in camera/IMU samples
 
+    min_n_solve: int  # Minimum number of samples for solve
     max_time_diff: float  # [s] Maximum time difference between pairs of samples
     min_angle_diff: float  # [rad] Pair samples with large enough angle difference
     max_age: float  # [s] Maximum age of sample compared to current time
 
-    def __init__(self, candidate_buffer_length=10, diff_buffer_length=10,
-                 max_time_diff=10, min_angle_diff=np.deg2rad(5), max_age=1200):
+    def __init__(self, candidate_buffer_length=60, min_n_solve=10, 
+                 max_time_diff=2.0, min_angle_diff=np.deg2rad(5), max_age=1200):
+        """
+        candidate_buffer_length: Should be around sample_freq * max_time_diff
+        """
         self.candidate_buffer = SampleBuffer(max_buffer_length=candidate_buffer_length)
-        self.diff_buffer = SampleBuffer(max_buffer_length=diff_buffer_length)
+        self.diff_buffer = SampleBuffer(max_buffer_length=min_n_solve)
 
+        self.min_n_solve = min_n_solve
         self.max_time_diff = max_time_diff
         self.min_angle_diff = min_angle_diff
         self.max_age = max_age
 
+        self._samples_since_last_pair_attempt = 0
+
     def reset_buffers(self):
         self.candidate_buffer.reset_buffer()
         self.diff_buffer.reset_buffer()
@@ -189,13 +198,36 @@ def purge_old_samples(self, current_time: float):
         if remove_idx_list:
             self.diff_buffer.remove_samples(remove_idx_list)
 
-    def pair_samples(self):
+    def purge_old_candidates(self):
+        """
+        Remove samples from candidate_buffer that are older than
+        self.max_time_diff from other samples in buffer because these will be
+        never paired.
+        """
+        if self.candidate_buffer.len <= 1:
+            return
+
+        remove_idx_list = []
+        timestamps = np.array([samp.timestamp for samp in self.candidate_buffer])
+        for isamp in range(timestamps.shape[0]):
+            dt = np.abs(timestamps - timestamps[isamp])
+            if np.sum(dt < self.max_time_diff) <= 1:
+                remove_idx_list.append(isamp)
+
+        if remove_idx_list:
+            self.candidate_buffer.remove_samples(remove_idx_list)
+
+    def pair_samples(self) -> int:
         """
         Go through the candidate_buffer from the first sample in the buffer.
         Pair two sets of camera/IMU samples from the candidate buffer that meet
         the criteria and remove them from the buffer. Repeat all pairable
         samples have been removed from the candidate_buffer.
         """
+        n_pairs = 0
+        if self.candidate_buffer.len == 0:
+            return n_pairs
+
         remove_idx_list = []
         for isamp1, samp1 in enumerate(self.candidate_buffer[:-1]):
             if isamp1 in remove_idx_list:
@@ -224,17 +256,49 @@ def pair_samples(self):
                 self.diff_buffer.add_sample((samp1, samp2))
                 remove_idx_list.append(isamp1)
                 remove_idx_list.append(isamp2)
+                n_pairs += 1
 
         if remove_idx_list:
             self.candidate_buffer.remove_samples(remove_idx_list)
-        self.trim_buffers()  # Clean up
+        return n_pairs  # Number of successful pairings
 
     def solve(self, n_pairs=None):
         """
-        Solve for the alignment between the camera and IMU using the last
-        n_pairs or all available pairs (if None).
+        Solve for the alignment between the camera and IMU using at least the
+        last n_pairs or all available pairs (if None).
         """
         if n_pairs is None:
-            n_pairs = self.diff_buffer.len
-        
+            n_pairs = self.diff_buffer.len  # Use all available data
+        #TODO
+        return None
 
+    def add_sample_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
+        """
+        Add a new sample to the buffer. When the buffer fills up, pair samples 
+        and solve.
+        """
+        self.add_sample(timestamp, cam_eq, q_x2imu)
+
+        # Pair samples and solve
+        if ((self._samples_since_last_pair_attempt >= self.min_n_solve) or
+            (self.candidate_buffer.len >= self.candidate_buffer.max_buffer_length)):
+            self.purge_old_samples(timestamp)
+            self.purge_old_candidates()
+            self.pair_samples()
+            self.trim_buffers()
+
+            # If the candidate buffer is still full after pairing, remove a 
+            # batch of the older samples from the buffer
+            if self.candidate_buffer.len >= self.candidate_buffer.max_buffer_length:
+                remove_list = list(range(self.min_n_solve))
+                self.candidate_buffer.remove_samples(remove_list)
+            
+            self._samples_since_last_pair_attempt = 0
+        else:
+            self._samples_since_last_pair_attempt += 1
+
+        # Solve if there are enough samples
+        if self.diff_buffer.len >= self.min_n_solve:
+            solution = self.solve()
+            self.diff_buffer.reset_buffer()  # Flush the values used for solve
+    
\ No newline at end of file
diff --git a/python/PiFinder/imu/imu_alignment_solver.py b/python/PiFinder/imu/imu_alignment_solver.py
index 7a6be1571..37fef0a64 100644
--- a/python/PiFinder/imu/imu_alignment_solver.py
+++ b/python/PiFinder/imu/imu_alignment_solver.py
@@ -1,5 +1,11 @@
 """
-Solver for IMU alignment
+Core solver functionalities for solving the quaternion form of the hand-eye
+problem: 
+
+q1 * q_12 = q_12 * q2
+
+Where the goal is to solve for the rotation q_12. Given enough measurements of 
+q1 and q2, we can solve for q_12.
 """
 import logging
 import numpy as np

From f69d68d8eededcbd44d0c0b9d184001873aca231 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Fri, 7 Aug 2026 22:12:10 +0200
Subject: [PATCH 13/45] Move to imu_align/

---
 python/PiFinder/imu/{ => imu_align}/README.md               | 0
 python/PiFinder/imu/imu_align/__init__.py                   | 0
 python/PiFinder/imu/{ => imu_align}/imu_alignment.py        | 0
 python/PiFinder/imu/{ => imu_align}/imu_alignment_solver.py | 0
 4 files changed, 0 insertions(+), 0 deletions(-)
 rename python/PiFinder/imu/{ => imu_align}/README.md (100%)
 create mode 100644 python/PiFinder/imu/imu_align/__init__.py
 rename python/PiFinder/imu/{ => imu_align}/imu_alignment.py (100%)
 rename python/PiFinder/imu/{ => imu_align}/imu_alignment_solver.py (100%)

diff --git a/python/PiFinder/imu/README.md b/python/PiFinder/imu/imu_align/README.md
similarity index 100%
rename from python/PiFinder/imu/README.md
rename to python/PiFinder/imu/imu_align/README.md
diff --git a/python/PiFinder/imu/imu_align/__init__.py b/python/PiFinder/imu/imu_align/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/python/PiFinder/imu/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
similarity index 100%
rename from python/PiFinder/imu/imu_alignment.py
rename to python/PiFinder/imu/imu_align/imu_alignment.py
diff --git a/python/PiFinder/imu/imu_alignment_solver.py b/python/PiFinder/imu/imu_align/imu_alignment_solver.py
similarity index 100%
rename from python/PiFinder/imu/imu_alignment_solver.py
rename to python/PiFinder/imu/imu_align/imu_alignment_solver.py

From a44acb7ce6c3bd1258811dbe7fa1ed088510da31 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Fri, 7 Aug 2026 22:28:14 +0200
Subject: [PATCH 14/45] Generalise

---
 .../imu/imu_align/imu_alignment_solver.py     | 59 ++++++++++---------
 1 file changed, 31 insertions(+), 28 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/imu_alignment_solver.py b/python/PiFinder/imu/imu_align/imu_alignment_solver.py
index 37fef0a64..0a4652665 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment_solver.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment_solver.py
@@ -199,13 +199,14 @@ def _random_quaternions(N: int, max_rot=None) -> list_of_quats:
     return qs
 
 
-def simulate_measurements(q_cam2imu: quaternion.quaternion,  # True q_cam2imu (camera-to-IMU alignment)
-                          N: int = 100,  # Number of samples to simulate
-                          max_rot = None,  # Max rotation from previous orientation
-                          camera_noise_amp: float = np.deg2rad(0.1),  # Camera noise amp in radians
-                          imu_noise_amp: float = np.deg2rad(0.1),  # IMU noise amp in radians
-                          seed=0  # Random seed. None to disable
-                          ):
+def simulate_quaternion_measurements(
+        q_12: quaternion.quaternion,  # True rel. orientations (q1 ro q2 alignment)
+        N: int = 100,  # Number of samples to simulate
+        max_rot = None,  # Max rotation from previous orientation
+        q1_noise_amp: float = np.deg2rad(0.1),  # Noise amp in radians
+        q2_noise_amp: float = np.deg2rad(0.1),  # Noise amp in radians
+        seed=0  # Random seed. None to disable
+        ):
     """
     Simulate camera and IMU measurements
     """
@@ -213,49 +214,51 @@ def simulate_measurements(q_cam2imu: quaternion.quaternion,  # True q_cam2imu (c
         np.random.seed(seed)
         
     # Generate random IMU orientations
-    q_imu_true = _random_quaternions(N, max_rot=max_rot)
+    q2_true = _random_quaternions(N, max_rot=max_rot)
 
     # Generate corresponding camera orientations
-    q_imu2cam = q_cam2imu.conjugate()
-    q_cam_true = []
-    for q in q_imu_true:
-        q_cam_true.append(q * q_imu2cam)
+    q_21 = q_12.conjugate()
+    q1_true = []
+    for q in q2_true:
+        q1_true.append(q * q_21)
 
     # Add noise
-    q_cam = _add_noise_to_quaternion_list(q_cam_true, camera_noise_amp)
-    q_imu = _add_noise_to_quaternion_list(q_imu_true, imu_noise_amp)
+    q1 = _add_noise_to_quaternion_list(q1_true, q1_noise_amp)
+    q2 = _add_noise_to_quaternion_list(q2_true, q2_noise_amp)
 
-    return q_cam, q_imu
+    return q1, q2
 
 
 if __name__ == "__main__":
     """ 
-    The main block simulates pairs of random IMU/camera measurements and solves
-    for the camera-to-IMU alignment (q_cam2imu).
+    The main block simulates pairs of q1 and q2 measurements and solves
+    for the camera-to-IMU alignment (q_12).
     """
 
     # Set the true camera-from-body rotation
     true_rotvec = np.radians([10, -5, 20])
-    q_cam2imu_true = quaternion.from_rotation_vector(true_rotvec)
+    q_12_true = quaternion.from_rotation_vector(true_rotvec)
 
     # Simulate measurements:
-    q_cam, q_imu = simulate_measurements(
-        q_cam2imu_true, N=100, camera_noise_amp=np.deg2rad(0.1), 
+    q1, q2 = simulate_quaternion_measurements(
+        q_12_true, N=100, camera_noise_amp=np.deg2rad(0.1), 
         imu_noise_amp=np.deg2rad(0.1), seed=0)
 
-    # Calibrate
-    q_est, sigma_total, condition_number = calibrate_camera_imu(
-        q_cam, q_imu, step=2, min_rotation=np.deg2rad(1.0))
+    # Optional steps: Reject small rotations
+
+    # solve
+    q_12_est, sigma_total, condition_number = solve_rotation(
+        q1, q2, residual_threshold = 0.01, verbose=True)
 
     # Results
-    print("\nTrue q_cam2imu:")
-    print(quaternion.as_float_array(q_cam2imu_true))
+    print("\nTrue q_12:")
+    print(quaternion.as_float_array(q_12_true))
 
-    print("\nEstimated q_cam2imu:")
-    print(quaternion.as_float_array(q_est))
+    print("\nEstimated q_12_est:")
+    print(quaternion.as_float_array(q_12_est))
 
     # Error
-    q_error = q_est.conjugate() * q_cam2imu_true
+    q_error = q_12_est.conjugate() * q_12_true
     error_deg = np.rad2deg(
         np.linalg.norm(
             quaternion.as_rotation_vector(q_error)

From 88d2d94e202e441cd912573f891480288772f971 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sat, 8 Aug 2026 00:16:15 +0200
Subject: [PATCH 15/45] Move funcs around

---
 ...alignment_solver.py => hand_eye_solver.py} | 77 ++++++++++---------
 1 file changed, 42 insertions(+), 35 deletions(-)
 rename python/PiFinder/imu/imu_align/{imu_alignment_solver.py => hand_eye_solver.py} (97%)

diff --git a/python/PiFinder/imu/imu_align/imu_alignment_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
similarity index 97%
rename from python/PiFinder/imu/imu_align/imu_alignment_solver.py
rename to python/PiFinder/imu/imu_align/hand_eye_solver.py
index 0a4652665..ddb9517f1 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -20,39 +20,6 @@
 
 logger = logging.getLogger("IMU.Align")
 
-
-def ensure_quat_list_continuity(q_list: list_of_quats) -> list_of_quats:
-    """
-    Ensures that consecutive quaternions in the list have consistent signs (due
-    to the double coverage property of quaternions where q and -q represent
-    same rotation).
-    TODO: Possibly not needed. If so, remove.
-    """
-    q_list_out = [q_list[0]]
-    for q in q_list[1:]:
-        q = qt.ensure_quat_continuity(q_list_out[-1], q)
-        q_list_out.append(q)
-
-    return q_list_out
-
-
-def calculate_relative_rotations(q1_list: list_of_quats, q2_list: list_of_quats) -> list_of_quats:
-    """
-    Calculate the relative rotation between q1_list and the corresponding q2_list:
-    dq[k] = q1[k].conjugate() * q2[k]
-    """
-    return [q1.conjugate() * q2 for q1, q2 in zip(q1_list, q2_list)]
-
-
-def reject_small_rotations(dq_list: list_of_quats,
-                           min_rotation=np.deg2rad(1.0),  # Reject rotations below this [radians]
-                           ):
-    """
-    Reject small rotations
-    """
-    pass
-
-
 N_UNKNOWN_PARAMS = 3  # Number of unknown parameters in the problem to solve
 
 def residual_rotation_vector(x,  # (3,) Trial solution (q as rotation vector) 
@@ -159,6 +126,42 @@ def _solution_diagnostics(result):
 
     return sigma_total, condition_number
 
+
+# ------- Helper functions -------
+
+
+def ensure_quat_list_continuity(q_list: list_of_quats) -> list_of_quats:
+    """
+    Ensures that consecutive quaternions in the list have consistent signs (due
+    to the double coverage property of quaternions where q and -q represent
+    same rotation).
+    TODO: Possibly not needed. If so, remove.
+    """
+    q_list_out = [q_list[0]]
+    for q in q_list[1:]:
+        q = qt.ensure_quat_continuity(q_list_out[-1], q)
+        q_list_out.append(q)
+
+    return q_list_out
+
+
+def calculate_relative_rotations(q1_list: list_of_quats, q2_list: list_of_quats) -> list_of_quats:
+    """
+    Calculate the relative rotation between q1_list and the corresponding q2_list:
+    dq[k] = q1[k].conjugate() * q2[k]
+    """
+    return [q1.conjugate() * q2 for q1, q2 in zip(q1_list, q2_list)]
+
+
+def reject_small_rotations(dq_list: list_of_quats,
+                           min_rotation=np.deg2rad(1.0),  # Reject rotations below this [radians]
+                           ):
+    """
+    Reject small rotations
+    """
+    pass
+
+
 # ------ Simulation functions for testing & analysis --------------------------
 
 def _q_noise(noise_amp: float):
@@ -232,7 +235,9 @@ def simulate_quaternion_measurements(
 if __name__ == "__main__":
     """ 
     The main block simulates pairs of q1 and q2 measurements and solves
-    for the camera-to-IMU alignment (q_12).
+    for the q_12 for the quaternion form of the hand-eye problem:
+    
+    q1 * q_12 = q_12 * q2
     """
 
     # Set the true camera-from-body rotation
@@ -244,7 +249,9 @@ def simulate_quaternion_measurements(
         q_12_true, N=100, camera_noise_amp=np.deg2rad(0.1), 
         imu_noise_amp=np.deg2rad(0.1), seed=0)
 
-    # Optional steps: Reject small rotations
+    # Optional steps: 
+    # Pair up and calculate relative rotations
+    # Reject small rotations
 
     # solve
     q_12_est, sigma_total, condition_number = solve_rotation(

From 03af9631d5aeb4ff6f12721a35fe908e1b06058d Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sat, 8 Aug 2026 00:16:37 +0200
Subject: [PATCH 16/45] Fix

---
 .../PiFinder/imu/imu_align/imu_alignment.py   | 22 ++++++++++---------
 1 file changed, 12 insertions(+), 10 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index 6fe749906..ccf3501c1 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -115,8 +115,8 @@ def len(self):
         return len(self.buffer)
 
     def add_sample(self, sample: CameraImuSample):
-        if len(self.samples) >= self.max_buffer_length:
-            self.samples.pop(0)  # Remove oldest sample from buffer
+        if len(self.buffer) >= self.max_buffer_length:
+            self.buffer.pop(0)  # Remove oldest sample from buffer
         self.buffer.append(sample)
 
     def pop_sample(self, idx: int):
@@ -186,13 +186,13 @@ def purge_old_samples(self, current_time: float):
         allowed_timestamp = current_time - self.max_age  # Purge anything older than this
         
         # Purge candidate_buffer:
-        remove_idx_list = [i for i, samp in enumerate(self.candidate_buffer) 
+        remove_idx_list = [i for i, samp in enumerate(self.candidate_buffer.buffer) 
                            if samp.timestamp < allowed_timestamp]
         if remove_idx_list:
             self.candidate_buffer.remove_samples(remove_idx_list)
 
         # Purge diff_buffer:
-        remove_idx_list = [i for i, (samp1, samp2) in enumerate(self.diff_buffer) 
+        remove_idx_list = [i for i, (samp1, samp2) in enumerate(self.diff_buffer.buffer) 
                            if samp1.timestamp < allowed_timestamp 
                            or samp2.timestamp < allowed_timestamp]
         if remove_idx_list:
@@ -208,7 +208,7 @@ def purge_old_candidates(self):
             return
 
         remove_idx_list = []
-        timestamps = np.array([samp.timestamp for samp in self.candidate_buffer])
+        timestamps = np.array([samp.timestamp for samp in self.candidate_buffer.buffer])
         for isamp in range(timestamps.shape[0]):
             dt = np.abs(timestamps - timestamps[isamp])
             if np.sum(dt < self.max_time_diff) <= 1:
@@ -229,14 +229,14 @@ def pair_samples(self) -> int:
             return n_pairs
 
         remove_idx_list = []
-        for isamp1, samp1 in enumerate(self.candidate_buffer[:-1]):
+        for isamp1, samp1 in enumerate(self.candidate_buffer.buffer[:-1]):
             if isamp1 in remove_idx_list:
                 continue
 
             for isamp2 in range(isamp1 + 1, self.candidate_buffer.len):
                 if isamp2 in remove_idx_list:
                     continue
-                samp2 = self.candidate_buffer[isamp2]
+                samp2 = self.candidate_buffer.buffer[isamp2]
 
                 # Check time difference between samples:
                 dt = samp2.timestamp - samp1.timestamp
@@ -274,8 +274,8 @@ def solve(self, n_pairs=None):
 
     def add_sample_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
         """
-        Add a new sample to the buffer. When the buffer fills up, pair samples 
-        and solve.
+        For general use, call this method. Add a new sample to the buffer. When
+        the buffer fills up, pair samples and solve.
         """
         self.add_sample(timestamp, cam_eq, q_x2imu)
 
@@ -301,4 +301,6 @@ def add_sample_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu:
         if self.diff_buffer.len >= self.min_n_solve:
             solution = self.solve()
             self.diff_buffer.reset_buffer()  # Flush the values used for solve
-    
\ No newline at end of file
+            return solution
+        else:
+            return None

From 3b23d654ebdc01d6bdb1441613718ce66df38f23 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sat, 8 Aug 2026 01:10:33 +0200
Subject: [PATCH 17/45] Use sets for remove_lidx for efficiency & simplicity.
 Refactor.

---
 .../PiFinder/imu/imu_align/imu_alignment.py   | 60 +++++++++----------
 1 file changed, 30 insertions(+), 30 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index ccf3501c1..5c89d31c3 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -123,7 +123,7 @@ def pop_sample(self, idx: int):
         """Remove and return the sample at the given index"""
         return self.buffer.pop(idx)
     
-    def remove_samples(self, idx_list: list[int]):
+    def remove_samples(self, idx_list: set[int]):
         """Remove multiple samples by indices"""
         self.buffer = [self.buffer[i] for i in range(len(self.buffer)) if i not in idx_list]
 
@@ -182,6 +182,8 @@ def purge_old_samples(self, current_time: float):
         """
         Remove samples from the candidate_buffer that are older than the current
         time.
+
+        This should be run on a schedule every self.max_age [s].
         """
         allowed_timestamp = current_time - self.max_age  # Purge anything older than this
         
@@ -189,33 +191,35 @@ def purge_old_samples(self, current_time: float):
         remove_idx_list = [i for i, samp in enumerate(self.candidate_buffer.buffer) 
                            if samp.timestamp < allowed_timestamp]
         if remove_idx_list:
-            self.candidate_buffer.remove_samples(remove_idx_list)
+            self.candidate_buffer.remove_samples(set(remove_idx_list))
 
         # Purge diff_buffer:
         remove_idx_list = [i for i, (samp1, samp2) in enumerate(self.diff_buffer.buffer) 
                            if samp1.timestamp < allowed_timestamp 
                            or samp2.timestamp < allowed_timestamp]
         if remove_idx_list:
-            self.diff_buffer.remove_samples(remove_idx_list)
+            self.diff_buffer.remove_samples(set(remove_idx_list))
 
     def purge_old_candidates(self):
         """
         Remove samples from candidate_buffer that are older than
         self.max_time_diff from other samples in buffer because these will be
         never paired.
+        
+        This should be run on a schedule every self.max_time_diff [s].
         """
         if self.candidate_buffer.len <= 1:
             return
 
-        remove_idx_list = []
+        remove_ids = set()
         timestamps = np.array([samp.timestamp for samp in self.candidate_buffer.buffer])
         for isamp in range(timestamps.shape[0]):
             dt = np.abs(timestamps - timestamps[isamp])
             if np.sum(dt < self.max_time_diff) <= 1:
-                remove_idx_list.append(isamp)
+                remove_ids.add(isamp)
 
-        if remove_idx_list:
-            self.candidate_buffer.remove_samples(remove_idx_list)
+        if remove_ids:
+            self.candidate_buffer.remove_samples(remove_ids)
 
     def pair_samples(self) -> int:
         """
@@ -228,38 +232,34 @@ def pair_samples(self) -> int:
         if self.candidate_buffer.len == 0:
             return n_pairs
 
-        remove_idx_list = []
+        remove_ids = set()
         for isamp1, samp1 in enumerate(self.candidate_buffer.buffer[:-1]):
-            if isamp1 in remove_idx_list:
-                continue
-
-            for isamp2 in range(isamp1 + 1, self.candidate_buffer.len):
-                if isamp2 in remove_idx_list:
-                    continue
-                samp2 = self.candidate_buffer.buffer[isamp2]
-
+            for isamp2, samp2 in enumerate(self.candidate_buffer.buffer[isamp1+1:]):
                 # Check time difference between samples:
                 dt = samp2.timestamp - samp1.timestamp
                 if dt > self.max_time_diff:
-                    continue  # Samples too far apart in time
+                    # Samples too far apart in time (subsequent samp2 will be even newer)
+                    remove_ids.add(isamp1)
+                    break
                 if dt <= 0:
-                    # Duplicate samples or sample1 is newer. Remove sample1
-                    remove_idx_list.append(isamp1)
-                    continue
-                
+                    # Duplicate samples or out-of-order (sample1 is newer). Remove sample1
+                    remove_ids.add(isamp1)
+                    break
+
                 # Check angle difference (from camera solve) between samples:
                 dtheta = qt.get_quat_angular_diff(samp1.q_cam, samp2.q_cam)
                 if np.abs(dtheta) < self.min_angle_diff:
                     continue  # Samples too close in angle
 
-                # Pair samples and remove from candidate buffer:
+                # Pair samples and remove samp1 from candidate buffer (later).
+                # This prevents the same pair being used again if this method is
+                # re-run. Note that this loop will continue matching samp1.
                 self.diff_buffer.add_sample((samp1, samp2))
-                remove_idx_list.append(isamp1)
-                remove_idx_list.append(isamp2)
+                remove_ids.add(isamp1)
                 n_pairs += 1
 
-        if remove_idx_list:
-            self.candidate_buffer.remove_samples(remove_idx_list)
+        if remove_ids:
+            self.candidate_buffer.remove_samples(remove_ids)
         return n_pairs  # Number of successful pairings
 
     def solve(self, n_pairs=None):
@@ -282,16 +282,16 @@ def add_sample_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu:
         # Pair samples and solve
         if ((self._samples_since_last_pair_attempt >= self.min_n_solve) or
             (self.candidate_buffer.len >= self.candidate_buffer.max_buffer_length)):
-            self.purge_old_samples(timestamp)
-            self.purge_old_candidates()
+            self.purge_old_samples(timestamp)  # TODO: Run less frequently
+            self.purge_old_candidates()  # TODO: Run less frequently
             self.pair_samples()
             self.trim_buffers()
 
             # If the candidate buffer is still full after pairing, remove a 
             # batch of the older samples from the buffer
             if self.candidate_buffer.len >= self.candidate_buffer.max_buffer_length:
-                remove_list = list(range(self.min_n_solve))
-                self.candidate_buffer.remove_samples(remove_list)
+                remove_set = set(range(self.min_n_solve))
+                self.candidate_buffer.remove_samples(remove_set)
             
             self._samples_since_last_pair_attempt = 0
         else:

From ff88d5aad90efcdd1d47f64c36eed2465fe0b34f Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sat, 8 Aug 2026 01:43:10 +0200
Subject: [PATCH 18/45] Simplify. Return solution

---
 .../PiFinder/imu/imu_align/imu_alignment.py   | 27 +++++++++++--------
 1 file changed, 16 insertions(+), 11 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index 5c89d31c3..691e93df2 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -80,6 +80,7 @@
 
 from PiFinder.types.coordinates import RaDecRoll
 from PiFinder.pointing_model import quaternion_transforms as qt
+from PiFinder.imu.imu_align.hand_eye_solver import solve_rotation
 
 list_of_quats = list[quaternion.quaternion]
 
@@ -237,12 +238,9 @@ def pair_samples(self) -> int:
             for isamp2, samp2 in enumerate(self.candidate_buffer.buffer[isamp1+1:]):
                 # Check time difference between samples:
                 dt = samp2.timestamp - samp1.timestamp
-                if dt > self.max_time_diff:
-                    # Samples too far apart in time (subsequent samp2 will be even newer)
-                    remove_ids.add(isamp1)
-                    break
-                if dt <= 0:
-                    # Duplicate samples or out-of-order (sample1 is newer). Remove sample1
+                if dt > self.max_time_diff or dt <= 0:
+                    # 1) Samples too far apart in time (subsequent samp2 will be even newer), or
+                    # 2) Duplicate samples or out-of-order (sample1 is newer). Remove sample1
                     remove_ids.add(isamp1)
                     break
 
@@ -269,8 +267,15 @@ def solve(self, n_pairs=None):
         """
         if n_pairs is None:
             n_pairs = self.diff_buffer.len  # Use all available data
-        #TODO
-        return None
+
+        q_cam_list = []
+        q_imu_list = []
+        for samp_cam, samp_imu in self.diff_buffer.buffer:
+            q_cam_list.append(samp_imu.q_cam)
+            q_imu_list.append(samp_imu.q_imu)
+
+        q_cam2imu = solve_rotation(q_cam_list, q_imu_list, residual_threshold = 0.01, verbose=False)
+        return q_cam2imu
 
     def add_sample_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
         """
@@ -282,10 +287,10 @@ def add_sample_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu:
         # Pair samples and solve
         if ((self._samples_since_last_pair_attempt >= self.min_n_solve) or
             (self.candidate_buffer.len >= self.candidate_buffer.max_buffer_length)):
-            self.purge_old_samples(timestamp)  # TODO: Run less frequently
-            self.purge_old_candidates()  # TODO: Run less frequently
+            #self.purge_old_samples(timestamp)  # TODO: Run less frequently
+            #self.purge_old_candidates()  # TODO: Run less frequently
             self.pair_samples()
-            self.trim_buffers()
+            #self.trim_buffers()
 
             # If the candidate buffer is still full after pairing, remove a 
             # batch of the older samples from the buffer

From b71e81e9709498e699c838e1cac28f12583c4788 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sat, 15 Aug 2026 23:09:26 +0200
Subject: [PATCH 19/45] Refactor. Class for diagnostics, use logging

---
 .../PiFinder/imu/imu_align/hand_eye_solver.py | 92 +++++++++++++------
 .../PiFinder/imu/imu_align/imu_alignment.py   |  4 +-
 python/uv.lock                                |  2 +-
 3 files changed, 69 insertions(+), 29 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index ddb9517f1..a95b67524 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -7,6 +7,7 @@
 Where the goal is to solve for the rotation q_12. Given enough measurements of 
 q1 and q2, we can solve for q_12.
 """
+from dataclasses import dataclass
 import logging
 import numpy as np
 import quaternion  # Note: numpy-quaternion convention: quaternion(w, x, y, z)
@@ -18,10 +19,15 @@
 
 list_of_quats = list[quaternion.quaternion]
 
-logger = logging.getLogger("IMU.Align")
+logger = logging.getLogger("IMU.AlignSolver")
 
 N_UNKNOWN_PARAMS = 3  # Number of unknown parameters in the problem to solve
 
+@dataclass
+class HandEyeSolverDiagnostics:
+    residuals: np.ndarray
+    
+
 def residual_rotation_vector(x,  # (3,) Trial solution (q as rotation vector) 
                              q1_list: list_of_quats,  # List of rotation quaternions
                              q2_list: list_of_quats 
@@ -50,16 +56,27 @@ def solve_rotation(
         q1_list: list_of_quats,  # List of rotation quaternions
         q2_list: list_of_quats,
         x0: Union[np.ndarray, list] = np.zeros(N_UNKNOWN_PARAMS),  # Initial guess
-        residual_threshold = 0.01,  # Reject samples with residual > resid_threshold in first pass
-        verbose=True
-        ):
+        ) -> tuple[quaternion.quaternion, HandEyeSolverDiagnostics]:
     """
-    Solve the quaternion form of the hand-eye problem
+    Solve the quaternion form of the hand-eye problem using least-squares
+    optimization of the rotation q_12 parameterized as a rotation vector:
+
     dq1 * q_12 = q_12 * dq2
 
     Where q_12 is the unknown rotation that rotates q1 to q2
+
+    x0 is the initial guess for q_12 as a rotation vector. The default (zeros)
+    is the identity rotation.
     """
-    # Solve for x by non-linear least squares (Levenberg-Marquardt)
+    if len(q1_list) != len(q2_list):
+        raise ValueError("q1_list and q2_list must be the same length")
+    if len(q1_list) < N_UNKNOWN_PARAMS:
+        raise ValueError(f"q1_list and q2_list must have at least "
+                        f"{N_UNKNOWN_PARAMS} elements. Got {len(q1_list)}")
+    if len(x0) != N_UNKNOWN_PARAMS:
+        raise ValueError("x0 must be a length-3 vector")
+
+    logger.debug(f"Solving for relative rotation from {len(q1_list)} sample pairs.")
     # TODO: Tune LM params
     # TODO: Calculate the Jacobians analytically? Current numerical Jacobians is probably fast enough?
     result = least_squares(residual_rotation_vector, x0, method='lm', 
@@ -68,31 +85,54 @@ def solve_rotation(
     #result = least_squares(residual_rotation_vector, x0, loss='cauchy', 
     #                       args=(dq_cam, dq_imu))
 
-    # Re-run least-squares with outliers removed
-    if residual_threshold is not None:
-        # NOTE: Each quaternion measurement is converted to rotation vectors with 3 values
-        resid_reshaped = result.fun.reshape(-1, 3)  # Each row is a sample
-        msk_accept = np.all(np.abs(resid_reshaped) < residual_threshold, axis=1)
-        q1_accept = np.array(q1_list)[msk_accept]
-        q2_accept = np.array(q2_list)[msk_accept]
-        if verbose:
-            print(f"Accepted {np.sum(msk_accept)}/{resid_reshaped.shape[0]} samples.")
-        # Run least-squares again (using previous solution as the initial guess)
-        result = least_squares(residual_rotation_vector, result.x, args=(q1_accept, q2_accept))
-
     # Convert estimate from rotation vector to quaternion
     q_12 = quaternion.from_rotation_vector(result.x)
-    
-    if verbose:
-        print(f"Estimated q_cam2imu: q_cam2imu={q_12}, ",
-            f"Func evaluations: {result.nfev}, Cost = {result.cost:.4g}, ", 
+
+    logger.debug(f"Solved for relative rotation q_12={q_12}, "
+            f"Func evaluations: {result.nfev}, Cost = {result.cost:.4g}, "
             f"Success: {result.success}, {result.message}")
 
     # Diagnostics  TODO: Return these
-    sigma_total, condition_number = _solution_diagnostics(result)
-    residuals = result.fun
+    #sigma_total, condition_number = _solution_diagnostics(result)
+    diagnostics = HandEyeSolverDiagnostics(residuals=result.fun)
+
+    return q_12, diagnostics
+
+
+def solve_rotation_with_outlier_removal(
+        q1_list: list_of_quats,  # List of rotation quaternions
+        q2_list: list_of_quats,
+        x0: Union[np.ndarray, list] = np.zeros(N_UNKNOWN_PARAMS),  # Initial guess
+        residual_threshold = 0.8,  # Reject samples with residual > resid_threshold in first pass
+        n_min_samples = N_UNKNOWN_PARAMS,  # Minimum number of sample pairs for a solution
+        ):
+    """
+    Solve the hand-eye problem with a single pass of outlier rejection (see 
+    solve_rotation() for details).
+    """
+    # First pass:
+    q12_solution, diagnostics = solve_rotation(q1_list, q2_list, x0)
+    if residual_threshold is None:
+        return q12_solution, diagnostics
+
+    # Second pass: Re-run least-squares with outliers removed
+    resid_reshaped = diagnostics.residuals.reshape(-1, 3)  # Each row is a sample
+    msk_accept = np.all(np.abs(resid_reshaped) < residual_threshold, axis=1)
+    if not np.all(msk_accept):
+        logger.debug("Re-solving for imu/camera alignment using "
+            f"{np.sum(msk_accept)}/{resid_reshaped.shape[0]} samples.")
+
+        if np.sum(msk_accept) < n_min_samples:
+            np.info(f"Less than {n_min_samples} samples remain. Exiting outlier removal.")
+            return q12_solution, diagnostics
+
+        # Re-run using previous solution as the initial guess
+        q1_accept = np.array(q1_list)[msk_accept]
+        q2_accept = np.array(q2_list)[msk_accept]
+        x0 = quaternion.as_rotation_vector(q12_solution)
+        q12_solution, diagnostics = solve_rotation(q1_list, q2_list, x0)
 
-    return q_12
+    return q12_solution, diagnostics
 
 
 def _solution_diagnostics(result):
@@ -254,7 +294,7 @@ def simulate_quaternion_measurements(
     # Reject small rotations
 
     # solve
-    q_12_est, sigma_total, condition_number = solve_rotation(
+    q_12_est, diagnostics = solve_rotation(
         q1, q2, residual_threshold = 0.01, verbose=True)
 
     # Results
diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index 691e93df2..ee9f3d219 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -80,7 +80,7 @@
 
 from PiFinder.types.coordinates import RaDecRoll
 from PiFinder.pointing_model import quaternion_transforms as qt
-from PiFinder.imu.imu_align.hand_eye_solver import solve_rotation
+from PiFinder.imu.imu_align.hand_eye_solver import solve_rotation, solve_rotation_with_outlier_removal
 
 list_of_quats = list[quaternion.quaternion]
 
@@ -274,7 +274,7 @@ def solve(self, n_pairs=None):
             q_cam_list.append(samp_imu.q_cam)
             q_imu_list.append(samp_imu.q_imu)
 
-        q_cam2imu = solve_rotation(q_cam_list, q_imu_list, residual_threshold = 0.01, verbose=False)
+        q_cam2imu, diagnostics = solve_rotation(q_cam_list, q_imu_list)
         return q_cam2imu
 
     def add_sample_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
diff --git a/python/uv.lock b/python/uv.lock
index bda020730..c0ba0910c 100644
--- a/python/uv.lock
+++ b/python/uv.lock
@@ -1,3 +1,3 @@
 version = 1
 revision = 3
-requires-python = ">=3.13"
+requires-python = ">=3.9"

From 05ac43c4171bd4f4a4802cc01decf9124c336f72 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sun, 16 Aug 2026 00:10:14 +0200
Subject: [PATCH 20/45] Runs but solution isn't sensible

---
 python/PiFinder/imu/imu_align/hand_eye_solver.py |  6 +++++-
 python/PiFinder/imu/imu_align/imu_alignment.py   | 16 +++++++++++++---
 .../pointing_model/quaternion_transforms.py      |  2 +-
 3 files changed, 19 insertions(+), 5 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index a95b67524..13b384475 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -77,6 +77,11 @@ def solve_rotation(
         raise ValueError("x0 must be a length-3 vector")
 
     logger.debug(f"Solving for relative rotation from {len(q1_list)} sample pairs.")
+
+    # TODO: This is inefficient if re-run by outlier removal. Also not sure if necessary?
+    q1_list = ensure_quat_list_continuity(q1_list)
+    q2_list = ensure_quat_list_continuity(q2_list)
+
     # TODO: Tune LM params
     # TODO: Calculate the Jacobians analytically? Current numerical Jacobians is probably fast enough?
     result = least_squares(residual_rotation_vector, x0, method='lm', 
@@ -169,7 +174,6 @@ def _solution_diagnostics(result):
 
 # ------- Helper functions -------
 
-
 def ensure_quat_list_continuity(q_list: list_of_quats) -> list_of_quats:
     """
     Ensures that consecutive quaternions in the list have consistent signs (due
diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index ee9f3d219..3b328062f 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -152,7 +152,8 @@ def __init__(self, candidate_buffer_length=60, min_n_solve=10,
         candidate_buffer_length: Should be around sample_freq * max_time_diff
         """
         self.candidate_buffer = SampleBuffer(max_buffer_length=candidate_buffer_length)
-        self.diff_buffer = SampleBuffer(max_buffer_length=min_n_solve)
+        diff_buffer_length = candidate_buffer_length  # TODO: Come up with a better value
+        self.diff_buffer = SampleBuffer(max_buffer_length=diff_buffer_length)
 
         self.min_n_solve = min_n_solve
         self.max_time_diff = max_time_diff
@@ -231,11 +232,13 @@ def pair_samples(self) -> int:
         """
         n_pairs = 0
         if self.candidate_buffer.len == 0:
+            logger.debug("No samples in candidate buffer for pairing.")
             return n_pairs
 
         remove_ids = set()
         for isamp1, samp1 in enumerate(self.candidate_buffer.buffer[:-1]):
-            for isamp2, samp2 in enumerate(self.candidate_buffer.buffer[isamp1+1:]):
+            for isamp2 in range(isamp1+1, self.candidate_buffer.len):
+                samp2 =self.candidate_buffer.buffer[isamp2]
                 # Check time difference between samples:
                 dt = samp2.timestamp - samp1.timestamp
                 if dt > self.max_time_diff or dt <= 0:
@@ -253,11 +256,18 @@ def pair_samples(self) -> int:
                 # This prevents the same pair being used again if this method is
                 # re-run. Note that this loop will continue matching samp1.
                 self.diff_buffer.add_sample((samp1, samp2))
-                remove_ids.add(isamp1)
                 n_pairs += 1
+                remove_ids.add(isamp1)
+                if self.diff_buffer.len >= self.diff_buffer.max_buffer_length:
+                    break
+            if self.diff_buffer.len >= self.diff_buffer.max_buffer_length:
+                break
+
+        logger.debug(f"paired {n_pairs} from {self.candidate_buffer.len} samples.")
 
         if remove_ids:
             self.candidate_buffer.remove_samples(remove_ids)
+
         return n_pairs  # Number of successful pairings
 
     def solve(self, n_pairs=None):
diff --git a/python/PiFinder/pointing_model/quaternion_transforms.py b/python/PiFinder/pointing_model/quaternion_transforms.py
index bf82654df..cf6dbebef 100644
--- a/python/PiFinder/pointing_model/quaternion_transforms.py
+++ b/python/PiFinder/pointing_model/quaternion_transforms.py
@@ -73,7 +73,7 @@ def ensure_quat_continuity(q_prev: quaternion.quaternion,
     q1 = quaternion.as_float_array(q_new)
 
     if np.dot(q0, q1) < 0:
-        return quaternion.quaternion(-q1)
+        return -q_new
     else:
         return q_new
 

From 6edea94a1aa280fbcab8b4840da574deb89f6598 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sun, 16 Aug 2026 00:33:21 +0200
Subject: [PATCH 21/45] Trying out quaternion continuity

---
 .../PiFinder/imu/imu_align/hand_eye_solver.py |  4 ++--
 .../PiFinder/imu/imu_align/imu_alignment.py   | 22 +++++++++++++------
 2 files changed, 17 insertions(+), 9 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index 13b384475..18e637848 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -79,8 +79,8 @@ def solve_rotation(
     logger.debug(f"Solving for relative rotation from {len(q1_list)} sample pairs.")
 
     # TODO: This is inefficient if re-run by outlier removal. Also not sure if necessary?
-    q1_list = ensure_quat_list_continuity(q1_list)
-    q2_list = ensure_quat_list_continuity(q2_list)
+    #q1_list = ensure_quat_list_continuity(q1_list)
+    #q2_list = ensure_quat_list_continuity(q2_list)
 
     # TODO: Tune LM params
     # TODO: Calculate the Jacobians analytically? Current numerical Jacobians is probably fast enough?
diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index 3b328062f..c0181f954 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -170,15 +170,23 @@ def trim_buffers(self):
         self.candidate_buffer.trim_to_max_length()
         self.diff_buffer.trim_to_max_length()
 
-    def add_sample(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
+    def add_candidate(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
         """
         Add to the candidate_buffer the camera solve & corresponding IMU sample
         from integrator.
         """
         if timestamp is None or cam_eq is None or cam_eq.valid is False or q_x2imu is None:
             return
-        self.candidate_buffer.add_sample(
-            CameraImuSample(timestamp, cam_eq.as_quaternion(), q_x2imu))
+
+        # Ensure quaternion continuity from previous candidate sample
+        q_cam = cam_eq.as_quaternion()
+        if self.candidate_buffer.len > 0:
+            last_candidate = self.candidate_buffer.buffer[-1]
+            q_cam = qt.ensure_quat_continuity(last_candidate.q_cam, q_cam)
+            q_imu = qt.ensure_quat_continuity(last_candidate.q_imu, q_x2imu)
+            self.candidate_buffer.add_sample(CameraImuSample(timestamp, q_cam, q_imu))
+        else:
+            self.candidate_buffer.add_sample(CameraImuSample(timestamp, q_cam, q_x2imu))
 
     def purge_old_samples(self, current_time: float):
         """
@@ -287,12 +295,12 @@ def solve(self, n_pairs=None):
         q_cam2imu, diagnostics = solve_rotation(q_cam_list, q_imu_list)
         return q_cam2imu
 
-    def add_sample_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
+    def add_candidate_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
         """
-        For general use, call this method. Add a new sample to the buffer. When
-        the buffer fills up, pair samples and solve.
+        For general use, call this method. Add a new candidate to the buffer.
+        When the buffer fills up, pair samples and solve.
         """
-        self.add_sample(timestamp, cam_eq, q_x2imu)
+        self.add_candidate(timestamp, cam_eq, q_x2imu)
 
         # Pair samples and solve
         if ((self._samples_since_last_pair_attempt >= self.min_n_solve) or

From 975667b74cf83fa54abdb4ff7ac04cb2c8e05309 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sun, 16 Aug 2026 09:18:54 +0200
Subject: [PATCH 22/45] Fixed by giving relative rotation quaternions dq to the
 solver

---
 .../PiFinder/imu/imu_align/imu_alignment.py   | 21 ++++++++++---------
 1 file changed, 11 insertions(+), 10 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index c0181f954..9428d2d36 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -180,13 +180,13 @@ def add_candidate(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion
 
         # Ensure quaternion continuity from previous candidate sample
         q_cam = cam_eq.as_quaternion()
-        if self.candidate_buffer.len > 0:
+        if self.candidate_buffer.len == 0:
+            self.candidate_buffer.add_sample(CameraImuSample(timestamp, q_cam, q_x2imu))
+        else:
             last_candidate = self.candidate_buffer.buffer[-1]
             q_cam = qt.ensure_quat_continuity(last_candidate.q_cam, q_cam)
             q_imu = qt.ensure_quat_continuity(last_candidate.q_imu, q_x2imu)
             self.candidate_buffer.add_sample(CameraImuSample(timestamp, q_cam, q_imu))
-        else:
-            self.candidate_buffer.add_sample(CameraImuSample(timestamp, q_cam, q_x2imu))
 
     def purge_old_samples(self, current_time: float):
         """
@@ -281,18 +281,19 @@ def pair_samples(self) -> int:
     def solve(self, n_pairs=None):
         """
         Solve for the alignment between the camera and IMU using at least the
-        last n_pairs or all available pairs (if None).
+        last n_pairs or all available pairs (if None) in diff_buffer.
         """
         if n_pairs is None:
             n_pairs = self.diff_buffer.len  # Use all available data
 
-        q_cam_list = []
-        q_imu_list = []
-        for samp_cam, samp_imu in self.diff_buffer.buffer:
-            q_cam_list.append(samp_imu.q_cam)
-            q_imu_list.append(samp_imu.q_imu)
+        # Generate relative rotation quaternions between paired samp1 and samp2
+        dq_cam_list = []
+        dq_imu_list = []
+        for samp1, samp2 in self.diff_buffer.buffer:
+            dq_cam_list.append(samp1.q_cam.conj() * samp2.q_cam)
+            dq_imu_list.append(samp1.q_imu.conj() * samp2.q_imu)
 
-        q_cam2imu, diagnostics = solve_rotation(q_cam_list, q_imu_list)
+        q_cam2imu, diagnostics = solve_rotation(dq_cam_list, dq_imu_list)
         return q_cam2imu
 
     def add_candidate_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):

From 66f7a1146d9bf5e80c7a62ce32e1580bd7a802bd Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sun, 16 Aug 2026 21:36:22 +0200
Subject: [PATCH 23/45] Refactor diagnostics

---
 .../PiFinder/imu/imu_align/hand_eye_solver.py | 28 ++++++---
 .../PiFinder/imu/imu_align/imu_alignment.py   | 60 ++++++++++++-------
 2 files changed, 59 insertions(+), 29 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index 18e637848..e618125c7 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -23,10 +23,25 @@
 
 N_UNKNOWN_PARAMS = 3  # Number of unknown parameters in the problem to solve
 
-@dataclass
 class HandEyeSolverDiagnostics:
     residuals: np.ndarray
-    
+    residual_norms: np.ndarray  # Residual norms of each sample [rad]
+    rotation_angles: np.ndarray  # Rotation angles of each sample [rad]
+
+    # Optional
+    meta_data: dict = {}
+
+    def __init__(self, lsq_result, q1_list: list_of_quats, q2_list: list_of_quats):
+        self.residuals = lsq_result.fun
+
+        rs = self.residuals.reshape((-1, N_UNKNOWN_PARAMS))  # Each row corresponds to a sample 
+        self.residual_norms = np.linalg.norm(rs, axis=1)  # Residual per sample in radians
+
+        # Calculate rotations of each sample [rad]
+        self.rotation_angles = [qt.get_quat_angular_diff(q1, q2) for q1, q2 in zip(q1_list, q2_list)]
+
+        self.results = lsq_result
+
 
 def residual_rotation_vector(x,  # (3,) Trial solution (q as rotation vector) 
                              q1_list: list_of_quats,  # List of rotation quaternions
@@ -63,7 +78,7 @@ def solve_rotation(
 
     dq1 * q_12 = q_12 * dq2
 
-    Where q_12 is the unknown rotation that rotates q1 to q2
+    Where q_12 is the unknown rotation that rotates q1 to q2.
 
     x0 is the initial guess for q_12 as a rotation vector. The default (zeros)
     is the identity rotation.
@@ -78,10 +93,6 @@ def solve_rotation(
 
     logger.debug(f"Solving for relative rotation from {len(q1_list)} sample pairs.")
 
-    # TODO: This is inefficient if re-run by outlier removal. Also not sure if necessary?
-    #q1_list = ensure_quat_list_continuity(q1_list)
-    #q2_list = ensure_quat_list_continuity(q2_list)
-
     # TODO: Tune LM params
     # TODO: Calculate the Jacobians analytically? Current numerical Jacobians is probably fast enough?
     result = least_squares(residual_rotation_vector, x0, method='lm', 
@@ -99,7 +110,8 @@ def solve_rotation(
 
     # Diagnostics  TODO: Return these
     #sigma_total, condition_number = _solution_diagnostics(result)
-    diagnostics = HandEyeSolverDiagnostics(residuals=result.fun)
+    diagnostics = HandEyeSolverDiagnostics(result, q1_list, q2_list)
+    diagnostics.meta_data["lsq_result"] = result
 
     return q_12, diagnostics
 
diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index 9428d2d36..7b4b945f4 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -139,7 +139,7 @@ class ImuCameraAlignment:
     gyro drift over the time between samples.
     """
     candidate_buffer: SampleBuffer  # Buffer of camera/IMU samples
-    diff_buffer: SampleBuffer  # Buffer of paired differences in camera/IMU samples
+    pair_buffer: SampleBuffer  # Buffer of paired samples ofcamera/IMU samples
 
     min_n_solve: int  # Minimum number of samples for solve
     max_time_diff: float  # [s] Maximum time difference between pairs of samples
@@ -150,10 +150,16 @@ def __init__(self, candidate_buffer_length=60, min_n_solve=10,
                  max_time_diff=2.0, min_angle_diff=np.deg2rad(5), max_age=1200):
         """
         candidate_buffer_length: Should be around sample_freq * max_time_diff
+
+        :param candidate_buffer_length: [int] Number of candidate samples to buffer
+        :param min_n_solve: [int] Minimum number of samples required for solve
+        :param max_time_diff: [s] Maximum allowed time difference between pairs of samples
+        :param min_angle_diff: [rad] Minimum allowed angle difference between pairs of samples
+        :param max_age: [s] Remove samples older than this. None to ignore
         """
         self.candidate_buffer = SampleBuffer(max_buffer_length=candidate_buffer_length)
         diff_buffer_length = candidate_buffer_length  # TODO: Come up with a better value
-        self.diff_buffer = SampleBuffer(max_buffer_length=diff_buffer_length)
+        self.pair_buffer = SampleBuffer(max_buffer_length=diff_buffer_length)
 
         self.min_n_solve = min_n_solve
         self.max_time_diff = max_time_diff
@@ -164,11 +170,11 @@ def __init__(self, candidate_buffer_length=60, min_n_solve=10,
 
     def reset_buffers(self):
         self.candidate_buffer.reset_buffer()
-        self.diff_buffer.reset_buffer()
+        self.pair_buffer.reset_buffer()
 
     def trim_buffers(self):
         self.candidate_buffer.trim_to_max_length()
-        self.diff_buffer.trim_to_max_length()
+        self.pair_buffer.trim_to_max_length()
 
     def add_candidate(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
         """
@@ -195,6 +201,9 @@ def purge_old_samples(self, current_time: float):
 
         This should be run on a schedule every self.max_age [s].
         """
+        if self.max_age is None:
+            return
+
         allowed_timestamp = current_time - self.max_age  # Purge anything older than this
         
         # Purge candidate_buffer:
@@ -204,11 +213,11 @@ def purge_old_samples(self, current_time: float):
             self.candidate_buffer.remove_samples(set(remove_idx_list))
 
         # Purge diff_buffer:
-        remove_idx_list = [i for i, (samp1, samp2) in enumerate(self.diff_buffer.buffer) 
+        remove_idx_list = [i for i, (samp1, samp2) in enumerate(self.pair_buffer.buffer) 
                            if samp1.timestamp < allowed_timestamp 
                            or samp2.timestamp < allowed_timestamp]
         if remove_idx_list:
-            self.diff_buffer.remove_samples(set(remove_idx_list))
+            self.pair_buffer.remove_samples(set(remove_idx_list))
 
     def purge_old_candidates(self):
         """
@@ -260,15 +269,16 @@ def pair_samples(self) -> int:
                 if np.abs(dtheta) < self.min_angle_diff:
                     continue  # Samples too close in angle
 
-                # Pair samples and remove samp1 from candidate buffer (later).
-                # This prevents the same pair being used again if this method is
-                # re-run. Note that this loop will continue matching samp1.
-                self.diff_buffer.add_sample((samp1, samp2))
+                # Pair samples and remove samp1 from candidate buffer after FOR
+                # loops. This prevents the same pair being used again if this
+                # method is re-run. Note that this loop will continue pairing
+                # with samp1.
+                self.pair_buffer.add_sample((samp1, samp2))
                 n_pairs += 1
                 remove_ids.add(isamp1)
-                if self.diff_buffer.len >= self.diff_buffer.max_buffer_length:
+                if self.pair_buffer.len >= self.pair_buffer.max_buffer_length:
                     break
-            if self.diff_buffer.len >= self.diff_buffer.max_buffer_length:
+            if self.pair_buffer.len >= self.pair_buffer.max_buffer_length:
                 break
 
         logger.debug(f"paired {n_pairs} from {self.candidate_buffer.len} samples.")
@@ -284,17 +294,25 @@ def solve(self, n_pairs=None):
         last n_pairs or all available pairs (if None) in diff_buffer.
         """
         if n_pairs is None:
-            n_pairs = self.diff_buffer.len  # Use all available data
+            n_pairs = self.pair_buffer.len  # Use all available data
+        if n_pairs <= self.min_n_solve:
+            raise ValueError(f"Oly {n_pairs} samples available for solve. Need {self.min_n_solve}.")
 
         # Generate relative rotation quaternions between paired samp1 and samp2
+        # The amount of relative rotation for camera and IMU should be the same
+        # and this will solve the relative rotation between them. 
         dq_cam_list = []
         dq_imu_list = []
-        for samp1, samp2 in self.diff_buffer.buffer:
+        dt_list = []
+        for samp1, samp2 in self.pair_buffer.buffer:
             dq_cam_list.append(samp1.q_cam.conj() * samp2.q_cam)
             dq_imu_list.append(samp1.q_imu.conj() * samp2.q_imu)
+            dt_list.append(samp2.timestamp - samp1.timestamp)
 
+        # Solve
         q_cam2imu, diagnostics = solve_rotation(dq_cam_list, dq_imu_list)
-        return q_cam2imu
+        diagnostics.meta_data["time_differences"] = dt_list
+        return q_cam2imu, diagnostics
 
     def add_candidate_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
         """
@@ -303,7 +321,7 @@ def add_candidate_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2i
         """
         self.add_candidate(timestamp, cam_eq, q_x2imu)
 
-        # Pair samples and solve
+        # Pair samples: Runs periodically
         if ((self._samples_since_last_pair_attempt >= self.min_n_solve) or
             (self.candidate_buffer.len >= self.candidate_buffer.max_buffer_length)):
             #self.purge_old_samples(timestamp)  # TODO: Run less frequently
@@ -322,9 +340,9 @@ def add_candidate_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2i
             self._samples_since_last_pair_attempt += 1
 
         # Solve if there are enough samples
-        if self.diff_buffer.len >= self.min_n_solve:
-            solution = self.solve()
-            self.diff_buffer.reset_buffer()  # Flush the values used for solve
-            return solution
+        if self.pair_buffer.len >= self.min_n_solve:
+            q_cam2imu, diagnostics = self.solve()
+            self.pair_buffer.reset_buffer()  # Flush the values used for solve
+            return q_cam2imu, diagnostics
         else:
-            return None
+            return None, None

From 5081d93ff74778195d01c71bc8562609541fc24f Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sun, 16 Aug 2026 23:35:28 +0200
Subject: [PATCH 24/45] Calculate uncertainty at solution. Get outlier removing
 working - improvement but not a lot (still around 1.5 deg std error)

---
 .../PiFinder/imu/imu_align/hand_eye_solver.py | 70 +++++++++++++------
 .../PiFinder/imu/imu_align/imu_alignment.py   |  3 +-
 2 files changed, 51 insertions(+), 22 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index e618125c7..f96e66762 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -11,7 +11,7 @@
 import logging
 import numpy as np
 import quaternion  # Note: numpy-quaternion convention: quaternion(w, x, y, z)
-from scipy.optimize import least_squares
+from scipy.optimize import least_squares, OptimizeResult
 import time
 from typing import Union
 
@@ -24,23 +24,52 @@
 N_UNKNOWN_PARAMS = 3  # Number of unknown parameters in the problem to solve
 
 class HandEyeSolverDiagnostics:
-    residuals: np.ndarray
+    lsq_result: OptimizeResult  # Result from scipy.optimize.least_squares
+
     residual_norms: np.ndarray  # Residual norms of each sample [rad]
     rotation_angles: np.ndarray  # Rotation angles of each sample [rad]
+    sol_cov_matrix: np.ndarray  # Solution covariance matrix
+    sol_angle_error: np.ndarray  # Solution angle error
 
     # Optional
-    meta_data: dict = {}
+    meta_data: dict
 
     def __init__(self, lsq_result, q1_list: list_of_quats, q2_list: list_of_quats):
-        self.residuals = lsq_result.fun
+        self.lsq_result = lsq_result
 
-        rs = self.residuals.reshape((-1, N_UNKNOWN_PARAMS))  # Each row corresponds to a sample 
-        self.residual_norms = np.linalg.norm(rs, axis=1)  # Residual per sample in radians
+        # Residual norm per sample (collapse the 3 measurements per sample into one)
+        resid = lsq_result.fun.reshape((-1, N_UNKNOWN_PARAMS))  # Each row corresponds to a sample 
+        self.residual_norms = np.linalg.norm(resid, axis=1)  # Residual per sample in radians
 
         # Calculate rotations of each sample [rad]
         self.rotation_angles = [qt.get_quat_angular_diff(q1, q2) for q1, q2 in zip(q1_list, q2_list)]
 
-        self.results = lsq_result
+        self.sol_cov_matrix, self.sol_angle_error =self.calculate_solution_uncertainty()
+        self.meta_data = {}
+
+    def calculate_solution_uncertainty(self):
+        """
+        Calculate the standard error of the solution:
+        Cov = sigma ** 2 * inv(J.T @ J)
+        """
+        # Extract Jacobian from least_squares result
+        J = self.lsq_result.jac  # Jacobian matrix (m_meas, n_sol)
+        m_meas, n_sol = J.shape
+
+        # Calculate the inverse using "backslash": Solve: (J.T @ J) @ X = I
+        # NOTE: Could be speeded up using QR decomposition but this is good enough
+        inv_JTJ, _, _, _ = np.linalg.lstsq(J.T @ J, np.eye(n_sol), rcond=None)
+
+        # Calculate reduced Chi-square
+        dof = m_meas - n_sol  # Degrees of freedom
+        rss = 2 * self.lsq_result.cost  # res.cost is 0.5 * sum(residuals**2)
+        chi_square = rss / dof
+
+        # Extract uncertainty 
+        sol_cov_matrix = chi_square * inv_JTJ
+        sol_angle_error = np.sqrt(np.trace(sol_cov_matrix))  # [rad]
+
+        return sol_cov_matrix, sol_angle_error
 
 
 def residual_rotation_vector(x,  # (3,) Trial solution (q as rotation vector) 
@@ -97,9 +126,6 @@ def solve_rotation(
     # TODO: Calculate the Jacobians analytically? Current numerical Jacobians is probably fast enough?
     result = least_squares(residual_rotation_vector, x0, method='lm', 
                            args=(q1_list, q2_list))
-    # TODO: Investigate using robust loss functions?
-    #result = least_squares(residual_rotation_vector, x0, loss='cauchy', 
-    #                       args=(dq_cam, dq_imu))
 
     # Convert estimate from rotation vector to quaternion
     q_12 = quaternion.from_rotation_vector(result.x)
@@ -108,10 +134,7 @@ def solve_rotation(
             f"Func evaluations: {result.nfev}, Cost = {result.cost:.4g}, "
             f"Success: {result.success}, {result.message}")
 
-    # Diagnostics  TODO: Return these
-    #sigma_total, condition_number = _solution_diagnostics(result)
     diagnostics = HandEyeSolverDiagnostics(result, q1_list, q2_list)
-    diagnostics.meta_data["lsq_result"] = result
 
     return q_12, diagnostics
 
@@ -120,7 +143,7 @@ def solve_rotation_with_outlier_removal(
         q1_list: list_of_quats,  # List of rotation quaternions
         q2_list: list_of_quats,
         x0: Union[np.ndarray, list] = np.zeros(N_UNKNOWN_PARAMS),  # Initial guess
-        residual_threshold = 0.8,  # Reject samples with residual > resid_threshold in first pass
+        mad_threshold = 4.45,
         n_min_samples = N_UNKNOWN_PARAMS,  # Minimum number of sample pairs for a solution
         ):
     """
@@ -129,25 +152,30 @@ def solve_rotation_with_outlier_removal(
     """
     # First pass:
     q12_solution, diagnostics = solve_rotation(q1_list, q2_list, x0)
-    if residual_threshold is None:
+    if mad_threshold is None:
         return q12_solution, diagnostics
 
     # Second pass: Re-run least-squares with outliers removed
-    resid_reshaped = diagnostics.residuals.reshape(-1, 3)  # Each row is a sample
-    msk_accept = np.all(np.abs(resid_reshaped) < residual_threshold, axis=1)
+    median = np.median(diagnostics.residual_norms)
+    mad = np.median(np.abs(diagnostics.residual_norms - median))
+    msk_accept = diagnostics.residual_norms < (median + mad * mad_threshold)
     if not np.all(msk_accept):
         logger.debug("Re-solving for imu/camera alignment using "
-            f"{np.sum(msk_accept)}/{resid_reshaped.shape[0]} samples.")
+            f"{np.sum(msk_accept)}/{diagnostics.residual_norms.shape[0]} samples.")
 
         if np.sum(msk_accept) < n_min_samples:
             np.info(f"Less than {n_min_samples} samples remain. Exiting outlier removal.")
             return q12_solution, diagnostics
 
         # Re-run using previous solution as the initial guess
-        q1_accept = np.array(q1_list)[msk_accept]
-        q2_accept = np.array(q2_list)[msk_accept]
+        # TODO: NOT A LIST!
+        #q1_accept = np.array(q1_list)[msk_accept]
+        #q2_accept = np.array(q2_list)[msk_accept]
+        q1_accept = [q for ii, q in enumerate(q1_list) if msk_accept[ii]]
+        q2_accept = [q for ii, q in enumerate(q2_list) if msk_accept[ii]]
+
         x0 = quaternion.as_rotation_vector(q12_solution)
-        q12_solution, diagnostics = solve_rotation(q1_list, q2_list, x0)
+        q12_solution, diagnostics = solve_rotation(q1_accept, q2_accept, x0)
 
     return q12_solution, diagnostics
 
diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index 7b4b945f4..039a5e4f3 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -295,7 +295,7 @@ def solve(self, n_pairs=None):
         """
         if n_pairs is None:
             n_pairs = self.pair_buffer.len  # Use all available data
-        if n_pairs <= self.min_n_solve:
+        if n_pairs < self.min_n_solve:
             raise ValueError(f"Oly {n_pairs} samples available for solve. Need {self.min_n_solve}.")
 
         # Generate relative rotation quaternions between paired samp1 and samp2
@@ -311,6 +311,7 @@ def solve(self, n_pairs=None):
 
         # Solve
         q_cam2imu, diagnostics = solve_rotation(dq_cam_list, dq_imu_list)
+        #q_cam2imu, diagnostics = solve_rotation_with_outlier_removal(dq_cam_list, dq_imu_list)
         diagnostics.meta_data["time_differences"] = dt_list
         return q_cam2imu, diagnostics
 

From b27d469a6c32df11bccc08228cc476546a3df6e2 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Wed, 19 Aug 2026 12:47:15 +0200
Subject: [PATCH 25/45] Add logging

---
 .../PiFinder/imu/imu_align/hand_eye_solver.py | 33 ++++++++++++-------
 .../PiFinder/imu/imu_align/imu_alignment.py   | 15 +++++----
 2 files changed, 30 insertions(+), 18 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index f96e66762..64dc8884a 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -100,7 +100,7 @@ def solve_rotation(
         q1_list: list_of_quats,  # List of rotation quaternions
         q2_list: list_of_quats,
         x0: Union[np.ndarray, list] = np.zeros(N_UNKNOWN_PARAMS),  # Initial guess
-        ) -> tuple[quaternion.quaternion, HandEyeSolverDiagnostics]:
+        ) -> tuple[Union[quaternion.quaternion, None], HandEyeSolverDiagnostics]:
     """
     Solve the quaternion form of the hand-eye problem using least-squares
     optimization of the rotation q_12 parameterized as a rotation vector:
@@ -111,6 +111,8 @@ def solve_rotation(
 
     x0 is the initial guess for q_12 as a rotation vector. The default (zeros)
     is the identity rotation.
+
+    Returns None for q_12 if the solver failed to converge. 
     """
     if len(q1_list) != len(q2_list):
         raise ValueError("q1_list and q2_list must be the same length")
@@ -130,11 +132,14 @@ def solve_rotation(
     # Convert estimate from rotation vector to quaternion
     q_12 = quaternion.from_rotation_vector(result.x)
 
-    logger.debug(f"Solved for relative rotation q_12={q_12}, "
+    diagnostics = HandEyeSolverDiagnostics(result, q1_list, q2_list)
+    logger.debug(f"Ran solver for relative rotation: Solution q_12={q_12}, "
+            f"Solution uncertainty: {np.rad2deg(diagnostics.sol_angle_error):.2f} degrees, "
             f"Func evaluations: {result.nfev}, Cost = {result.cost:.4g}, "
             f"Success: {result.success}, {result.message}")
 
-    diagnostics = HandEyeSolverDiagnostics(result, q1_list, q2_list)
+    if not result.success:
+        return None, diagnostics
 
     return q_12, diagnostics
 
@@ -143,7 +148,7 @@ def solve_rotation_with_outlier_removal(
         q1_list: list_of_quats,  # List of rotation quaternions
         q2_list: list_of_quats,
         x0: Union[np.ndarray, list] = np.zeros(N_UNKNOWN_PARAMS),  # Initial guess
-        mad_threshold = 4.45,
+        mad_threshold = 4.45,  # Reject outlier above this multiple of MAD in first pass
         n_min_samples = N_UNKNOWN_PARAMS,  # Minimum number of sample pairs for a solution
         ):
     """
@@ -152,28 +157,32 @@ def solve_rotation_with_outlier_removal(
     """
     # First pass:
     q12_solution, diagnostics = solve_rotation(q1_list, q2_list, x0)
+    if q12_solution is None:
+        logger.debug("First-pass solve for imu/camera alignment failed to converge.")
+        return None, diagnostics
     if mad_threshold is None:
         return q12_solution, diagnostics
 
     # Second pass: Re-run least-squares with outliers removed
     median = np.median(diagnostics.residual_norms)
     mad = np.median(np.abs(diagnostics.residual_norms - median))
+    logger.debug(f"MAD after first-pass: {np.rad2deg(mad):.2f} degrees. "
+                 f"Solution uncertainty: {np.rad2deg(diagnostics.sol_angle_error):.2f} degrees.")
     msk_accept = diagnostics.residual_norms < (median + mad * mad_threshold)
-    if not np.all(msk_accept):
-        logger.debug("Re-solving for imu/camera alignment using "
+    if np.all(msk_accept):
+        logger.debug("No outliers. Returning solution from first-pass.")
+    else:
+        logger.debug("Running 2nd-pass solve for imu/camera alignment using "
             f"{np.sum(msk_accept)}/{diagnostics.residual_norms.shape[0]} samples.")
 
         if np.sum(msk_accept) < n_min_samples:
-            np.info(f"Less than {n_min_samples} samples remain. Exiting outlier removal.")
+            np.info(f"Less than {n_min_samples} samples remain. "
+                    "Not enough samples for outlier removal. Returning solution from first-pass.")
             return q12_solution, diagnostics
 
-        # Re-run using previous solution as the initial guess
-        # TODO: NOT A LIST!
-        #q1_accept = np.array(q1_list)[msk_accept]
-        #q2_accept = np.array(q2_list)[msk_accept]
+        # Solve again, using previous solution as the initial guess
         q1_accept = [q for ii, q in enumerate(q1_list) if msk_accept[ii]]
         q2_accept = [q for ii, q in enumerate(q2_list) if msk_accept[ii]]
-
         x0 = quaternion.as_rotation_vector(q12_solution)
         q12_solution, diagnostics = solve_rotation(q1_accept, q2_accept, x0)
 
diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index 039a5e4f3..8f01a4be9 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -281,10 +281,13 @@ def pair_samples(self) -> int:
             if self.pair_buffer.len >= self.pair_buffer.max_buffer_length:
                 break
 
-        logger.debug(f"paired {n_pairs} from {self.candidate_buffer.len} samples.")
+        logger.debug(f"Created {n_pairs}-way pairs from {self.candidate_buffer.len} candidate samples.")
 
         if remove_ids:
             self.candidate_buffer.remove_samples(remove_ids)
+        logger.debug(f"Removed {len(remove_ids)} samples from candidate buffer. "
+                     f"New candidate buffer length: {self.candidate_buffer.len}. "
+                     f"Pair buffer length: {self.pair_buffer.len}.")
 
         return n_pairs  # Number of successful pairings
 
@@ -310,15 +313,15 @@ def solve(self, n_pairs=None):
             dt_list.append(samp2.timestamp - samp1.timestamp)
 
         # Solve
-        q_cam2imu, diagnostics = solve_rotation(dq_cam_list, dq_imu_list)
-        #q_cam2imu, diagnostics = solve_rotation_with_outlier_removal(dq_cam_list, dq_imu_list)
+        #q_cam2imu, diagnostics = solve_rotation(dq_cam_list, dq_imu_list)
+        q_cam2imu, diagnostics = solve_rotation_with_outlier_removal(dq_cam_list, dq_imu_list)
         diagnostics.meta_data["time_differences"] = dt_list
         return q_cam2imu, diagnostics
 
     def add_candidate_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
         """
-        For general use, call this method. Add a new candidate to the buffer.
-        When the buffer fills up, pair samples and solve.
+        For general use, call this pipeline method. Add a new candidate to the
+        buffer. When the buffer fills up, pair samples and solve.
         """
         self.add_candidate(timestamp, cam_eq, q_x2imu)
 
@@ -328,7 +331,7 @@ def add_candidate_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2i
             #self.purge_old_samples(timestamp)  # TODO: Run less frequently
             #self.purge_old_candidates()  # TODO: Run less frequently
             self.pair_samples()
-            #self.trim_buffers()
+            #self.trim_buffers()  # TODO
 
             # If the candidate buffer is still full after pairing, remove a 
             # batch of the older samples from the buffer

From 026139ef870ffaf326e6706978de8b04081822fe Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Wed, 19 Aug 2026 16:43:14 +0200
Subject: [PATCH 26/45] Fixed issue with mis-matching time difference list
 length after outlier-removal

---
 .../PiFinder/imu/imu_align/hand_eye_solver.py | 38 +++++++++++++++----
 .../PiFinder/imu/imu_align/imu_alignment.py   |  8 ++--
 2 files changed, 35 insertions(+), 11 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index 64dc8884a..03968fc33 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -17,7 +17,9 @@
 
 import PiFinder.pointing_model.quaternion_transforms as qt
 
+# Typing:
 list_of_quats = list[quaternion.quaternion]
+list_of_float_pairs = list[tuple[float, float]]
 
 logger = logging.getLogger("IMU.AlignSolver")
 
@@ -26,6 +28,8 @@
 class HandEyeSolverDiagnostics:
     lsq_result: OptimizeResult  # Result from scipy.optimize.least_squares
 
+    sample_timestamps: Union[list_of_float_pairs, None]  # Timestamps of each sample-pair
+    sample_time_differences: Union[np.ndarray, None]  # Time differences between each sample [s]
     residual_norms: np.ndarray  # Residual norms of each sample [rad]
     rotation_angles: np.ndarray  # Rotation angles of each sample [rad]
     sol_cov_matrix: np.ndarray  # Solution covariance matrix
@@ -34,7 +38,20 @@ class HandEyeSolverDiagnostics:
     # Optional
     meta_data: dict
 
-    def __init__(self, lsq_result, q1_list: list_of_quats, q2_list: list_of_quats):
+    def __init__(self, lsq_result, q1_list: list_of_quats, q2_list: list_of_quats, 
+                 sample_timestamps: Union[list_of_float_pairs, None] = None):
+                
+        if len(q1_list) != len(q2_list):
+            raise ValueError("q1_list and q2_list must be the same length")
+        if sample_timestamps is None:
+            self.sample_timestamps = None
+            self.sample_time_differences = None
+        else:
+            if len(sample_timestamps) != len(q1_list):
+                raise ValueError("sample_timestamps must be the same length as q1_list and q2_list")
+            self.sample_timestamps = sample_timestamps.copy()
+            self.sample_time_differences = np.array([t2 - t1 for t1, t2 in self.sample_timestamps])
+
         self.lsq_result = lsq_result
 
         # Residual norm per sample (collapse the 3 measurements per sample into one)
@@ -100,6 +117,7 @@ def solve_rotation(
         q1_list: list_of_quats,  # List of rotation quaternions
         q2_list: list_of_quats,
         x0: Union[np.ndarray, list] = np.zeros(N_UNKNOWN_PARAMS),  # Initial guess
+        sample_timestamps: Union[list_of_float_pairs, None] = None,
         ) -> tuple[Union[quaternion.quaternion, None], HandEyeSolverDiagnostics]:
     """
     Solve the quaternion form of the hand-eye problem using least-squares
@@ -132,7 +150,7 @@ def solve_rotation(
     # Convert estimate from rotation vector to quaternion
     q_12 = quaternion.from_rotation_vector(result.x)
 
-    diagnostics = HandEyeSolverDiagnostics(result, q1_list, q2_list)
+    diagnostics = HandEyeSolverDiagnostics(result, q1_list, q2_list, sample_timestamps=sample_timestamps)
     logger.debug(f"Ran solver for relative rotation: Solution q_12={q_12}, "
             f"Solution uncertainty: {np.rad2deg(diagnostics.sol_angle_error):.2f} degrees, "
             f"Func evaluations: {result.nfev}, Cost = {result.cost:.4g}, "
@@ -148,6 +166,7 @@ def solve_rotation_with_outlier_removal(
         q1_list: list_of_quats,  # List of rotation quaternions
         q2_list: list_of_quats,
         x0: Union[np.ndarray, list] = np.zeros(N_UNKNOWN_PARAMS),  # Initial guess
+        sample_timestamps: Union[list_of_float_pairs, None] = None,
         mad_threshold = 4.45,  # Reject outlier above this multiple of MAD in first pass
         n_min_samples = N_UNKNOWN_PARAMS,  # Minimum number of sample pairs for a solution
         ):
@@ -156,7 +175,7 @@ def solve_rotation_with_outlier_removal(
     solve_rotation() for details).
     """
     # First pass:
-    q12_solution, diagnostics = solve_rotation(q1_list, q2_list, x0)
+    q12_solution, diagnostics = solve_rotation(q1_list, q2_list, x0, sample_timestamps)
     if q12_solution is None:
         logger.debug("First-pass solve for imu/camera alignment failed to converge.")
         return None, diagnostics
@@ -171,6 +190,7 @@ def solve_rotation_with_outlier_removal(
     msk_accept = diagnostics.residual_norms < (median + mad * mad_threshold)
     if np.all(msk_accept):
         logger.debug("No outliers. Returning solution from first-pass.")
+        return q12_solution, diagnostics
     else:
         logger.debug("Running 2nd-pass solve for imu/camera alignment using "
             f"{np.sum(msk_accept)}/{diagnostics.residual_norms.shape[0]} samples.")
@@ -180,11 +200,15 @@ def solve_rotation_with_outlier_removal(
                     "Not enough samples for outlier removal. Returning solution from first-pass.")
             return q12_solution, diagnostics
 
-        # Solve again, using previous solution as the initial guess
-        q1_accept = [q for ii, q in enumerate(q1_list) if msk_accept[ii]]
-        q2_accept = [q for ii, q in enumerate(q2_list) if msk_accept[ii]]
+        # Remove outliers
+        q1_accepted = [q for ii, q in enumerate(q1_list) if msk_accept[ii]]
+        q2_accepted = [q for ii, q in enumerate(q2_list) if msk_accept[ii]]
+        timestamps_accepted = [t for ii, t in enumerate(sample_timestamps) if msk_accept[ii]]
+
+        # Solve again after outlier removal, using previous solution as the initial guess
         x0 = quaternion.as_rotation_vector(q12_solution)
-        q12_solution, diagnostics = solve_rotation(q1_accept, q2_accept, x0)
+        q12_solution, diagnostics = solve_rotation(
+            q1_accepted, q2_accepted, x0, timestamps_accepted)
 
     return q12_solution, diagnostics
 
diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index 8f01a4be9..158181ef6 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -306,16 +306,16 @@ def solve(self, n_pairs=None):
         # and this will solve the relative rotation between them. 
         dq_cam_list = []
         dq_imu_list = []
-        dt_list = []
+        sample_timestamps = []
         for samp1, samp2 in self.pair_buffer.buffer:
             dq_cam_list.append(samp1.q_cam.conj() * samp2.q_cam)
             dq_imu_list.append(samp1.q_imu.conj() * samp2.q_imu)
-            dt_list.append(samp2.timestamp - samp1.timestamp)
+            sample_timestamps.append((samp1.timestamp, samp2.timestamp))
 
         # Solve
         #q_cam2imu, diagnostics = solve_rotation(dq_cam_list, dq_imu_list)
-        q_cam2imu, diagnostics = solve_rotation_with_outlier_removal(dq_cam_list, dq_imu_list)
-        diagnostics.meta_data["time_differences"] = dt_list
+        q_cam2imu, diagnostics = solve_rotation_with_outlier_removal(
+            dq_cam_list, dq_imu_list, sample_timestamps=sample_timestamps)
         return q_cam2imu, diagnostics
 
     def add_candidate_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):

From 37458d7183bf1f2b94ee4cf8e83598bcef41b5c5 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Wed, 19 Aug 2026 17:36:35 +0200
Subject: [PATCH 27/45] Save diagnostics from first pass

---
 .../PiFinder/imu/imu_align/hand_eye_solver.py | 44 ++++++++++---------
 1 file changed, 24 insertions(+), 20 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index 03968fc33..312143e21 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -183,6 +183,7 @@ def solve_rotation_with_outlier_removal(
         return q12_solution, diagnostics
 
     # Second pass: Re-run least-squares with outliers removed
+    # Detect outliers above MAD threshold
     median = np.median(diagnostics.residual_norms)
     mad = np.median(np.abs(diagnostics.residual_norms - median))
     logger.debug(f"MAD after first-pass: {np.rad2deg(mad):.2f} degrees. "
@@ -191,26 +192,29 @@ def solve_rotation_with_outlier_removal(
     if np.all(msk_accept):
         logger.debug("No outliers. Returning solution from first-pass.")
         return q12_solution, diagnostics
-    else:
-        logger.debug("Running 2nd-pass solve for imu/camera alignment using "
-            f"{np.sum(msk_accept)}/{diagnostics.residual_norms.shape[0]} samples.")
-
-        if np.sum(msk_accept) < n_min_samples:
-            np.info(f"Less than {n_min_samples} samples remain. "
-                    "Not enough samples for outlier removal. Returning solution from first-pass.")
-            return q12_solution, diagnostics
-
-        # Remove outliers
-        q1_accepted = [q for ii, q in enumerate(q1_list) if msk_accept[ii]]
-        q2_accepted = [q for ii, q in enumerate(q2_list) if msk_accept[ii]]
-        timestamps_accepted = [t for ii, t in enumerate(sample_timestamps) if msk_accept[ii]]
-
-        # Solve again after outlier removal, using previous solution as the initial guess
-        x0 = quaternion.as_rotation_vector(q12_solution)
-        q12_solution, diagnostics = solve_rotation(
-            q1_accepted, q2_accepted, x0, timestamps_accepted)
-
-    return q12_solution, diagnostics
+    
+    logger.debug("Outlier removal. Keeping"
+        f"{np.sum(msk_accept)}/{diagnostics.residual_norms.shape[0]} samples.")
+
+    if np.sum(msk_accept) < n_min_samples:
+        np.info(f"Less than {n_min_samples} samples after outlier removal. "
+                "Not enough samples for second pass. Returning solution from first-pass.")
+        return q12_solution, diagnostics
+
+    # Remove outliers
+    q1_accepted = [q for ii, q in enumerate(q1_list) if msk_accept[ii]]
+    q2_accepted = [q for ii, q in enumerate(q2_list) if msk_accept[ii]]
+    timestamps_accepted = [t for ii, t in enumerate(sample_timestamps) if msk_accept[ii]]
+
+    # Solve again after outlier removal, using previous solution as the initial guess
+    x0 = quaternion.as_rotation_vector(q12_solution)
+    q12_solution_new, diagnostics_new = solve_rotation(
+        q1_accepted, q2_accepted, x0, timestamps_accepted)
+    # Store solutions and diagnostics from first pass
+    diagnostics_new.meta_data['first_pass_solution'] = q12_solution
+    diagnostics_new.meta_data['first_pass_diagnostics'] = diagnostics
+
+    return q12_solution_new, diagnostics_new
 
 
 def _solution_diagnostics(result):

From 4e8828642b31f9d5c34d7651552c18c00f9c808e Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Wed, 19 Aug 2026 20:55:46 +0200
Subject: [PATCH 28/45] Use SD as an additional metric for outlier detection

---
 python/PiFinder/imu/imu_align/hand_eye_solver.py | 11 ++++++++---
 python/PiFinder/imu/imu_align/imu_alignment.py   |  9 +++++++--
 2 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index 312143e21..fd8c4e38d 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -186,9 +186,14 @@ def solve_rotation_with_outlier_removal(
     # Detect outliers above MAD threshold
     median = np.median(diagnostics.residual_norms)
     mad = np.median(np.abs(diagnostics.residual_norms - median))
-    logger.debug(f"MAD after first-pass: {np.rad2deg(mad):.2f} degrees. "
-                 f"Solution uncertainty: {np.rad2deg(diagnostics.sol_angle_error):.2f} degrees.")
-    msk_accept = diagnostics.residual_norms < (median + mad * mad_threshold)
+    mean = np.mean(diagnostics.residual_norms)
+    sd = np.std(diagnostics.residual_norms)
+    logger.debug("After first pass: "
+        f"Solution uncertainty: {np.rad2deg(diagnostics.sol_angle_error):.2f} degrees "
+        f"MAD: {np.rad2deg(mad):.2f} degrees SD: {np.rad2deg(sd):.2f} degrees.")
+                 
+    msk_accept = np.logical_and(diagnostics.residual_norms < (median + mad_threshold * mad),
+                                diagnostics.residual_norms < (mean + 3 * sd))
     if np.all(msk_accept):
         logger.debug("No outliers. Returning solution from first-pass.")
         return q12_solution, diagnostics
diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index 158181ef6..019d2bb4b 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -73,10 +73,12 @@
    mount, the rotation will likely be around two axes. This may result in a 
    larger uncertainty for the rotation/alignment about some axes.
 """
+from dataclasses import dataclass
+
 import logging
 import numpy as np
 import quaternion
-from dataclasses import dataclass
+import time
 
 from PiFinder.types.coordinates import RaDecRoll
 from PiFinder.pointing_model import quaternion_transforms as qt
@@ -326,7 +328,7 @@ def add_candidate_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2i
         self.add_candidate(timestamp, cam_eq, q_x2imu)
 
         # Pair samples: Runs periodically
-        if ((self._samples_since_last_pair_attempt >= self.min_n_solve) or
+        if ((self._samples_since_last_pair_attempt >= self.min_n_solve) or  # TODO: Tune!
             (self.candidate_buffer.len >= self.candidate_buffer.max_buffer_length)):
             #self.purge_old_samples(timestamp)  # TODO: Run less frequently
             #self.purge_old_candidates()  # TODO: Run less frequently
@@ -345,7 +347,10 @@ def add_candidate_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2i
 
         # Solve if there are enough samples
         if self.pair_buffer.len >= self.min_n_solve:
+            t_start = time.time()
             q_cam2imu, diagnostics = self.solve()
+            diagnostics.meta_data["total_solve_time"] = time.time() - t_start
+
             self.pair_buffer.reset_buffer()  # Flush the values used for solve
             return q_cam2imu, diagnostics
         else:

From fb4e81effcc125cf9ff8a0d23f91e5be5c26547f Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Wed, 19 Aug 2026 21:18:34 +0200
Subject: [PATCH 29/45] Clean up, rename

---
 .../PiFinder/imu/imu_align/hand_eye_solver.py |   9 --
 .../PiFinder/imu/imu_align/imu_alignment.py   | 101 +++++++++---------
 2 files changed, 51 insertions(+), 59 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index fd8c4e38d..d6d7d953e 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -279,15 +279,6 @@ def calculate_relative_rotations(q1_list: list_of_quats, q2_list: list_of_quats)
     return [q1.conjugate() * q2 for q1, q2 in zip(q1_list, q2_list)]
 
 
-def reject_small_rotations(dq_list: list_of_quats,
-                           min_rotation=np.deg2rad(1.0),  # Reject rotations below this [radians]
-                           ):
-    """
-    Reject small rotations
-    """
-    pass
-
-
 # ------ Simulation functions for testing & analysis --------------------------
 
 def _q_noise(noise_amp: float):
diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index 019d2bb4b..c658b8ed9 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -159,8 +159,9 @@ def __init__(self, candidate_buffer_length=60, min_n_solve=10,
         :param min_angle_diff: [rad] Minimum allowed angle difference between pairs of samples
         :param max_age: [s] Remove samples older than this. None to ignore
         """
-        self.candidate_buffer = SampleBuffer(max_buffer_length=candidate_buffer_length)
-        diff_buffer_length = candidate_buffer_length  # TODO: Come up with a better value
+        self.candidate_buffer = SampleBuffer(
+            max_buffer_length=max(candidate_buffer_length, min_n_solve))
+        diff_buffer_length = candidate_buffer_length
         self.pair_buffer = SampleBuffer(max_buffer_length=diff_buffer_length)
 
         self.min_n_solve = min_n_solve
@@ -170,15 +171,52 @@ def __init__(self, candidate_buffer_length=60, min_n_solve=10,
 
         self._samples_since_last_pair_attempt = 0
 
-    def reset_buffers(self):
+    def add_candidate_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
+        """
+        For general use, call this pipeline method. Add a new candidate to the
+        buffer. When the buffer fills up, pair samples and solve.
+        """
+        self._add_candidate(timestamp, cam_eq, q_x2imu)
+
+        # Pair samples: Runs periodically
+        if ((self._samples_since_last_pair_attempt >= self.min_n_solve) or
+            (self.candidate_buffer.len >= self.candidate_buffer.max_buffer_length)):
+            self._purge_old_samples(timestamp)
+            self._purge_old_candidates()
+            self._pair_samples()
+
+            # If the candidate buffer is still full after pairing, remove a 
+            # batch of the older samples from the buffer
+            if self.candidate_buffer.len >= self.candidate_buffer.max_buffer_length:
+                remove_set = set(range(self.min_n_solve))
+                self.candidate_buffer.remove_samples(remove_set)
+            
+            self._samples_since_last_pair_attempt = 0
+        else:
+            self._samples_since_last_pair_attempt += 1
+
+        # Solve if there are enough samples
+        if self.pair_buffer.len >= self.min_n_solve:
+            t_start = time.time()
+            q_cam2imu, diagnostics = self._solve()
+            diagnostics.meta_data["total_solve_time"] = time.time() - t_start
+
+            self.pair_buffer.reset_buffer()  # Flush the values used for solve
+            self.candidate_buffer.trim_to_max_length()
+
+            return q_cam2imu, diagnostics
+        else:
+            return None, None
+
+    def _reset_buffers(self):
         self.candidate_buffer.reset_buffer()
         self.pair_buffer.reset_buffer()
 
-    def trim_buffers(self):
+    def _trim_buffers(self):
         self.candidate_buffer.trim_to_max_length()
         self.pair_buffer.trim_to_max_length()
 
-    def add_candidate(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
+    def _add_candidate(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
         """
         Add to the candidate_buffer the camera solve & corresponding IMU sample
         from integrator.
@@ -196,17 +234,16 @@ def add_candidate(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion
             q_imu = qt.ensure_quat_continuity(last_candidate.q_imu, q_x2imu)
             self.candidate_buffer.add_sample(CameraImuSample(timestamp, q_cam, q_imu))
 
-    def purge_old_samples(self, current_time: float):
+    def _purge_old_samples(self, ref_time: float):
         """
-        Remove samples from the candidate_buffer that are older than the current
-        time.
-
-        This should be run on a schedule every self.max_age [s].
+        Remove samples from the candidate_buffer that are older than max_age
+        relative to ref_time. This should be run on a schedule every
+        self.max_age [s].
         """
         if self.max_age is None:
             return
 
-        allowed_timestamp = current_time - self.max_age  # Purge anything older than this
+        allowed_timestamp = ref_time - self.max_age  # Purge anything older than this
         
         # Purge candidate_buffer:
         remove_idx_list = [i for i, samp in enumerate(self.candidate_buffer.buffer) 
@@ -221,7 +258,7 @@ def purge_old_samples(self, current_time: float):
         if remove_idx_list:
             self.pair_buffer.remove_samples(set(remove_idx_list))
 
-    def purge_old_candidates(self):
+    def _purge_old_candidates(self):
         """
         Remove samples from candidate_buffer that are older than
         self.max_time_diff from other samples in buffer because these will be
@@ -242,7 +279,7 @@ def purge_old_candidates(self):
         if remove_ids:
             self.candidate_buffer.remove_samples(remove_ids)
 
-    def pair_samples(self) -> int:
+    def _pair_samples(self) -> int:
         """
         Go through the candidate_buffer from the first sample in the buffer.
         Pair two sets of camera/IMU samples from the candidate buffer that meet
@@ -293,7 +330,7 @@ def pair_samples(self) -> int:
 
         return n_pairs  # Number of successful pairings
 
-    def solve(self, n_pairs=None):
+    def _solve(self, n_pairs=None):
         """
         Solve for the alignment between the camera and IMU using at least the
         last n_pairs or all available pairs (if None) in diff_buffer.
@@ -319,39 +356,3 @@ def solve(self, n_pairs=None):
         q_cam2imu, diagnostics = solve_rotation_with_outlier_removal(
             dq_cam_list, dq_imu_list, sample_timestamps=sample_timestamps)
         return q_cam2imu, diagnostics
-
-    def add_candidate_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
-        """
-        For general use, call this pipeline method. Add a new candidate to the
-        buffer. When the buffer fills up, pair samples and solve.
-        """
-        self.add_candidate(timestamp, cam_eq, q_x2imu)
-
-        # Pair samples: Runs periodically
-        if ((self._samples_since_last_pair_attempt >= self.min_n_solve) or  # TODO: Tune!
-            (self.candidate_buffer.len >= self.candidate_buffer.max_buffer_length)):
-            #self.purge_old_samples(timestamp)  # TODO: Run less frequently
-            #self.purge_old_candidates()  # TODO: Run less frequently
-            self.pair_samples()
-            #self.trim_buffers()  # TODO
-
-            # If the candidate buffer is still full after pairing, remove a 
-            # batch of the older samples from the buffer
-            if self.candidate_buffer.len >= self.candidate_buffer.max_buffer_length:
-                remove_set = set(range(self.min_n_solve))
-                self.candidate_buffer.remove_samples(remove_set)
-            
-            self._samples_since_last_pair_attempt = 0
-        else:
-            self._samples_since_last_pair_attempt += 1
-
-        # Solve if there are enough samples
-        if self.pair_buffer.len >= self.min_n_solve:
-            t_start = time.time()
-            q_cam2imu, diagnostics = self.solve()
-            diagnostics.meta_data["total_solve_time"] = time.time() - t_start
-
-            self.pair_buffer.reset_buffer()  # Flush the values used for solve
-            return q_cam2imu, diagnostics
-        else:
-            return None, None

From e513fee1fc41782e3a1604c394a68eb9378a3ca9 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Wed, 19 Aug 2026 22:13:09 +0200
Subject: [PATCH 30/45] Refactor. Assign default values to ImuCameraAlignment
 that works well

---
 python/PiFinder/imu/imu_align/hand_eye_solver.py | 10 +++++-----
 python/PiFinder/imu/imu_align/imu_alignment.py   |  8 ++++++--
 2 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index d6d7d953e..f29248196 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -33,7 +33,7 @@ class HandEyeSolverDiagnostics:
     residual_norms: np.ndarray  # Residual norms of each sample [rad]
     rotation_angles: np.ndarray  # Rotation angles of each sample [rad]
     sol_cov_matrix: np.ndarray  # Solution covariance matrix
-    sol_angle_error: np.ndarray  # Solution angle error
+    sol_angle_error: np.ndarray  # Solution angle error [rad]
 
     # Optional
     meta_data: dict
@@ -61,10 +61,10 @@ def __init__(self, lsq_result, q1_list: list_of_quats, q2_list: list_of_quats,
         # Calculate rotations of each sample [rad]
         self.rotation_angles = [qt.get_quat_angular_diff(q1, q2) for q1, q2 in zip(q1_list, q2_list)]
 
-        self.sol_cov_matrix, self.sol_angle_error =self.calculate_solution_uncertainty()
+        self.sol_cov_matrix, self.sol_angle_error =self._calculate_solution_uncertainty()
         self.meta_data = {}
 
-    def calculate_solution_uncertainty(self):
+    def _calculate_solution_uncertainty(self):
         """
         Calculate the standard error of the solution:
         Cov = sigma ** 2 * inv(J.T @ J)
@@ -79,10 +79,10 @@ def calculate_solution_uncertainty(self):
 
         # Calculate reduced Chi-square
         dof = m_meas - n_sol  # Degrees of freedom
-        rss = 2 * self.lsq_result.cost  # res.cost is 0.5 * sum(residuals**2)
+        rss = 2 * self.lsq_result.cost  # Because cost = 0.5 * sum(residuals**2)
         chi_square = rss / dof
 
-        # Extract uncertainty 
+        # Estimate uncertainty about the solution
         sol_cov_matrix = chi_square * inv_JTJ
         sol_angle_error = np.sqrt(np.trace(sol_cov_matrix))  # [rad]
 
diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index c658b8ed9..280aa2491 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -148,8 +148,12 @@ class ImuCameraAlignment:
     min_angle_diff: float  # [rad] Pair samples with large enough angle difference
     max_age: float  # [s] Maximum age of sample compared to current time
 
-    def __init__(self, candidate_buffer_length=60, min_n_solve=10, 
-                 max_time_diff=2.0, min_angle_diff=np.deg2rad(5), max_age=1200):
+    def __init__(self, 
+                 candidate_buffer_length: int = 60, 
+                 min_n_solve: int = 20, 
+                 max_time_diff: float = 20.0, 
+                 min_angle_diff: float = np.deg2rad(5.0), 
+                 max_age: float = 600.0):
         """
         candidate_buffer_length: Should be around sample_freq * max_time_diff
 

From 3e1888a02d12f4ab6328a38b968a5e1b52ded090 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Wed, 19 Aug 2026 22:13:35 +0200
Subject: [PATCH 31/45] Integrate IMU/Camera alignement into integrator.py. Not
 tested yet!

---
 python/PiFinder/integrator.py | 30 ++++++++++++++++++++++++++++++
 1 file changed, 30 insertions(+)

diff --git a/python/PiFinder/integrator.py b/python/PiFinder/integrator.py
index 6e0e5b350..4bb3bc850 100644
--- a/python/PiFinder/integrator.py
+++ b/python/PiFinder/integrator.py
@@ -68,6 +68,7 @@
     SolveSource,
     SuccessfulSolve,
 )
+from PiFinder.imu.imu_align import ImuCameraAlignment
 
 logger = logging.getLogger("IMU.Integrator")
 
@@ -229,6 +230,15 @@ def integrator(
 
         lens_self_heal = LensSelfHeal(cfg, shared_state)
 
+        # ---------- TESTING ------------
+        # Initialize continual IMU/Camera alignment
+        # TODO: Move to a different location
+        logger.info("IMU/Camera alignment: Initialized with q_imu2cam: ", idr.q_imu2cam)
+        imu_align = ImuCameraAlignment(
+            candidate_buffer_length=60, min_n_solve=20, 
+            max_time_diff=20.0, min_angle_diff=np.deg2rad(5.0), max_age=600.0)
+        # -------------------------------------------------
+
         while True:
             state_utils.sleep_for_framerate(shared_state)
 
@@ -280,6 +290,26 @@ def integrator(
                 estimate = _apply_successful_solve(estimate, solve_result, idr)
                 pointing_updated = True
 
+                # --------------- TESTING ---------------
+                # Add IMU/Camera samples to the buffer and attempt solve if buffer is full
+                # TODO: Move to a different location
+                new_q_cam2imu, _diag = imu_align.add_candidate_attempt_solve(
+                    solve_result.last_solve_success, 
+                    solve_result.camera.as_radecroll(), 
+                    solve_result.imu_anchor)
+                if new_q_cam2imu is not None:
+                    angular_diff = qt.get_quat_angular_diff(idr.q_imu2cam, new_q_cam2imu)
+                    logger.info("IMU/Camera alignment: New estimate q_imu2cam: ", new_q_cam2imu)
+                    logger.info("IMU/Camera alignment: Angular difference from previous estimate: "
+                                f"{np.rad2deg(angular_diff):.2f} deg | "
+                                "Solution uncertainty: "
+                                f"{np.rad2deg(_diag.sol_angle_error):.2f} deg | "
+                                f"Solve time: {_diag.meta_data['total_solve_time']}")
+                    # Update:
+                    idr.q_imu2cam = new_q_cam2imu
+                # ---------------------------------------
+
+
                 # Append plate-solve and IMU states to IMU/camera alignment buffer
                 # TODO: Append the following:
                 # solve_result.last_solve_success (timestamp)

From df284da226249a0aaad7dec4c80654c3d76d202b Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Wed, 19 Aug 2026 22:17:48 +0200
Subject: [PATCH 32/45] Add commentary

---
 python/PiFinder/imu/imu_align/hand_eye_solver.py | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index f29248196..4d55fb338 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -78,6 +78,9 @@ def _calculate_solution_uncertainty(self):
         inv_JTJ, _, _, _ = np.linalg.lstsq(J.T @ J, np.eye(n_sol), rcond=None)
 
         # Calculate reduced Chi-square
+        # NOTE: Probably underestimates the reduced Chi-square because the DoF
+        # if overestimated. This is because the measurements are derived from
+        # multi-way pairs of samples and the samples are re-used.
         dof = m_meas - n_sol  # Degrees of freedom
         rss = 2 * self.lsq_result.cost  # Because cost = 0.5 * sum(residuals**2)
         chi_square = rss / dof

From 71a501a55ec8bce78877c004096aa8b8bba30d95 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Fri, 21 Aug 2026 23:47:57 +0200
Subject: [PATCH 33/45] Estimate the DoF properly accounting for connected
 samples. Uncertainty estimates are more realistic now.

---
 .../PiFinder/imu/imu_align/hand_eye_solver.py | 49 ++++++++++++++++---
 1 file changed, 42 insertions(+), 7 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index 4d55fb338..e4c0f6ba4 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -12,6 +12,8 @@
 import numpy as np
 import quaternion  # Note: numpy-quaternion convention: quaternion(w, x, y, z)
 from scipy.optimize import least_squares, OptimizeResult
+from scipy.sparse import csr_matrix
+from scipy.sparse.csgraph import connected_components
 import time
 from typing import Union
 
@@ -61,10 +63,12 @@ def __init__(self, lsq_result, q1_list: list_of_quats, q2_list: list_of_quats,
         # Calculate rotations of each sample [rad]
         self.rotation_angles = [qt.get_quat_angular_diff(q1, q2) for q1, q2 in zip(q1_list, q2_list)]
 
-        self.sol_cov_matrix, self.sol_angle_error =self._calculate_solution_uncertainty()
+        self.sol_cov_matrix, self.sol_angle_error =self._calculate_solution_uncertainty(sample_timestamps)
         self.meta_data = {}
 
-    def _calculate_solution_uncertainty(self):
+
+    def _calculate_solution_uncertainty(
+            self, sample_timestamps: Union[list_of_float_pairs, None]) -> tuple[np.ndarray, float]:
         """
         Calculate the standard error of the solution:
         Cov = sigma ** 2 * inv(J.T @ J)
@@ -77,11 +81,14 @@ def _calculate_solution_uncertainty(self):
         # NOTE: Could be speeded up using QR decomposition but this is good enough
         inv_JTJ, _, _, _ = np.linalg.lstsq(J.T @ J, np.eye(n_sol), rcond=None)
 
+        # Calculate degrees of freedom of the problem
+        if sample_timestamps is None:
+            dof = N_UNKNOWN_PARAMS * (m_meas - n_sol)  # Overestimates the DoF!
+        else:
+            n_connected_components, n_nodes = self._calculate_connected_components(sample_timestamps)
+            dof = N_UNKNOWN_PARAMS * (n_nodes - n_connected_components - 1)  # Degrees of freedom
+
         # Calculate reduced Chi-square
-        # NOTE: Probably underestimates the reduced Chi-square because the DoF
-        # if overestimated. This is because the measurements are derived from
-        # multi-way pairs of samples and the samples are re-used.
-        dof = m_meas - n_sol  # Degrees of freedom
         rss = 2 * self.lsq_result.cost  # Because cost = 0.5 * sum(residuals**2)
         chi_square = rss / dof
 
@@ -92,6 +99,34 @@ def _calculate_solution_uncertainty(self):
         return sol_cov_matrix, sol_angle_error
 
 
+    @staticmethod
+    def _calculate_connected_components(sample_timestamps: list_of_float_pairs) -> int:
+        """
+        Returns the number of connected components in the sample_timestamps
+        measurements. For example, if we have 5 measurement pairs (edges) from
+        7 unique samples (nodes):
+
+        [(0, 1), (0, 2), (3, 4), (4, 5), (5, 7)]
+
+        There are 3 connected components: [(0, 1, 2), (3, 4, 5), (7,)]
+        """
+        # Flattened along the rows & convert from float timestamps to pairs of indices (0..N)
+        _, inverse_idx = np.unique(np.array(sample_timestamps), return_inverse=True)
+        n_nodes = np.max(inverse_idx) + 1  # Number of unique samples
+        idx_pairs = inverse_idx.reshape(-1, 2)
+
+        # Matrix A has a 1 in row/column pairs
+        rows, cols = idx_pairs.T
+        A = csr_matrix(
+            (np.ones(2 * len(idx_pairs)),
+            (np.r_[rows, cols], np.r_[cols, rows])),
+            shape=(n_nodes, n_nodes)
+        )
+
+        # Return the number of connected components
+        return connected_components(A, directed=False, return_labels=False), n_nodes
+
+
 def residual_rotation_vector(x,  # (3,) Trial solution (q as rotation vector) 
                              q1_list: list_of_quats,  # List of rotation quaternions
                              q2_list: list_of_quats 
@@ -201,7 +236,7 @@ def solve_rotation_with_outlier_removal(
         logger.debug("No outliers. Returning solution from first-pass.")
         return q12_solution, diagnostics
     
-    logger.debug("Outlier removal. Keeping"
+    logger.debug("Outlier removal. Keeping "
         f"{np.sum(msk_accept)}/{diagnostics.residual_norms.shape[0]} samples.")
 
     if np.sum(msk_accept) < n_min_samples:

From 66910835f66892bf3bd008754e20776e9a380017 Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sat, 22 Aug 2026 00:19:57 +0200
Subject: [PATCH 34/45] Fix import

---
 python/PiFinder/integrator.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/python/PiFinder/integrator.py b/python/PiFinder/integrator.py
index 4bb3bc850..e00ddcd51 100644
--- a/python/PiFinder/integrator.py
+++ b/python/PiFinder/integrator.py
@@ -68,7 +68,7 @@
     SolveSource,
     SuccessfulSolve,
 )
-from PiFinder.imu.imu_align import ImuCameraAlignment
+from PiFinder.imu.imu_align.imu_alignment import ImuCameraAlignment
 
 logger = logging.getLogger("IMU.Integrator")
 

From 1803365dc61c92a0191d6aaaa150d270d6375dbf Mon Sep 17 00:00:00 2001
From: TakKanekoGit <>
Date: Fri, 21 Aug 2026 23:42:10 +0100
Subject: [PATCH 35/45] Fix  incorrect change in coordinates.py

---
 python/PiFinder/types/coordinates.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/python/PiFinder/types/coordinates.py b/python/PiFinder/types/coordinates.py
index 62c9ecdfe..0a6ca23b3 100644
--- a/python/PiFinder/types/coordinates.py
+++ b/python/PiFinder/types/coordinates.py
@@ -32,7 +32,7 @@ def __init__(self, ra: float, dec: float, roll: float, deg=False):
     @classmethod
     def from_quaternion(cls, q_eq: quaternion.quaternion):
         ra, dec, roll = q_eq2radec(q_eq)
-        return cls(ra=ra, dec=dec, roll=roll, valid=True)
+        return cls(ra=ra, dec=dec, roll=roll)
 
     def reset(self):
         """Reset to unset state"""

From 1f0f67e0a70bdbe402294c4e9ac362921f3ccd42 Mon Sep 17 00:00:00 2001
From: TakKanekoGit <>
Date: Sat, 22 Aug 2026 00:02:00 +0100
Subject: [PATCH 36/45] Move main block to tests

---
 .../PiFinder/imu/imu_align/hand_eye_solver.py | 48 ++-------------
 python/tests/test_imu_align.py                | 60 +++++++++++++++++++
 2 files changed, 64 insertions(+), 44 deletions(-)
 create mode 100644 python/tests/test_imu_align.py

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index e4c0f6ba4..5ef33f879 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -205,7 +205,7 @@ def solve_rotation_with_outlier_removal(
         q2_list: list_of_quats,
         x0: Union[np.ndarray, list] = np.zeros(N_UNKNOWN_PARAMS),  # Initial guess
         sample_timestamps: Union[list_of_float_pairs, None] = None,
-        mad_threshold = 4.45,  # Reject outlier above this multiple of MAD in first pass
+        mad_threshold = 4.45,  # TODO: Remove? Reject outlier above this multiple of MAD in first pass
         n_min_samples = N_UNKNOWN_PARAMS,  # Minimum number of sample pairs for a solution
         ):
     """
@@ -220,7 +220,7 @@ def solve_rotation_with_outlier_removal(
     if mad_threshold is None:
         return q12_solution, diagnostics
 
-    # Second pass: Re-run least-squares with outliers removed
+    # Second pass:
     # Detect outliers above MAD threshold
     median = np.median(diagnostics.residual_norms)
     mad = np.median(np.abs(diagnostics.residual_norms - median))
@@ -266,6 +266,8 @@ def _solution_diagnostics(result):
     `result` is the output from scipy.optimize.least_squares.
     
     Condition number: < 10 excellent, < 100 acceptable, <1E4 weak observability
+
+    TODO: Remove?
     """
     t_start = time.time()
 
@@ -385,45 +387,3 @@ def simulate_quaternion_measurements(
     q2 = _add_noise_to_quaternion_list(q2_true, q2_noise_amp)
 
     return q1, q2
-
-
-if __name__ == "__main__":
-    """ 
-    The main block simulates pairs of q1 and q2 measurements and solves
-    for the q_12 for the quaternion form of the hand-eye problem:
-    
-    q1 * q_12 = q_12 * q2
-    """
-
-    # Set the true camera-from-body rotation
-    true_rotvec = np.radians([10, -5, 20])
-    q_12_true = quaternion.from_rotation_vector(true_rotvec)
-
-    # Simulate measurements:
-    q1, q2 = simulate_quaternion_measurements(
-        q_12_true, N=100, camera_noise_amp=np.deg2rad(0.1), 
-        imu_noise_amp=np.deg2rad(0.1), seed=0)
-
-    # Optional steps: 
-    # Pair up and calculate relative rotations
-    # Reject small rotations
-
-    # solve
-    q_12_est, diagnostics = solve_rotation(
-        q1, q2, residual_threshold = 0.01, verbose=True)
-
-    # Results
-    print("\nTrue q_12:")
-    print(quaternion.as_float_array(q_12_true))
-
-    print("\nEstimated q_12_est:")
-    print(quaternion.as_float_array(q_12_est))
-
-    # Error
-    q_error = q_12_est.conjugate() * q_12_true
-    error_deg = np.rad2deg(
-        np.linalg.norm(
-            quaternion.as_rotation_vector(q_error)
-        )
-    )
-    print(f"\nCalibration error: {error_deg:.6f} deg")
diff --git a/python/tests/test_imu_align.py b/python/tests/test_imu_align.py
new file mode 100644
index 000000000..ea22744c0
--- /dev/null
+++ b/python/tests/test_imu_align.py
@@ -0,0 +1,60 @@
+import pytest
+
+import numpy as np
+import quaternion  # Note: numpy-quaternion convention: quaternion(w, x, y, z)
+from PiFinder.imu.imu_align.hand_eye_solver import solve_rotation, simulate_quaternion_measurements
+
+
+def test_simulate_quaternion_measurements():
+    N = 100  # Number of samples to simulate    
+
+    # Set the true camera-from-body rotation
+    true_rotvec = np.radians([10, -5, 20])
+    q_12_true = quaternion.from_rotation_vector(true_rotvec)
+
+    # Simulate measurements:
+    q1, q2 = simulate_quaternion_measurements(
+        q_12_true, N=N, q1_noise_amp=np.deg2rad(0.1), 
+        q2_noise_amp=np.deg2rad(0.1), seed=0)
+    assert len(q1) == N
+    assert len(q2) == N
+
+
+def test_solve_rotation():
+    """ 
+    The main block simulates pairs of q1 and q2 measurements and solves
+    for the q_12 for the quaternion form of the hand-eye problem:
+
+    q1 * q_12 = q_12 * q2
+    """
+    # Set the true camera-from-body rotation
+    true_rotvec = np.radians([10, -5, 20])
+    q_12_true = quaternion.from_rotation_vector(true_rotvec)
+
+    # Simulate measurements:
+    q1, q2 = simulate_quaternion_measurements(
+        q_12_true, N=100, q1_noise_amp=np.deg2rad(0.1), 
+        q2_noise_amp=np.deg2rad(0.1), seed=0)
+
+    # Optional steps: 
+    # Pair up and calculate relative rotations
+    # Reject small rotations
+
+    # solve
+    q_12_est, diagnostics = solve_rotation(q1, q2)
+
+    # Results
+    print("\nTrue q_12:")
+    print(quaternion.as_float_array(q_12_true))
+
+    print("\nEstimated q_12_est:")
+    print(quaternion.as_float_array(q_12_est))
+
+    # Error
+    q_error = q_12_est.conjugate() * q_12_true
+    error_deg = np.rad2deg(
+        np.linalg.norm(
+            quaternion.as_rotation_vector(q_error)
+        )
+    )
+    #print(f"\nCalibration error: {error_deg:.6f} deg")

From b9fc235b79936e92432fd48186ffbb8e8deef633 Mon Sep 17 00:00:00 2001
From: Richard <rich@brickbots.com>
Date: Wed, 19 Aug 2026 14:10:16 -0700
Subject: [PATCH 37/45] Relabeling this as 2.6.3 to skip private 2.6.2 release

---
 release_notes/{2.6.2.md => 2.6.3.md} | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 rename release_notes/{2.6.2.md => 2.6.3.md} (100%)

diff --git a/release_notes/2.6.2.md b/release_notes/2.6.3.md
similarity index 100%
rename from release_notes/2.6.2.md
rename to release_notes/2.6.3.md

From 31090c489ecb350de1b7109ab237bdba28220eeb Mon Sep 17 00:00:00 2001
From: Richard <rich@brickbots.com>
Date: Wed, 19 Aug 2026 21:26:32 -0700
Subject: [PATCH 38/45] fix(optics): no FOV gate for frames from an unknown
 optical train (#632)

* docs(optics): name the unknown optical train, the FOV gate's third rung

`--camera debug` stopped solving for anyone with a stated lens. ADR 0027
relabelled the debug camera's sensor to `hq` so the archived 10.2 degree
frames would be covered, but the lens half still reads live from
`camera_lens`, and `resolve_lens` honours a statement whatever profile it
is handed. So the gate gets derived from hq x 16mm (17.12 deg) or hq x 12mm
(20.43 deg) -- trains that have never physically existed -- and every debug
frame falls outside the window. Measured against the real solver:

  hq + no stated lens  10.33 deg  [8.78, 11.88]   solves (fitted 10.20)
  hq + stated 25mm     10.33 deg  [8.78, 11.88]   solves
  hq + stated 16mm     17.12 deg  [14.55, 19.69]  no solve
  hq + stated 12mm     20.43 deg  [17.37, 23.49]  no solve

Same shape as the regression ADR 0029 exists to fix, mirrored: there an
assumption wore a statement's confidence, here a statement is made about
hardware that is not in the loop.

Names the state rather than special-casing the debug camera. An **unknown
optical train** is one whose frames did not come through this device's
optics, and it extends the confidence ladder 0029 already established:
stated -> +/-15%, assumed -> union over shipped lenses, unknown -> no gate
at all. Two consequences follow from one rule -- nothing about the device
may be asserted about the frames (no FOV hint), and nothing about the
device may be inferred from them (no lens self-heal).

The self-heal half is a live bug this records the fix for: with no
`camera_lens` in config, debug mode solves at 10.20 deg, self-heal matches
that to the hq's 25mm within 1.3%, and writes it into the developer's
config. Attach a real imx462 afterwards and the now-stated 25mm derives
6.4 deg, nothing solves, and self-heal cannot undo it -- it only ever
writes into an absence.

Docs only; the code change follows.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01CUbR3ryYDL7RizsNhPxGkX

* fix(solver): hand no FOV gate to frames from an unknown optical train

`--camera debug` stopped solving for anyone with a stated lens. This is
the code half of the decision recorded in the previous commit.

The debug camera declares the sensor its archived frames were shot on
(`hq`), but the lens half of the train is read live from `camera_lens`, so
stating one pairs `hq` with glass that is not in the loop: 16mm implies
17.12 deg and 12mm 20.43, against frames that are 10.2. tetra3 prunes by
implied field of view before verification, so every frame is rejected --
no solves at all, and the symptom presents as an exposure problem.

Rather than special-case the debug camera, name the state and let the two
consumers that must not act on it read one flag:

* `CameraInterface.optical_train_known()` defaults True, so a camera has
  to opt *out* and a new hardware backend inherits the gate rather than
  silently losing it. `CameraDebug` returns False.
* The camera process publishes it beside `camera_type`; `SharedStateObj`
  defaults it True so the boot window before the camera reports keeps its
  gate on real hardware.
* The solver omits `fov_estimate`/`fov_max_error` entirely -- not a third
  window to keep consistent, and any frame from any train solves, which
  is what lets a developer drop their own captures into test_images/.
* Lens self-heal declines to write. This half fixes a bug that has been
  live since 0029: a debug run on a config with no lens fits 10.20 deg,
  matches that to the hq's 25mm within 1.3%, and states it. Rich's own
  log has it firing on 2026-08-17. Attach a real imx462 afterwards and
  the now-stated 25mm derives 6.4 deg, nothing solves, and self-heal
  cannot undo it -- it only ever writes into an absence.

Verified end to end, not just in unit tests: launched headless with
`camera_lens: "16mm"` stated -- the exact config that produced zero
solves -- and it solves RA 296.37 Dec -1.70 in Aql, fitted FOV 10.20,
21 matches, `solve_source: CAM`, with the integrator logging that
self-heal is off and the config left unwritten.

test_optics_solving.py now parametrises over every lens a config can
name, asserting both halves: the derived gate rejects these frames for
16mm and 12mm, and no-gate solves them whatever is stated. That gap --
every existing case passed no lens key at all -- is why this shipped.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01CUbR3ryYDL7RizsNhPxGkX

---------

Co-authored-by: Claude Opus 5 (1M context) <noreply@anthropic.com>
---
 ...-fov-gate-width-follows-lens-confidence.md | 103 ++++++++++++++++++
 docs/ax/camera/CONTEXT.md                     |  27 ++++-
 python/PiFinder/camera_debug.py               |  25 ++++-
 python/PiFinder/camera_interface.py           |  28 +++++
 python/PiFinder/integrator.py                 |  24 +++-
 python/PiFinder/solver.py                     |  73 +++++++++----
 python/PiFinder/state.py                      |  17 +++
 python/tests/test_camera_interface.py         |  43 ++++++++
 python/tests/test_lens_self_heal.py           |  74 ++++++++++++-
 python/tests/test_optics.py                   |  10 +-
 python/tests/test_optics_solving.py           |  82 +++++++++++++-
 11 files changed, 475 insertions(+), 31 deletions(-)

diff --git a/docs/adr/0029-fov-gate-width-follows-lens-confidence.md b/docs/adr/0029-fov-gate-width-follows-lens-confidence.md
index 65d506526..a2ff5f6c3 100644
--- a/docs/adr/0029-fov-gate-width-follows-lens-confidence.md
+++ b/docs/adr/0029-fov-gate-width-follows-lens-confidence.md
@@ -200,3 +200,106 @@ One caveat, recorded honestly: 13.04 mm rests on a **single 12 mm sample**. The
 16 mm earned its 15.61 by reproducing two independently calibrated field
 widths on two different sensors. A second 12 mm — ideally on an imx296, so the
 sensor half varies too — would put it on the same footing.
+
+## Amendment: a third rung — no gate when the optical train is unknown
+
+The ladder above has two rungs, and both assume the frames came through the
+optics the device is configured with. `--camera debug` breaks that assumption:
+it replays archived frames, and the train those frames were shot through is
+not the train this machine is configured for. So the ladder gains a third
+rung — **stated → ±15%, assumed → union over shipped lenses, unknown → no gate
+at all** — and the confidence the width follows becomes confidence in the
+pairing rather than only in the lens.
+
+### The failure
+
+ADR 0027 relabelled the debug camera's sensor from `imx296` to `hq`, because
+the frames in `test_images/` are 10.2° and only the hq's train covers that.
+That fixed the sensor half and left the lens half reading live from
+`camera_lens`, which `resolve_lens` honours whatever profile it is handed. The
+result is a train that has never physically existed:
+
+| train | derived FOV | gate | debug frames |
+|---|---|---|---|
+| hq + no stated lens | 10.33° | [8.78, 11.88] | solve (fitted 10.20°) |
+| hq + stated `25mm` | 10.33° | [8.78, 11.88] | solve |
+| hq + stated `16mm` | 17.12° | [14.55, 19.69] | **no solve** |
+| hq + stated `12mm` | 20.43° | [17.37, 23.49] | **no solve** |
+
+Any developer who has ever opened the Lens menu, and every device whose lens
+this ADR's own self-heal has written, loses `--camera debug` entirely. It is
+the same shape as the regression this document exists to fix, mirrored: there
+an assumption wore a statement's confidence; here a statement is made about
+hardware that is not in the loop.
+
+`tests/test_optics_solving.py` did not catch it because every case calls
+`build_optical_train("hq")` with no lens key — only ever the assumed path.
+
+### Why no hint, rather than the alternatives
+
+**Declaring a lens from the camera** (debug publishes hq + 25 mm, config
+loses) keeps the gate and its mis-solve rejection, and was the first
+instinct. Rejected because it needs a lens statement that must never reach
+`camera_lens` — a second, differently-scoped writer for a key whose whole
+meaning is "the user's claim" — and because it pins debug mode to the one
+frame set we happen to ship. Dropping your own unsolvable field into
+`test_images/` is the main thing debug mode is *for*, and under a declared
+lens those frames are gated out exactly as the shipped ones are today.
+
+**Emulating the configured train** (serve frames matching the user's sensor
+and lens, so `-fh` behaves like their rev4) is the right answer to a different
+question and needs a real archived frame per shipped combination. The frames
+we have are 10.2°, too narrow to re-project outward from; this stays open if
+anyone wants it, and it wants captures, not code.
+
+**A database-range gate** of `[10, 30]°` is no-hint with extra steps — the
+database floor is already doing that pruning — and leaves a constant to
+maintain for no protection gained.
+
+**Rejecting non-shipped pairings** in `resolve_lens` (an hq never shipped with
+a 12 mm, so that statement is not credible) would fix debug as a side effect
+and close the sensor-swap deadlock too. Rejected here as blast radius: it
+makes a user's explicit statement conditionally non-authoritative, which is a
+direct contradiction of 0027, and it deserves its own decision rather than
+riding along with a development-tool fix.
+
+The measured cost of no-hint is small and known. On the shipped frames all
+three widths return identical RA/Dec, `Matches` and `Prob` within ~1 ms of
+each other. What no-hint gives up is the upper bound that rejected confident
+mis-solves in this document's noise trials — a protection whose value is that
+a device under the stars never reports a wrong pointing. Nothing is under the
+stars in debug mode.
+
+### Consequences
+
+**The gate is omitted, not widened.** `fov_estimate` is not passed at all when
+the train is unknown, so there is no third window to keep consistent with the
+other two, and any frame from any train solves.
+
+**Self-heal is barred under an unknown train**, and this is a live bug the
+amendment fixes rather than a hazard it introduces. With no `camera_lens` in
+config — the ordinary state of a development machine — `--camera debug` solves
+at 10.20°, `identify_lens_from_fitted_fov` matches that to the hq's `25mm`
+within 1.3%, and after three frames the integrator writes `camera_lens: 25mm`
+into the developer's config. It is not wrong about the *frames*; it is a
+statement about a device made from a recording. The trap springs later: attach
+a real imx462 and the now-stated 25 mm derives 6.4°, nothing solves, and
+self-heal cannot undo it because it only ever writes into an absence. A fitted
+FOV measures the train the frames passed through, so under an unknown train it
+measures nothing about this device and must not be learned from.
+
+**Only the gate and self-heal follow the unknown train.** The **frustum** keeps
+deriving from the configured optics, deliberately: it answers "what would my
+camera image", which is a question about the device and stays meaningful while
+a recording plays. Propagating would also delete a frustum that is *correct*
+on a dev laptop (10.33° derived against 10.20° frames) to avoid one that is
+wrong only when a lens is stated. SQM is unaffected either way — only
+`camera_pi` publishes radiometer samples, so the radiometric path is inert
+under the debug camera.
+
+**A stated lens still means no solves on real hardware.** Nothing here softens
+0027. The unknown train is a property of where the frames came from, not a
+new escape hatch for a wrong statement — which is why the sensor-swap
+deadlock (switch imx296 → hq from the Camera Type menu with a stated 16 mm,
+derive 17.12°, and be unable to measure out of it) is untouched by this and
+remains a documentation path (#613).
diff --git a/docs/ax/camera/CONTEXT.md b/docs/ax/camera/CONTEXT.md
index 85a2dce4e..421f096ec 100644
--- a/docs/ax/camera/CONTEXT.md
+++ b/docs/ax/camera/CONTEXT.md
@@ -123,7 +123,9 @@ the one field of view it implies, and nothing overrides it, so a wrong
 statement still means no solves (that is [ADR
 0027](../../adr/0027-fov-gate-derived-from-optical-train.md)'s deliberate
 consequence, not a regression). Written by the user from the Lens menu, or
-once by the device itself from a fitted FOV it is confident about.
+once by the device itself from a fitted FOV it is confident about — but never
+from a fitted FOV measured under an **unknown optical train**, which measures
+a recording rather than this device.
 _Avoid_: configured lens (true of a self-healed value too, so it does not
 distinguish), selected lens, user lens.
 
@@ -133,7 +135,9 @@ stated. Not a claim about the hardware — an admission that nobody has said,
 which is the ordinary condition of an install predating the setting. Because
 there is nothing to trust, the FOV gate widens to cover *every* lens that
 sensor has shipped with rather than centring on the fallback. An assumed lens
-is a temporary state: the first confident solve turns it into a stated one.
+is a temporary state: the first confident solve turns it into a stated one —
+unless the train is **unknown**, in which case no solve ever ends the
+assumption, because none of them measured this device.
 _Avoid_: default lens (reads as a preference rather than an absence of
 information), unset lens (the field of view is never unset — some lens is
 always assumed).
@@ -166,6 +170,25 @@ _Avoid_: optical configuration ("configuration" already names the physical
 build variants — see [Positioning](../positioning/CONTEXT.md) *screen
 direction*), camera setup, imaging train.
 
+**Unknown optical train**:
+The state of a camera whose frames did not come through this device's optics
+at all — today only the debug camera, which replays archived frames. The
+device's train still resolves to *something* (the debug camera declares the
+sensor its frames were shot on), but that train describes the machine, not
+the frames, so two things follow: nothing about the device may be **asserted**
+about the frames — the solver is handed no FOV gate rather than a derived one
+— and nothing about the device may be **inferred** from them, so a fitted FOV
+cannot promote an **assumed lens** to a **stated** one. This is the third rung
+of the confidence ladder the FOV gate's width follows: stated → ±15%, assumed
+→ the union over the sensor's shipped lenses, unknown → no gate at all (see
+[ADR 0029](../../adr/0029-fov-gate-width-follows-lens-confidence.md)).
+Deliberately *not* a blanket "everything derived goes dark": the **frustum**
+still answers what the configured camera would image, which is a question
+about the device and stays meaningful while a recording is being replayed.
+_Avoid_: debug FOV, no FOV (the frames have one — nobody here knows it),
+unknown lens (the lens resolves normally; it is the pairing that means
+nothing), fake camera.
+
 **Field of view**:
 The angular width of the **crop**, in degrees — the edge-to-edge extent, not
 the diagonal. Derived from the optical train. Every consumer that needs to
diff --git a/python/PiFinder/camera_debug.py b/python/PiFinder/camera_debug.py
index a4aa8faf1..73aa91d11 100644
--- a/python/PiFinder/camera_debug.py
+++ b/python/PiFinder/camera_debug.py
@@ -33,11 +33,14 @@ class CameraDebug(CameraInterface):
     def __init__(self, exposure_time) -> None:
         logger.debug("init camera debug")
         # Format matches PI cameras for compatibility. The sensor named here
-        # is not cosmetic: it half-determines the solver's FOV gate, and the
-        # frames in test_images/ are ~10.2 deg, which only the hq profile's
-        # train covers. Declaring imx296 centres the gate on 13.71 deg and
-        # every debug frame is rejected before verification -- no solves at
-        # all under `--camera debug`. See docs/adr/0027.
+        # is the one the frames in test_images/ were actually shot on, which
+        # is what makes SQM's profile lookup and the Lens menu resolve to
+        # something plausible. It is *not* a claim that this machine has an hq
+        # attached, and it does not settle the field of view on its own: the
+        # other half of the train comes from config, and a config that states
+        # a lens (16mm -> 17.12 deg, 12mm -> 20.43 deg) gates these ~10.2 deg
+        # frames straight out. Hence optical_train_known below. See
+        # docs/adr/0027 and the third-rung amendment to docs/adr/0029.
         self.camType = "Debug hq"
         self.path = utils.pifinder_dir / "test_images"
         self.exposure_time = exposure_time
@@ -106,6 +109,18 @@ def set_camera_config(
     def get_cam_type(self) -> str:
         return self.camType
 
+    def optical_train_known(self) -> bool:
+        """No. These frames are a recording, not a view through this device.
+
+        Whatever lens config states is a claim about glass that is not in the
+        loop, so pairing it with the sensor above produces a field of view
+        that describes nothing. Saying so is what keeps `--camera debug`
+        solving whatever the config happens to hold -- including frames a
+        developer drops into test_images/ from another train entirely, which
+        no derived gate could have anticipated.
+        """
+        return False
+
 
 def get_images(shared_state, camera_image, command_queue, console_queue, log_queue):
     """
diff --git a/python/PiFinder/camera_interface.py b/python/PiFinder/camera_interface.py
index a63d74183..8dd4f3c17 100644
--- a/python/PiFinder/camera_interface.py
+++ b/python/PiFinder/camera_interface.py
@@ -287,6 +287,22 @@ def set_camera_config(
     def get_cam_type(self) -> str:
         return "foo"
 
+    def optical_train_known(self) -> bool:
+        """Whether these frames came through this device's own optics.
+
+        True for anything pointed at the sky, which is why it defaults that
+        way: a camera has to opt *out*, so a new hardware backend inherits the
+        FOV gate rather than silently losing it.
+
+        False is an **unknown optical train** (docs/ax/camera/CONTEXT.md): the
+        frames were captured through some other train, so the resolved one
+        describes this machine and not them. Two things follow, both handled
+        by the consumers rather than here -- the solver asserts no FOV gate,
+        and lens self-heal declines to infer a lens from a fitted FOV that
+        measured a recording. See docs/adr/0029.
+        """
+        return True
+
     def start_camera(self) -> None:
         pass
 
@@ -308,6 +324,18 @@ def get_image_loop(
                 shared_state.set_camera_type(camera_type)
                 logger.info(f"Camera type set to: {camera_type}")
 
+            # Published beside the sensor because it qualifies it: the sensor
+            # a playback camera declares is the one its *frames* were shot on,
+            # which is not the same claim a live camera makes.
+            train_known = self.optical_train_known()
+            shared_state.set_optical_train_known(train_known)
+            if not train_known:
+                logger.info(
+                    "Optical train is unknown: these frames did not come "
+                    "through this device's optics, so the solver gets no FOV "
+                    "gate and the lens cannot self-heal from them"
+                )
+
             # Check if auto-exposure was previously enabled in config
             config_exp = cfg.get_option("camera_exp")
             if config_exp == "auto":
diff --git a/python/PiFinder/integrator.py b/python/PiFinder/integrator.py
index e00ddcd51..6ad7ed5f0 100644
--- a/python/PiFinder/integrator.py
+++ b/python/PiFinder/integrator.py
@@ -102,6 +102,14 @@ class LensSelfHeal:
       authoritative, so this only ever writes into an absence. That also means
       it writes at most once in the life of a device -- the write is what ends
       the condition it triggers on.
+    * **An unknown optical train never writes at all.** A fitted FOV measures
+      the train the frames passed through; under ``--camera debug`` that is
+      the train they were *recorded* on, so it says nothing about this
+      machine. Without this, a debug run on a config with no lens writes the
+      hq's ``25mm`` (the archived frames fit 10.20 deg, 1.3% off its derived
+      10.33) into that developer's config -- and a real imx462 attached
+      afterwards then derives 6.4 deg, solves nothing, and cannot heal,
+      because healing only writes into an absence that no longer exists.
     * **A fitted FOV matching no shipped lens writes nothing.** Third-party
       glass leaves the lens assumed and the gate wide, which is honest: we do
       not know its focal length, and a wrong write would be worse than none
@@ -116,8 +124,9 @@ def __init__(self, cfg, shared_state):
         self._shared_state = shared_state
         self._candidate: Optional[str] = None
         self._streak = 0
-        # Both latch to keep a per-frame condition from logging per frame.
+        # All three latch to keep a per-frame condition from logging per frame.
         self._logged_unidentified = False
+        self._logged_unknown_train = False
         self._disabled = False
 
     def observe(self, result: SuccessfulSolve) -> None:
@@ -133,6 +142,19 @@ def observe(self, result: SuccessfulSolve) -> None:
             self._disabled = True
 
     def _observe(self, result: SuccessfulSolve) -> None:
+        if not self._shared_state.optical_train_known():
+            # Checked before the lens, not after: this is not "nothing to
+            # heal" but "nothing here can heal anything", and the streak must
+            # not carry across into a later run on real optics.
+            if not self._logged_unknown_train:
+                self._logged_unknown_train = True
+                logger.info(
+                    "Optical train is unknown, so a fitted FOV measures the "
+                    "recording rather than this device; lens self-heal is off"
+                )
+            self._reset()
+            return
+
         lens_key = self._shared_state.camera_lens()
         if lens_is_stated(lens_key):
             # Nothing to heal -- and after a successful write this is the
diff --git a/python/PiFinder/solver.py b/python/PiFinder/solver.py
index 9ce0b060f..2e16f86d3 100644
--- a/python/PiFinder/solver.py
+++ b/python/PiFinder/solver.py
@@ -979,26 +979,49 @@ def solver(
                     train = optical_train.resolve(
                         shared_state.camera_type(), shared_state.camera_lens()
                     )
+                    # Read live for the same reason the train is: the camera
+                    # process publishes this after the solver is already
+                    # looping, so latching it here would miss it.
+                    train_known = shared_state.optical_train_known()
                 except (BrokenPipeError, ConnectionResetError) as e:
                     logger.error(f"Lost connection to shared state manager: {e}")
                     continue
-                if train is not logged_train:
-                    logged_train = train
-                    logger.info(
-                        # Say which of the two it is: under an assumed lens
-                        # the gate is wider than the stated field of view
-                        # implies, and a reader diagnosing "why did it solve
-                        # / not solve" needs to know the lens is a fallback
-                        # rather than something the device was told.
-                        "Optical train: %s %s lens on %s, field of view "
-                        "%.2f deg, FOV gate [%.2f, %.2f]",
-                        "stated" if train.lens_stated else "assumed",
-                        train.lens.menu_label,
-                        shared_state.camera_type(),
-                        train.fov_degrees,
-                        *_fov_gate_bounds(train),
-                    )
-                    _warn_if_outside_solver_database(t3, train)
+                if (train, train_known) != logged_train:
+                    logged_train = (train, train_known)
+                    if not train_known:
+                        # The resolved train is still worth printing -- it is
+                        # what SQM and the frustum are using -- but saying it
+                        # without saying it describes the device rather than
+                        # the frames is how somebody concludes the gate is
+                        # wrong when there is no gate.
+                        logger.info(
+                            "Optical train: %s %s lens on %s (%.2f deg), but "
+                            "these frames did not come through it -- solving "
+                            "with no FOV gate",
+                            "stated" if train.lens_stated else "assumed",
+                            train.lens.menu_label,
+                            shared_state.camera_type(),
+                            train.fov_degrees,
+                        )
+                    else:
+                        logger.info(
+                            # Say which of the two it is: under an assumed
+                            # lens the gate is wider than the stated field of
+                            # view implies, and a reader diagnosing "why did
+                            # it solve / not solve" needs to know the lens is
+                            # a fallback rather than something the device was
+                            # told.
+                            "Optical train: %s %s lens on %s, field of view "
+                            "%.2f deg, FOV gate [%.2f, %.2f]",
+                            "stated" if train.lens_stated else "assumed",
+                            train.lens.menu_label,
+                            shared_state.camera_type(),
+                            train.fov_degrees,
+                            *_fov_gate_bounds(train),
+                        )
+                        # Only meaningful against a gate we are actually
+                        # going to hand over.
+                        _warn_if_outside_solver_database(t3, train)
 
                 # Every camera frame already carries a tiny radiometer sample
                 # reduced in the camera process. Collect all of them and publish
@@ -1076,12 +1099,22 @@ def solver(
                         # view before verification and rejects survivors after
                         # fitting, so this window has to describe the actual
                         # hardware or nothing solves. See docs/adr/0027.
-                        fov_estimate, fov_max_error = train.solver_fov_params()
+                        #
+                        # Under an **unknown optical train** there is nothing
+                        # to derive it from -- the frames came through some
+                        # other optics -- so no gate is passed at all rather
+                        # than a wrong one. Omitting costs the upper bound
+                        # that rejects confident mis-solves, which is a trade
+                        # only defensible because nothing is being pointed at
+                        # the sky. See the third-rung amendment to 0029.
+                        if train_known:
+                            (
+                                _solver_args["fov_estimate"],
+                                _solver_args["fov_max_error"],
+                            ) = train.solver_fov_params()
                         solution = t3.solve_from_centroids(
                             centroids,
                             (512, 512),
-                            fov_estimate=fov_estimate,
-                            fov_max_error=fov_max_error,
                             match_max_error=0.005,
                             return_matches=True,  # Required for SQM calculation
                             target_pixel=shared_state.target_pixel(),
diff --git a/python/PiFinder/state.py b/python/PiFinder/state.py
index 555b6dd04..c0111eb4d 100644
--- a/python/PiFinder/state.py
+++ b/python/PiFinder/state.py
@@ -309,6 +309,11 @@ def __init__(self) -> None:
         # None means "not stated", which resolves to the sensor's shipped lens
         # -- that is what lets installs predating this setting keep working.
         self.__camera_lens = config.Config().get_option("camera_lens")
+        # Whether the frames arriving actually came through the optics the two
+        # halves above describe. True until a camera says otherwise, so the
+        # window before the camera process reports behaves like the hardware
+        # case rather than silently dropping the FOV gate on every boot.
+        self.__optical_train_known = True
         # Degrees the camera process rotates the solve/display image relative
         # to the stored raw frame (PIL CCW). None until the camera reports.
         self.__solve_image_rotation = None
@@ -396,6 +401,18 @@ def set_camera_lens(self, v: Optional[str]):
         """
         self.__camera_lens = v
 
+    def optical_train_known(self) -> bool:
+        """False when the frames did not come through this device's optics.
+
+        See ``CameraInterface.optical_train_known``. Read alongside
+        ``camera_type``/``camera_lens`` rather than instead of them: the train
+        still resolves, it just does not describe the frames.
+        """
+        return self.__optical_train_known
+
+    def set_optical_train_known(self, v: bool):
+        self.__optical_train_known = bool(v)
+
     def sats(self):
         return self.__sats
 
diff --git a/python/tests/test_camera_interface.py b/python/tests/test_camera_interface.py
index 034ac5823..e17d134af 100644
--- a/python/tests/test_camera_interface.py
+++ b/python/tests/test_camera_interface.py
@@ -120,3 +120,46 @@ def test_recovers_after_stuck_capture_clears(self):
         assert cam.capture_calls == 2
         assert cam._capture_thread is not wedged_thread
         assert cam._capture_thread is None
+
+
+@pytest.mark.unit
+class TestOpticalTrainKnown:
+    """Which cameras are entitled to a derived FOV gate.
+
+    The default direction is the whole point: a camera has to opt *out*, so a
+    new hardware backend inherits the gate rather than silently losing it and
+    the mis-solve protection with it. See the third-rung amendment to
+    docs/adr/0029-fov-gate-width-follows-lens-confidence.md.
+    """
+
+    def test_a_camera_pointed_at_the_sky_defaults_to_known(self):
+        assert _ScriptedCamera().optical_train_known() is True
+
+    def test_the_debug_camera_declares_its_train_unknown(self):
+        """It replays a recording, so config's lens describes absent glass.
+
+        This is what keeps `--camera debug` solving on a config that states a
+        lens -- hq x 16mm gates [14.55, 19.69] and hq x 12mm [17.37, 23.49],
+        against frames that are 10.2 deg.
+        """
+        from PiFinder.camera_debug import CameraDebug
+
+        assert CameraDebug(exposure_time=400000).optical_train_known() is False
+
+    def test_the_debug_camera_still_declares_the_sensor_it_recorded_on(self):
+        # Unknown is about the *pairing*, not the sensor: SQM's profile lookup
+        # and the Lens menu still need a plausible half to resolve from.
+        from PiFinder.camera_debug import CameraDebug
+
+        assert CameraDebug(exposure_time=400000).get_cam_type() == "Debug hq"
+
+    def test_shared_state_assumes_known_before_any_camera_reports(self):
+        """The boot window every run passes through must keep its gate.
+
+        The camera process publishes this after the solver is already
+        looping, so a False default would drop the FOV gate on real hardware
+        for the first few frames of every boot.
+        """
+        from PiFinder.state import SharedStateObj
+
+        assert SharedStateObj().optical_train_known() is True
diff --git a/python/tests/test_lens_self_heal.py b/python/tests/test_lens_self_heal.py
index b05eb0682..8b637c763 100644
--- a/python/tests/test_lens_self_heal.py
+++ b/python/tests/test_lens_self_heal.py
@@ -42,9 +42,10 @@ def get_option(self, option, default=None):
 
 
 class FakeSharedState:
-    def __init__(self, camera_type="imx462", camera_lens=None):
+    def __init__(self, camera_type="imx462", camera_lens=None, train_known=True):
         self._camera_type = camera_type
         self._camera_lens = camera_lens
+        self._train_known = train_known
         self.published = []
 
     def camera_type(self):
@@ -53,6 +54,12 @@ def camera_type(self):
     def camera_lens(self):
         return self._camera_lens
 
+    def optical_train_known(self):
+        return self._train_known
+
+    def set_optical_train_known(self, value):
+        self._train_known = value
+
     def set_camera_lens(self, value):
         self._camera_lens = value
         self.published.append(value)
@@ -76,6 +83,11 @@ def _solve(fov):
 TWELVE_MM_ON_IMX462 = build_optical_train("imx462", "12mm").fov_degrees
 SIXTEEN_MM_ON_IMX462 = build_optical_train("imx462", "16mm").fov_degrees
 
+# What tetra3 fits the archived frames in test_images/ at -- the frames the
+# debug camera replays. Measured, not derived; test_optics_solving.py asserts
+# the same figure against the real solver.
+DEBUG_FRAME_FITTED_FOV = 10.20
+
 
 def _feed(healer, fov, times):
     for _ in range(times):
@@ -248,6 +260,66 @@ def test_an_unidentifiable_fit_logs_once_not_once_per_frame(self, caplog):
         assert len(caplog.records) == 1
         assert "matches no lens" in caplog.records[0].getMessage()
 
+    def test_an_unknown_optical_train_never_promotes(self):
+        """The exact `--camera debug` case, which used to write.
+
+        The archived frames fit 10.20 deg, which is 1.3% off the hq's derived
+        10.33 -- comfortably inside LENS_IDENTIFY_TOLERANCE, so this is not a
+        measurement self-heal would reject on its merits. It has to be
+        declined on provenance: the fit measures the train the frames were
+        *recorded* on, and the developer's config is about a different
+        machine. Writing 25mm here is what leaves a real imx462 attached
+        afterwards deriving 6.4 deg and unable to heal its way back.
+        """
+        cfg = FakeConfig()
+        state = FakeSharedState(camera_type="hq", train_known=False)
+        healer = LensSelfHeal(cfg, state)
+
+        _feed(healer, DEBUG_FRAME_FITTED_FOV, LENS_IDENTIFY_CONSECUTIVE * 4)
+
+        assert "camera_lens" not in cfg.options
+        assert state.published == []
+        assert cfg.calls == []
+
+    def test_the_same_fit_would_have_been_promoted_on_real_optics(self):
+        # Pins the test above to provenance rather than to the number: change
+        # only train_known and the identical measurement writes.
+        cfg = FakeConfig()
+        state = FakeSharedState(camera_type="hq", train_known=True)
+        healer = LensSelfHeal(cfg, state)
+
+        _feed(healer, DEBUG_FRAME_FITTED_FOV, LENS_IDENTIFY_CONSECUTIVE)
+
+        assert cfg.options["camera_lens"] == "25mm"
+
+    def test_an_unknown_train_logs_once_not_once_per_frame(self, caplog):
+        cfg = FakeConfig()
+        state = FakeSharedState(camera_type="hq", train_known=False)
+        healer = LensSelfHeal(cfg, state)
+
+        with caplog.at_level("INFO", logger="IMU.Integrator"):
+            _feed(healer, DEBUG_FRAME_FITTED_FOV, 25)
+
+        assert len(caplog.records) == 1
+        assert "Optical train is unknown" in caplog.records[0].getMessage()
+
+    def test_a_run_does_not_survive_the_train_going_unknown(self):
+        """Agreement counted on real optics must not be spent on a recording.
+
+        Ordering matters here: the check sits ahead of the stated-lens branch
+        precisely so it resets the streak rather than falling through it.
+        """
+        cfg, state = FakeConfig(), FakeSharedState(camera_type="hq")
+        healer = LensSelfHeal(cfg, state)
+
+        _feed(healer, DEBUG_FRAME_FITTED_FOV, LENS_IDENTIFY_CONSECUTIVE - 1)
+        state.set_optical_train_known(False)
+        _feed(healer, DEBUG_FRAME_FITTED_FOV, 1)
+        state.set_optical_train_known(True)
+        _feed(healer, DEBUG_FRAME_FITTED_FOV, 1)
+
+        assert "camera_lens" not in cfg.options
+
 
 @pytest.mark.unit
 class TestSelfHealIsNeverFatal:
diff --git a/python/tests/test_optics.py b/python/tests/test_optics.py
index 767b25248..5477ecb45 100644
--- a/python/tests/test_optics.py
+++ b/python/tests/test_optics.py
@@ -617,7 +617,15 @@ def test_the_debug_frames_field_of_view_sits_inside_the_hq_gate(self):
         """The frames in test_images/ measure ~10.2 degrees when solved.
 
         That is inside hq + 25mm's window and outside imx296 + 16mm's, which
-        is the entire reason the debug camera's declared sensor changed.
+        is why the debug camera's declared sensor changed under ADR 0027.
+
+        It is no longer what makes `--camera debug` solve, though, and reading
+        it that way is how the regression got missed: this fits only because
+        no lens is stated. State one and the same sensor derives 17.12 or
+        20.43 degrees. The gate is now omitted entirely under an **unknown
+        optical train** -- see TestOpticalTrainKnown in
+        test_camera_interface.py and the no-gate cases in
+        test_optics_solving.py.
         """
         measured_debug_frame_fov = 10.2
 
diff --git a/python/tests/test_optics_solving.py b/python/tests/test_optics_solving.py
index 757e57aad..4e119b208 100644
--- a/python/tests/test_optics_solving.py
+++ b/python/tests/test_optics_solving.py
@@ -14,7 +14,7 @@
 from PIL import Image
 
 from PiFinder import utils
-from PiFinder.optics import build_optical_train
+from PiFinder.optics import LENSES, build_optical_train
 
 pytestmark = pytest.mark.integration
 
@@ -113,3 +113,83 @@ def test_a_mis_stated_train_rejects_a_perfectly_good_frame(
     """
     solution = _solve(solver, debug_centroids[frame], build_optical_train("imx296"))
     assert solution.get("RA") is None
+
+
+# Every lens a config can name. The debug camera's sensor is fixed at hq, so
+# these are exactly the trains `--camera debug` can find itself resolving --
+# and only one of them is the one the frames were shot through.
+STATEABLE_LENSES = tuple(LENSES)
+
+
+def _solve_without_a_gate(solver, centroids):
+    """What the solver does under an **unknown optical train**.
+
+    Mirrors `solver.py`'s branch rather than re-deriving anything: when the
+    frames did not come through this device's optics there is nothing to
+    derive a gate from, so `fov_estimate`/`fov_max_error` are not passed.
+    """
+    return solver.solve_from_centroids(
+        centroids,
+        (512, 512),
+        match_max_error=0.005,
+    )
+
+
+@pytest.mark.parametrize("frame", DEBUG_FRAMES)
+@pytest.mark.parametrize("lens_key", STATEABLE_LENSES)
+def test_a_stated_lens_gates_out_the_debug_frames(
+    solver, debug_centroids, frame, lens_key
+):
+    """The regression itself, asserted rather than described.
+
+    ADR 0027 fixed the debug camera's *sensor* and left its lens reading live
+    from config, so stating one pairs hq with glass that is not in the loop:
+    16mm implies 17.12 deg and 12mm 20.43, against 10.2 deg frames. Only the
+    25mm -- the lens these frames were actually shot through -- still solves,
+    which is precisely why nobody noticed until a config stated something.
+    """
+    train = build_optical_train("hq", lens_key)
+    solution = _solve(solver, debug_centroids[frame], train)
+
+    if lens_key == "25mm":
+        assert solution.get("RA") is not None
+    else:
+        assert solution.get("RA") is None, (
+            f"{lens_key} on hq derives {train.fov_degrees:.2f} deg; "
+            f"if this now solves the gate is no longer doing its job"
+        )
+
+
+@pytest.mark.parametrize("frame", DEBUG_FRAMES)
+@pytest.mark.parametrize("lens_key", STATEABLE_LENSES)
+def test_no_gate_solves_the_debug_frames_whatever_config_states(
+    solver, debug_centroids, frame, lens_key
+):
+    """The fix: `--camera debug` works on any config, including yours.
+
+    The lens is parametrised but unused by the solve on purpose -- that is
+    the property under test. Under an unknown optical train the config's lens
+    cannot reach the solver at all, so every one of these has to pass.
+    """
+    solution = _solve_without_a_gate(solver, debug_centroids[frame])
+
+    assert solution.get("RA") is not None, f"failed to solve with {lens_key} stated"
+    assert solution["FOV"] == pytest.approx(MEASURED_DEBUG_FRAME_FOV, abs=0.1)
+
+
+@pytest.mark.parametrize("frame", DEBUG_FRAMES)
+def test_dropping_in_frames_from_another_train_still_solves(
+    solver, debug_centroids, frame
+):
+    """Why no gate, rather than the camera declaring the frames' own FOV.
+
+    A developer replacing test_images/ with frames off their own device is
+    the main use of debug mode. Any declared gate -- including one centred on
+    10.2 deg -- would reject those, which is the bug we are fixing wearing a
+    different hat. Asserted here with the imx296/12mm gate (16.38 deg) as the
+    stand-in for "a train nobody anticipated": it rejects these frames, and
+    no-gate does not.
+    """
+    foreign = build_optical_train("imx296", "12mm")
+    assert _solve(solver, debug_centroids[frame], foreign).get("RA") is None
+    assert _solve_without_a_gate(solver, debug_centroids[frame]).get("RA") is not None

From b5d6a482ab2b64acaea883bcd1b5173e5f7bc68a Mon Sep 17 00:00:00 2001
From: Tak Kaneko <>
Date: Sat, 22 Aug 2026 01:11:57 +0200
Subject: [PATCH 39/45] Refactor

---
 python/PiFinder/imu/imu_align/hand_eye_solver.py | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index 5ef33f879..b7cb3181b 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -83,10 +83,10 @@ def _calculate_solution_uncertainty(
 
         # Calculate degrees of freedom of the problem
         if sample_timestamps is None:
-            dof = N_UNKNOWN_PARAMS * (m_meas - n_sol)  # Overestimates the DoF!
+            dof = N_UNKNOWN_PARAMS * (m_meas - n_sol)  # NOTE: Overestimates the DoF!
         else:
             n_connected_components, n_nodes = self._calculate_connected_components(sample_timestamps)
-            dof = N_UNKNOWN_PARAMS * (n_nodes - n_connected_components - 1)  # Degrees of freedom
+            dof = N_UNKNOWN_PARAMS * (n_nodes - n_connected_components - 1)
 
         # Calculate reduced Chi-square
         rss = 2 * self.lsq_result.cost  # Because cost = 0.5 * sum(residuals**2)
@@ -168,7 +168,10 @@ def solve_rotation(
     x0 is the initial guess for q_12 as a rotation vector. The default (zeros)
     is the identity rotation.
 
-    Returns None for q_12 if the solver failed to converge. 
+    Returns None for q_12 if the solver failed to converge.
+
+    NOTE: Possible future improvements: 1) Tune the LM parameters, 2) Calculate
+    the Jacobians analytically (though fast enough doing it numerically).
     """
     if len(q1_list) != len(q2_list):
         raise ValueError("q1_list and q2_list must be the same length")
@@ -179,9 +182,6 @@ def solve_rotation(
         raise ValueError("x0 must be a length-3 vector")
 
     logger.debug(f"Solving for relative rotation from {len(q1_list)} sample pairs.")
-
-    # TODO: Tune LM params
-    # TODO: Calculate the Jacobians analytically? Current numerical Jacobians is probably fast enough?
     result = least_squares(residual_rotation_vector, x0, method='lm', 
                            args=(q1_list, q2_list))
 
@@ -207,7 +207,7 @@ def solve_rotation_with_outlier_removal(
         sample_timestamps: Union[list_of_float_pairs, None] = None,
         mad_threshold = 4.45,  # TODO: Remove? Reject outlier above this multiple of MAD in first pass
         n_min_samples = N_UNKNOWN_PARAMS,  # Minimum number of sample pairs for a solution
-        ):
+        ) -> tuple[Union[quaternion.quaternion, None], HandEyeSolverDiagnostics]:
     """
     Solve the hand-eye problem with a single pass of outlier rejection (see 
     solve_rotation() for details).

From 6f581ee4be7c388e3c5e68b2deb841e8cbf6194c Mon Sep 17 00:00:00 2001
From: TakKanekoGit <>
Date: Sat, 22 Aug 2026 09:59:48 +0100
Subject: [PATCH 40/45] Lint

---
 python/PiFinder/imu/imu_align/hand_eye_solver.py | 1 -
 python/PiFinder/imu/imu_align/imu_alignment.py   | 2 +-
 python/tests/test_imu_align.py                   | 2 --
 3 files changed, 1 insertion(+), 4 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index b7cb3181b..09081c97d 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -7,7 +7,6 @@
 Where the goal is to solve for the rotation q_12. Given enough measurements of 
 q1 and q2, we can solve for q_12.
 """
-from dataclasses import dataclass
 import logging
 import numpy as np
 import quaternion  # Note: numpy-quaternion convention: quaternion(w, x, y, z)
diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index 280aa2491..48447ff72 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -82,7 +82,7 @@
 
 from PiFinder.types.coordinates import RaDecRoll
 from PiFinder.pointing_model import quaternion_transforms as qt
-from PiFinder.imu.imu_align.hand_eye_solver import solve_rotation, solve_rotation_with_outlier_removal
+from PiFinder.imu.imu_align.hand_eye_solver import solve_rotation_with_outlier_removal
 
 list_of_quats = list[quaternion.quaternion]
 
diff --git a/python/tests/test_imu_align.py b/python/tests/test_imu_align.py
index ea22744c0..7ea7ae0d9 100644
--- a/python/tests/test_imu_align.py
+++ b/python/tests/test_imu_align.py
@@ -1,5 +1,3 @@
-import pytest
-
 import numpy as np
 import quaternion  # Note: numpy-quaternion convention: quaternion(w, x, y, z)
 from PiFinder.imu.imu_align.hand_eye_solver import solve_rotation, simulate_quaternion_measurements

From 9b9a49a589f64b8f8d1f1b0e3df8eff93badb023 Mon Sep 17 00:00:00 2001
From: TakKanekoGit <>
Date: Sat, 22 Aug 2026 10:00:07 +0100
Subject: [PATCH 41/45] Lint: Format

---
 .../PiFinder/imu/imu_align/hand_eye_solver.py | 220 +++++++++++-------
 .../PiFinder/imu/imu_align/imu_alignment.py   | 109 +++++----
 python/PiFinder/integrator.py                 |  38 +--
 .../pointing_model/quaternion_transforms.py   |   6 +-
 python/tests/test_imu_align.py                |  35 +--
 5 files changed, 254 insertions(+), 154 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index 09081c97d..41249818f 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -1,12 +1,13 @@
 """
 Core solver functionalities for solving the quaternion form of the hand-eye
-problem: 
+problem:
 
 q1 * q_12 = q_12 * q2
 
-Where the goal is to solve for the rotation q_12. Given enough measurements of 
+Where the goal is to solve for the rotation q_12. Given enough measurements of
 q1 and q2, we can solve for q_12.
 """
+
 import logging
 import numpy as np
 import quaternion  # Note: numpy-quaternion convention: quaternion(w, x, y, z)
@@ -26,11 +27,16 @@
 
 N_UNKNOWN_PARAMS = 3  # Number of unknown parameters in the problem to solve
 
+
 class HandEyeSolverDiagnostics:
     lsq_result: OptimizeResult  # Result from scipy.optimize.least_squares
 
-    sample_timestamps: Union[list_of_float_pairs, None]  # Timestamps of each sample-pair
-    sample_time_differences: Union[np.ndarray, None]  # Time differences between each sample [s]
+    sample_timestamps: Union[
+        list_of_float_pairs, None
+    ]  # Timestamps of each sample-pair
+    sample_time_differences: Union[
+        np.ndarray, None
+    ]  # Time differences between each sample [s]
     residual_norms: np.ndarray  # Residual norms of each sample [rad]
     rotation_angles: np.ndarray  # Rotation angles of each sample [rad]
     sol_cov_matrix: np.ndarray  # Solution covariance matrix
@@ -39,9 +45,13 @@ class HandEyeSolverDiagnostics:
     # Optional
     meta_data: dict
 
-    def __init__(self, lsq_result, q1_list: list_of_quats, q2_list: list_of_quats, 
-                 sample_timestamps: Union[list_of_float_pairs, None] = None):
-                
+    def __init__(
+        self,
+        lsq_result,
+        q1_list: list_of_quats,
+        q2_list: list_of_quats,
+        sample_timestamps: Union[list_of_float_pairs, None] = None,
+    ):
         if len(q1_list) != len(q2_list):
             raise ValueError("q1_list and q2_list must be the same length")
         if sample_timestamps is None:
@@ -49,25 +59,37 @@ def __init__(self, lsq_result, q1_list: list_of_quats, q2_list: list_of_quats,
             self.sample_time_differences = None
         else:
             if len(sample_timestamps) != len(q1_list):
-                raise ValueError("sample_timestamps must be the same length as q1_list and q2_list")
+                raise ValueError(
+                    "sample_timestamps must be the same length as q1_list and q2_list"
+                )
             self.sample_timestamps = sample_timestamps.copy()
-            self.sample_time_differences = np.array([t2 - t1 for t1, t2 in self.sample_timestamps])
+            self.sample_time_differences = np.array(
+                [t2 - t1 for t1, t2 in self.sample_timestamps]
+            )
 
         self.lsq_result = lsq_result
 
         # Residual norm per sample (collapse the 3 measurements per sample into one)
-        resid = lsq_result.fun.reshape((-1, N_UNKNOWN_PARAMS))  # Each row corresponds to a sample 
-        self.residual_norms = np.linalg.norm(resid, axis=1)  # Residual per sample in radians
+        resid = lsq_result.fun.reshape(
+            (-1, N_UNKNOWN_PARAMS)
+        )  # Each row corresponds to a sample
+        self.residual_norms = np.linalg.norm(
+            resid, axis=1
+        )  # Residual per sample in radians
 
         # Calculate rotations of each sample [rad]
-        self.rotation_angles = [qt.get_quat_angular_diff(q1, q2) for q1, q2 in zip(q1_list, q2_list)]
+        self.rotation_angles = [
+            qt.get_quat_angular_diff(q1, q2) for q1, q2 in zip(q1_list, q2_list)
+        ]
 
-        self.sol_cov_matrix, self.sol_angle_error =self._calculate_solution_uncertainty(sample_timestamps)
+        self.sol_cov_matrix, self.sol_angle_error = (
+            self._calculate_solution_uncertainty(sample_timestamps)
+        )
         self.meta_data = {}
 
-
     def _calculate_solution_uncertainty(
-            self, sample_timestamps: Union[list_of_float_pairs, None]) -> tuple[np.ndarray, float]:
+        self, sample_timestamps: Union[list_of_float_pairs, None]
+    ) -> tuple[np.ndarray, float]:
         """
         Calculate the standard error of the solution:
         Cov = sigma ** 2 * inv(J.T @ J)
@@ -84,7 +106,9 @@ def _calculate_solution_uncertainty(
         if sample_timestamps is None:
             dof = N_UNKNOWN_PARAMS * (m_meas - n_sol)  # NOTE: Overestimates the DoF!
         else:
-            n_connected_components, n_nodes = self._calculate_connected_components(sample_timestamps)
+            n_connected_components, n_nodes = self._calculate_connected_components(
+                sample_timestamps
+            )
             dof = N_UNKNOWN_PARAMS * (n_nodes - n_connected_components - 1)
 
         # Calculate reduced Chi-square
@@ -97,7 +121,6 @@ def _calculate_solution_uncertainty(
 
         return sol_cov_matrix, sol_angle_error
 
-
     @staticmethod
     def _calculate_connected_components(sample_timestamps: list_of_float_pairs) -> int:
         """
@@ -117,21 +140,21 @@ def _calculate_connected_components(sample_timestamps: list_of_float_pairs) -> i
         # Matrix A has a 1 in row/column pairs
         rows, cols = idx_pairs.T
         A = csr_matrix(
-            (np.ones(2 * len(idx_pairs)),
-            (np.r_[rows, cols], np.r_[cols, rows])),
-            shape=(n_nodes, n_nodes)
+            (np.ones(2 * len(idx_pairs)), (np.r_[rows, cols], np.r_[cols, rows])),
+            shape=(n_nodes, n_nodes),
         )
 
         # Return the number of connected components
         return connected_components(A, directed=False, return_labels=False), n_nodes
 
 
-def residual_rotation_vector(x,  # (3,) Trial solution (q as rotation vector) 
-                             q1_list: list_of_quats,  # List of rotation quaternions
-                             q2_list: list_of_quats 
-                             ) -> np.ndarray:
+def residual_rotation_vector(
+    x,  # (3,) Trial solution (q as rotation vector)
+    q1_list: list_of_quats,  # List of rotation quaternions
+    q2_list: list_of_quats,
+) -> np.ndarray:
     """
-    For solving q_cam2imu in the quaternion form of the hand-eye problem: 
+    For solving q_cam2imu in the quaternion form of the hand-eye problem:
     q1 * q_12 = q_12 * q2
 
     Calculate the esiduals at the trial solution x for least squares
@@ -145,17 +168,17 @@ def residual_rotation_vector(x,  # (3,) Trial solution (q as rotation vector)
     for ii, (q1, q2) in enumerate(zip(q1_list, q2_list)):
         q_err = (q1 * q_12) * (q_12 * q2).conjugate()  # Error quaternion
         # Convert to rotation vector (Lie algebra logarithm map)
-        residuals[(3 * ii):(3 * ii + 3)] = quaternion.as_rotation_vector(q_err)
+        residuals[(3 * ii) : (3 * ii + 3)] = quaternion.as_rotation_vector(q_err)
 
     return np.array(residuals)
 
 
 def solve_rotation(
-        q1_list: list_of_quats,  # List of rotation quaternions
-        q2_list: list_of_quats,
-        x0: Union[np.ndarray, list] = np.zeros(N_UNKNOWN_PARAMS),  # Initial guess
-        sample_timestamps: Union[list_of_float_pairs, None] = None,
-        ) -> tuple[Union[quaternion.quaternion, None], HandEyeSolverDiagnostics]:
+    q1_list: list_of_quats,  # List of rotation quaternions
+    q2_list: list_of_quats,
+    x0: Union[np.ndarray, list] = np.zeros(N_UNKNOWN_PARAMS),  # Initial guess
+    sample_timestamps: Union[list_of_float_pairs, None] = None,
+) -> tuple[Union[quaternion.quaternion, None], HandEyeSolverDiagnostics]:
     """
     Solve the quaternion form of the hand-eye problem using least-squares
     optimization of the rotation q_12 parameterized as a rotation vector:
@@ -175,23 +198,30 @@ def solve_rotation(
     if len(q1_list) != len(q2_list):
         raise ValueError("q1_list and q2_list must be the same length")
     if len(q1_list) < N_UNKNOWN_PARAMS:
-        raise ValueError(f"q1_list and q2_list must have at least "
-                        f"{N_UNKNOWN_PARAMS} elements. Got {len(q1_list)}")
+        raise ValueError(
+            f"q1_list and q2_list must have at least "
+            f"{N_UNKNOWN_PARAMS} elements. Got {len(q1_list)}"
+        )
     if len(x0) != N_UNKNOWN_PARAMS:
         raise ValueError("x0 must be a length-3 vector")
 
     logger.debug(f"Solving for relative rotation from {len(q1_list)} sample pairs.")
-    result = least_squares(residual_rotation_vector, x0, method='lm', 
-                           args=(q1_list, q2_list))
+    result = least_squares(
+        residual_rotation_vector, x0, method="lm", args=(q1_list, q2_list)
+    )
 
     # Convert estimate from rotation vector to quaternion
     q_12 = quaternion.from_rotation_vector(result.x)
 
-    diagnostics = HandEyeSolverDiagnostics(result, q1_list, q2_list, sample_timestamps=sample_timestamps)
-    logger.debug(f"Ran solver for relative rotation: Solution q_12={q_12}, "
-            f"Solution uncertainty: {np.rad2deg(diagnostics.sol_angle_error):.2f} degrees, "
-            f"Func evaluations: {result.nfev}, Cost = {result.cost:.4g}, "
-            f"Success: {result.success}, {result.message}")
+    diagnostics = HandEyeSolverDiagnostics(
+        result, q1_list, q2_list, sample_timestamps=sample_timestamps
+    )
+    logger.debug(
+        f"Ran solver for relative rotation: Solution q_12={q_12}, "
+        f"Solution uncertainty: {np.rad2deg(diagnostics.sol_angle_error):.2f} degrees, "
+        f"Func evaluations: {result.nfev}, Cost = {result.cost:.4g}, "
+        f"Success: {result.success}, {result.message}"
+    )
 
     if not result.success:
         return None, diagnostics
@@ -200,15 +230,15 @@ def solve_rotation(
 
 
 def solve_rotation_with_outlier_removal(
-        q1_list: list_of_quats,  # List of rotation quaternions
-        q2_list: list_of_quats,
-        x0: Union[np.ndarray, list] = np.zeros(N_UNKNOWN_PARAMS),  # Initial guess
-        sample_timestamps: Union[list_of_float_pairs, None] = None,
-        mad_threshold = 4.45,  # TODO: Remove? Reject outlier above this multiple of MAD in first pass
-        n_min_samples = N_UNKNOWN_PARAMS,  # Minimum number of sample pairs for a solution
-        ) -> tuple[Union[quaternion.quaternion, None], HandEyeSolverDiagnostics]:
+    q1_list: list_of_quats,  # List of rotation quaternions
+    q2_list: list_of_quats,
+    x0: Union[np.ndarray, list] = np.zeros(N_UNKNOWN_PARAMS),  # Initial guess
+    sample_timestamps: Union[list_of_float_pairs, None] = None,
+    mad_threshold=4.45,  # TODO: Remove? Reject outlier above this multiple of MAD in first pass
+    n_min_samples=N_UNKNOWN_PARAMS,  # Minimum number of sample pairs for a solution
+) -> tuple[Union[quaternion.quaternion, None], HandEyeSolverDiagnostics]:
     """
-    Solve the hand-eye problem with a single pass of outlier rejection (see 
+    Solve the hand-eye problem with a single pass of outlier rejection (see
     solve_rotation() for details).
     """
     # First pass:
@@ -225,45 +255,56 @@ def solve_rotation_with_outlier_removal(
     mad = np.median(np.abs(diagnostics.residual_norms - median))
     mean = np.mean(diagnostics.residual_norms)
     sd = np.std(diagnostics.residual_norms)
-    logger.debug("After first pass: "
+    logger.debug(
+        "After first pass: "
         f"Solution uncertainty: {np.rad2deg(diagnostics.sol_angle_error):.2f} degrees "
-        f"MAD: {np.rad2deg(mad):.2f} degrees SD: {np.rad2deg(sd):.2f} degrees.")
-                 
-    msk_accept = np.logical_and(diagnostics.residual_norms < (median + mad_threshold * mad),
-                                diagnostics.residual_norms < (mean + 3 * sd))
+        f"MAD: {np.rad2deg(mad):.2f} degrees SD: {np.rad2deg(sd):.2f} degrees."
+    )
+
+    msk_accept = np.logical_and(
+        diagnostics.residual_norms < (median + mad_threshold * mad),
+        diagnostics.residual_norms < (mean + 3 * sd),
+    )
     if np.all(msk_accept):
         logger.debug("No outliers. Returning solution from first-pass.")
         return q12_solution, diagnostics
-    
-    logger.debug("Outlier removal. Keeping "
-        f"{np.sum(msk_accept)}/{diagnostics.residual_norms.shape[0]} samples.")
+
+    logger.debug(
+        "Outlier removal. Keeping "
+        f"{np.sum(msk_accept)}/{diagnostics.residual_norms.shape[0]} samples."
+    )
 
     if np.sum(msk_accept) < n_min_samples:
-        np.info(f"Less than {n_min_samples} samples after outlier removal. "
-                "Not enough samples for second pass. Returning solution from first-pass.")
+        np.info(
+            f"Less than {n_min_samples} samples after outlier removal. "
+            "Not enough samples for second pass. Returning solution from first-pass."
+        )
         return q12_solution, diagnostics
 
     # Remove outliers
     q1_accepted = [q for ii, q in enumerate(q1_list) if msk_accept[ii]]
     q2_accepted = [q for ii, q in enumerate(q2_list) if msk_accept[ii]]
-    timestamps_accepted = [t for ii, t in enumerate(sample_timestamps) if msk_accept[ii]]
+    timestamps_accepted = [
+        t for ii, t in enumerate(sample_timestamps) if msk_accept[ii]
+    ]
 
     # Solve again after outlier removal, using previous solution as the initial guess
     x0 = quaternion.as_rotation_vector(q12_solution)
     q12_solution_new, diagnostics_new = solve_rotation(
-        q1_accepted, q2_accepted, x0, timestamps_accepted)
+        q1_accepted, q2_accepted, x0, timestamps_accepted
+    )
     # Store solutions and diagnostics from first pass
-    diagnostics_new.meta_data['first_pass_solution'] = q12_solution
-    diagnostics_new.meta_data['first_pass_diagnostics'] = diagnostics
+    diagnostics_new.meta_data["first_pass_solution"] = q12_solution
+    diagnostics_new.meta_data["first_pass_diagnostics"] = diagnostics
 
     return q12_solution_new, diagnostics_new
 
 
 def _solution_diagnostics(result):
-    """ 
-    Returns the diagnostics of the least-squares solution. The input, 
+    """
+    Returns the diagnostics of the least-squares solution. The input,
     `result` is the output from scipy.optimize.least_squares.
-    
+
     Condition number: < 10 excellent, < 100 acceptable, <1E4 weak observability
 
     TODO: Remove?
@@ -272,13 +313,15 @@ def _solution_diagnostics(result):
 
     # Estimate the uncertainty of the solution
     residuals = result.fun
-    dof = len(residuals) - len(result.x)  # Degrees-of-freedom = Number of meas - Number of params
+    dof = len(residuals) - len(
+        result.x
+    )  # Degrees-of-freedom = Number of meas - Number of params
     residuals_var = np.sum(residuals**2) / dof  # Estimate of residual variance
 
     # Using 'backslash' rather than inv(): Faster but could be unstable?
-    #JTJ = result.jac.T @ result.jac  # Hessian approx from the Jacobians
-    #cov_x = residuals_var * np.linalg.solve(JTJ, np.eye(JTJ.shape[0]))  
-    
+    # JTJ = result.jac.T @ result.jac  # Hessian approx from the Jacobians
+    # cov_x = residuals_var * np.linalg.solve(JTJ, np.eye(JTJ.shape[0]))
+
     # Estimate the uncertainty at the solution using SVD: More robust
     U, s, Vt = np.linalg.svd(result.jac, full_matrices=False)
     cov_x = residuals_var * (Vt.T / s**2) @ Vt
@@ -286,15 +329,18 @@ def _solution_diagnostics(result):
     sigma_total = np.sqrt(np.trace(cov_x))  # [rad] Total rotaion uncertainty
 
     t_compute = time.time() - t_start
-    print(f"Diagnostics for q_cam2imu: compute time = {t_compute:.3f}s, ",
-          f"Total angular uncertainty = {np.rad2deg(sigma_total):.2} deg, ",
-          f"Condition number = {condition_number:.1g}")
+    print(
+        f"Diagnostics for q_cam2imu: compute time = {t_compute:.3f}s, ",
+        f"Total angular uncertainty = {np.rad2deg(sigma_total):.2} deg, ",
+        f"Condition number = {condition_number:.1g}",
+    )
 
     return sigma_total, condition_number
 
 
 # ------- Helper functions -------
 
+
 def ensure_quat_list_continuity(q_list: list_of_quats) -> list_of_quats:
     """
     Ensures that consecutive quaternions in the list have consistent signs (due
@@ -310,7 +356,9 @@ def ensure_quat_list_continuity(q_list: list_of_quats) -> list_of_quats:
     return q_list_out
 
 
-def calculate_relative_rotations(q1_list: list_of_quats, q2_list: list_of_quats) -> list_of_quats:
+def calculate_relative_rotations(
+    q1_list: list_of_quats, q2_list: list_of_quats
+) -> list_of_quats:
     """
     Calculate the relative rotation between q1_list and the corresponding q2_list:
     dq[k] = q1[k].conjugate() * q2[k]
@@ -320,22 +368,24 @@ def calculate_relative_rotations(q1_list: list_of_quats, q2_list: list_of_quats)
 
 # ------ Simulation functions for testing & analysis --------------------------
 
+
 def _q_noise(noise_amp: float):
-    """ Generates random quaternion noise. Noise amp is in radians """
+    """Generates random quaternion noise. Noise amp is in radians"""
     noise = np.radians(noise_amp) * np.random.randn(3)
     return quaternion.from_rotation_vector(noise)
 
 
 def _add_noise_to_quaternion_list(qs: list_of_quats, noise_amp: float):
-    """ Adds noise to a list of quaternions. noise_amp is in radians. """
+    """Adds noise to a list of quaternions. noise_amp is in radians."""
     qs_out = []
     for q in qs:
         qs_out.append(_q_noise(noise_amp) * q)
 
     return qs_out
 
+
 def _random_quaternions(N: int, max_rot=None) -> list_of_quats:
-    """ 
+    """
     Returns a list of N random quaternions. If max_rot is None, the quaternions
     will be random. If specified, it limits the maximum swing angle from the
     previous orientation.
@@ -354,24 +404,24 @@ def _random_quaternions(N: int, max_rot=None) -> list_of_quats:
             q = qs[-1] * dq
 
         qs.append(q)
-    
+
     return qs
 
 
 def simulate_quaternion_measurements(
-        q_12: quaternion.quaternion,  # True rel. orientations (q1 ro q2 alignment)
-        N: int = 100,  # Number of samples to simulate
-        max_rot = None,  # Max rotation from previous orientation
-        q1_noise_amp: float = np.deg2rad(0.1),  # Noise amp in radians
-        q2_noise_amp: float = np.deg2rad(0.1),  # Noise amp in radians
-        seed=0  # Random seed. None to disable
-        ):
+    q_12: quaternion.quaternion,  # True rel. orientations (q1 ro q2 alignment)
+    N: int = 100,  # Number of samples to simulate
+    max_rot=None,  # Max rotation from previous orientation
+    q1_noise_amp: float = np.deg2rad(0.1),  # Noise amp in radians
+    q2_noise_amp: float = np.deg2rad(0.1),  # Noise amp in radians
+    seed=0,  # Random seed. None to disable
+):
     """
     Simulate camera and IMU measurements
     """
     if seed is not None:
         np.random.seed(seed)
-        
+
     # Generate random IMU orientations
     q2_true = _random_quaternions(N, max_rot=max_rot)
 
diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index 48447ff72..246b81abd 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -14,12 +14,12 @@
 The measurements we have are:
 
 * q_eq2cam: Quaternion rotation of the camera center relative to the equatorial
-  frame. 
+  frame.
 * q_x2imu: The rotation of the IMU relative to some arbibtrary reference frame
   X.
 
 The camera and IMU measurements are paired and assumed to be simultaneous.
-  
+
 Algorithm:
 ----------
 
@@ -70,9 +70,10 @@
 4. In practice, the plate solver will have worse error in roll than RA and Dec.
    This is not accounted for.
 5. Ideally, the camera/IMU should be rotated around all three axes but on a
-   mount, the rotation will likely be around two axes. This may result in a 
+   mount, the rotation will likely be around two axes. This may result in a
    larger uncertainty for the rotation/alignment about some axes.
 """
+
 from dataclasses import dataclass
 
 import logging
@@ -91,8 +92,8 @@
 
 @dataclass
 class CameraImuSample:
-    """
-    """
+    """ """
+
     timestamp: float
     q_cam: quaternion.quaternion
     q_imu: quaternion.quaternion
@@ -102,6 +103,7 @@ class SampleBuffer:
     """
     Buffer of samples
     """
+
     buffer: list
     max_buffer_length: int
 
@@ -125,14 +127,16 @@ def add_sample(self, sample: CameraImuSample):
     def pop_sample(self, idx: int):
         """Remove and return the sample at the given index"""
         return self.buffer.pop(idx)
-    
+
     def remove_samples(self, idx_list: set[int]):
         """Remove multiple samples by indices"""
-        self.buffer = [self.buffer[i] for i in range(len(self.buffer)) if i not in idx_list]
+        self.buffer = [
+            self.buffer[i] for i in range(len(self.buffer)) if i not in idx_list
+        ]
 
     def trim_to_max_length(self):
         if self.len > self.max_buffer_length:
-            self.buffer = self.buffer[-self.max_buffer_length:]
+            self.buffer = self.buffer[-self.max_buffer_length :]
 
 
 class ImuCameraAlignment:
@@ -140,6 +144,7 @@ class ImuCameraAlignment:
     Note that max_time_diff should be kept to a few seconds at most to avoid
     gyro drift over the time between samples.
     """
+
     candidate_buffer: SampleBuffer  # Buffer of camera/IMU samples
     pair_buffer: SampleBuffer  # Buffer of paired samples ofcamera/IMU samples
 
@@ -148,12 +153,14 @@ class ImuCameraAlignment:
     min_angle_diff: float  # [rad] Pair samples with large enough angle difference
     max_age: float  # [s] Maximum age of sample compared to current time
 
-    def __init__(self, 
-                 candidate_buffer_length: int = 60, 
-                 min_n_solve: int = 20, 
-                 max_time_diff: float = 20.0, 
-                 min_angle_diff: float = np.deg2rad(5.0), 
-                 max_age: float = 600.0):
+    def __init__(
+        self,
+        candidate_buffer_length: int = 60,
+        min_n_solve: int = 20,
+        max_time_diff: float = 20.0,
+        min_angle_diff: float = np.deg2rad(5.0),
+        max_age: float = 600.0,
+    ):
         """
         candidate_buffer_length: Should be around sample_freq * max_time_diff
 
@@ -164,7 +171,8 @@ def __init__(self,
         :param max_age: [s] Remove samples older than this. None to ignore
         """
         self.candidate_buffer = SampleBuffer(
-            max_buffer_length=max(candidate_buffer_length, min_n_solve))
+            max_buffer_length=max(candidate_buffer_length, min_n_solve)
+        )
         diff_buffer_length = candidate_buffer_length
         self.pair_buffer = SampleBuffer(max_buffer_length=diff_buffer_length)
 
@@ -175,7 +183,9 @@ def __init__(self,
 
         self._samples_since_last_pair_attempt = 0
 
-    def add_candidate_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
+    def add_candidate_attempt_solve(
+        self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion
+    ):
         """
         For general use, call this pipeline method. Add a new candidate to the
         buffer. When the buffer fills up, pair samples and solve.
@@ -183,18 +193,19 @@ def add_candidate_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2i
         self._add_candidate(timestamp, cam_eq, q_x2imu)
 
         # Pair samples: Runs periodically
-        if ((self._samples_since_last_pair_attempt >= self.min_n_solve) or
-            (self.candidate_buffer.len >= self.candidate_buffer.max_buffer_length)):
+        if (self._samples_since_last_pair_attempt >= self.min_n_solve) or (
+            self.candidate_buffer.len >= self.candidate_buffer.max_buffer_length
+        ):
             self._purge_old_samples(timestamp)
             self._purge_old_candidates()
             self._pair_samples()
 
-            # If the candidate buffer is still full after pairing, remove a 
+            # If the candidate buffer is still full after pairing, remove a
             # batch of the older samples from the buffer
             if self.candidate_buffer.len >= self.candidate_buffer.max_buffer_length:
                 remove_set = set(range(self.min_n_solve))
                 self.candidate_buffer.remove_samples(remove_set)
-            
+
             self._samples_since_last_pair_attempt = 0
         else:
             self._samples_since_last_pair_attempt += 1
@@ -220,12 +231,19 @@ def _trim_buffers(self):
         self.candidate_buffer.trim_to_max_length()
         self.pair_buffer.trim_to_max_length()
 
-    def _add_candidate(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion):
+    def _add_candidate(
+        self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion
+    ):
         """
         Add to the candidate_buffer the camera solve & corresponding IMU sample
         from integrator.
         """
-        if timestamp is None or cam_eq is None or cam_eq.valid is False or q_x2imu is None:
+        if (
+            timestamp is None
+            or cam_eq is None
+            or cam_eq.valid is False
+            or q_x2imu is None
+        ):
             return
 
         # Ensure quaternion continuity from previous candidate sample
@@ -248,17 +266,23 @@ def _purge_old_samples(self, ref_time: float):
             return
 
         allowed_timestamp = ref_time - self.max_age  # Purge anything older than this
-        
+
         # Purge candidate_buffer:
-        remove_idx_list = [i for i, samp in enumerate(self.candidate_buffer.buffer) 
-                           if samp.timestamp < allowed_timestamp]
+        remove_idx_list = [
+            i
+            for i, samp in enumerate(self.candidate_buffer.buffer)
+            if samp.timestamp < allowed_timestamp
+        ]
         if remove_idx_list:
             self.candidate_buffer.remove_samples(set(remove_idx_list))
 
         # Purge diff_buffer:
-        remove_idx_list = [i for i, (samp1, samp2) in enumerate(self.pair_buffer.buffer) 
-                           if samp1.timestamp < allowed_timestamp 
-                           or samp2.timestamp < allowed_timestamp]
+        remove_idx_list = [
+            i
+            for i, (samp1, samp2) in enumerate(self.pair_buffer.buffer)
+            if samp1.timestamp < allowed_timestamp
+            or samp2.timestamp < allowed_timestamp
+        ]
         if remove_idx_list:
             self.pair_buffer.remove_samples(set(remove_idx_list))
 
@@ -267,7 +291,7 @@ def _purge_old_candidates(self):
         Remove samples from candidate_buffer that are older than
         self.max_time_diff from other samples in buffer because these will be
         never paired.
-        
+
         This should be run on a schedule every self.max_time_diff [s].
         """
         if self.candidate_buffer.len <= 1:
@@ -297,8 +321,8 @@ def _pair_samples(self) -> int:
 
         remove_ids = set()
         for isamp1, samp1 in enumerate(self.candidate_buffer.buffer[:-1]):
-            for isamp2 in range(isamp1+1, self.candidate_buffer.len):
-                samp2 =self.candidate_buffer.buffer[isamp2]
+            for isamp2 in range(isamp1 + 1, self.candidate_buffer.len):
+                samp2 = self.candidate_buffer.buffer[isamp2]
                 # Check time difference between samples:
                 dt = samp2.timestamp - samp1.timestamp
                 if dt > self.max_time_diff or dt <= 0:
@@ -324,13 +348,17 @@ def _pair_samples(self) -> int:
             if self.pair_buffer.len >= self.pair_buffer.max_buffer_length:
                 break
 
-        logger.debug(f"Created {n_pairs}-way pairs from {self.candidate_buffer.len} candidate samples.")
+        logger.debug(
+            f"Created {n_pairs}-way pairs from {self.candidate_buffer.len} candidate samples."
+        )
 
         if remove_ids:
             self.candidate_buffer.remove_samples(remove_ids)
-        logger.debug(f"Removed {len(remove_ids)} samples from candidate buffer. "
-                     f"New candidate buffer length: {self.candidate_buffer.len}. "
-                     f"Pair buffer length: {self.pair_buffer.len}.")
+        logger.debug(
+            f"Removed {len(remove_ids)} samples from candidate buffer. "
+            f"New candidate buffer length: {self.candidate_buffer.len}. "
+            f"Pair buffer length: {self.pair_buffer.len}."
+        )
 
         return n_pairs  # Number of successful pairings
 
@@ -342,11 +370,13 @@ def _solve(self, n_pairs=None):
         if n_pairs is None:
             n_pairs = self.pair_buffer.len  # Use all available data
         if n_pairs < self.min_n_solve:
-            raise ValueError(f"Oly {n_pairs} samples available for solve. Need {self.min_n_solve}.")
+            raise ValueError(
+                f"Oly {n_pairs} samples available for solve. Need {self.min_n_solve}."
+            )
 
         # Generate relative rotation quaternions between paired samp1 and samp2
         # The amount of relative rotation for camera and IMU should be the same
-        # and this will solve the relative rotation between them. 
+        # and this will solve the relative rotation between them.
         dq_cam_list = []
         dq_imu_list = []
         sample_timestamps = []
@@ -356,7 +386,8 @@ def _solve(self, n_pairs=None):
             sample_timestamps.append((samp1.timestamp, samp2.timestamp))
 
         # Solve
-        #q_cam2imu, diagnostics = solve_rotation(dq_cam_list, dq_imu_list)
+        # q_cam2imu, diagnostics = solve_rotation(dq_cam_list, dq_imu_list)
         q_cam2imu, diagnostics = solve_rotation_with_outlier_removal(
-            dq_cam_list, dq_imu_list, sample_timestamps=sample_timestamps)
+            dq_cam_list, dq_imu_list, sample_timestamps=sample_timestamps
+        )
         return q_cam2imu, diagnostics
diff --git a/python/PiFinder/integrator.py b/python/PiFinder/integrator.py
index 6ad7ed5f0..a123f96bf 100644
--- a/python/PiFinder/integrator.py
+++ b/python/PiFinder/integrator.py
@@ -257,8 +257,12 @@ def integrator(
         # TODO: Move to a different location
         logger.info("IMU/Camera alignment: Initialized with q_imu2cam: ", idr.q_imu2cam)
         imu_align = ImuCameraAlignment(
-            candidate_buffer_length=60, min_n_solve=20, 
-            max_time_diff=20.0, min_angle_diff=np.deg2rad(5.0), max_age=600.0)
+            candidate_buffer_length=60,
+            min_n_solve=20,
+            max_time_diff=20.0,
+            min_angle_diff=np.deg2rad(5.0),
+            max_age=600.0,
+        )
         # -------------------------------------------------
 
         while True:
@@ -316,22 +320,28 @@ def integrator(
                 # Add IMU/Camera samples to the buffer and attempt solve if buffer is full
                 # TODO: Move to a different location
                 new_q_cam2imu, _diag = imu_align.add_candidate_attempt_solve(
-                    solve_result.last_solve_success, 
-                    solve_result.camera.as_radecroll(), 
-                    solve_result.imu_anchor)
+                    solve_result.last_solve_success,
+                    solve_result.camera.as_radecroll(),
+                    solve_result.imu_anchor,
+                )
                 if new_q_cam2imu is not None:
-                    angular_diff = qt.get_quat_angular_diff(idr.q_imu2cam, new_q_cam2imu)
-                    logger.info("IMU/Camera alignment: New estimate q_imu2cam: ", new_q_cam2imu)
-                    logger.info("IMU/Camera alignment: Angular difference from previous estimate: "
-                                f"{np.rad2deg(angular_diff):.2f} deg | "
-                                "Solution uncertainty: "
-                                f"{np.rad2deg(_diag.sol_angle_error):.2f} deg | "
-                                f"Solve time: {_diag.meta_data['total_solve_time']}")
+                    angular_diff = qt.get_quat_angular_diff(
+                        idr.q_imu2cam, new_q_cam2imu
+                    )
+                    logger.info(
+                        "IMU/Camera alignment: New estimate q_imu2cam: ", new_q_cam2imu
+                    )
+                    logger.info(
+                        "IMU/Camera alignment: Angular difference from previous estimate: "
+                        f"{np.rad2deg(angular_diff):.2f} deg | "
+                        "Solution uncertainty: "
+                        f"{np.rad2deg(_diag.sol_angle_error):.2f} deg | "
+                        f"Solve time: {_diag.meta_data['total_solve_time']}"
+                    )
                     # Update:
                     idr.q_imu2cam = new_q_cam2imu
                 # ---------------------------------------
 
-
                 # Append plate-solve and IMU states to IMU/camera alignment buffer
                 # TODO: Append the following:
                 # solve_result.last_solve_success (timestamp)
@@ -341,7 +351,7 @@ def integrator(
                 # Update idr.q_imu2cam with the new estimate from IMU/camera alignment
                 #
                 # TODO: SuccessfulSolve.last_solve_success is the exposure end time. It's ambiguous...
-                # TODO: Move ImuDeadReckoning._q_imu2cam() to a stand-alone func in imu_dead_reckoning.py with a view to deprecating it 
+                # TODO: Move ImuDeadReckoning._q_imu2cam() to a stand-alone func in imu_dead_reckoning.py with a view to deprecating it
 
             elif isinstance(solve_result, FailedSolve):
                 telemetry.record_solve(
diff --git a/python/PiFinder/pointing_model/quaternion_transforms.py b/python/PiFinder/pointing_model/quaternion_transforms.py
index cf6dbebef..27cfc00e1 100644
--- a/python/PiFinder/pointing_model/quaternion_transforms.py
+++ b/python/PiFinder/pointing_model/quaternion_transforms.py
@@ -62,8 +62,9 @@ def get_quat_angular_diff(
     return d_theta  # In radians
 
 
-def ensure_quat_continuity(q_prev: quaternion.quaternion, 
-                           q_new: quaternion.quaternion) -> quaternion.quaternion:
+def ensure_quat_continuity(
+    q_prev: quaternion.quaternion, q_new: quaternion.quaternion
+) -> quaternion.quaternion:
     """
     Ensures that consecutive quaternions to have consistent signs (due
     to the double coverage property of quaternions where q and -q represent
@@ -77,6 +78,7 @@ def ensure_quat_continuity(q_prev: quaternion.quaternion,
     else:
         return q_new
 
+
 # ========== Equatorial frame functions ============================
 
 
diff --git a/python/tests/test_imu_align.py b/python/tests/test_imu_align.py
index 7ea7ae0d9..b060dd94f 100644
--- a/python/tests/test_imu_align.py
+++ b/python/tests/test_imu_align.py
@@ -1,10 +1,13 @@
 import numpy as np
 import quaternion  # Note: numpy-quaternion convention: quaternion(w, x, y, z)
-from PiFinder.imu.imu_align.hand_eye_solver import solve_rotation, simulate_quaternion_measurements
+from PiFinder.imu.imu_align.hand_eye_solver import (
+    solve_rotation,
+    simulate_quaternion_measurements,
+)
 
 
 def test_simulate_quaternion_measurements():
-    N = 100  # Number of samples to simulate    
+    N = 100  # Number of samples to simulate
 
     # Set the true camera-from-body rotation
     true_rotvec = np.radians([10, -5, 20])
@@ -12,14 +15,18 @@ def test_simulate_quaternion_measurements():
 
     # Simulate measurements:
     q1, q2 = simulate_quaternion_measurements(
-        q_12_true, N=N, q1_noise_amp=np.deg2rad(0.1), 
-        q2_noise_amp=np.deg2rad(0.1), seed=0)
+        q_12_true,
+        N=N,
+        q1_noise_amp=np.deg2rad(0.1),
+        q2_noise_amp=np.deg2rad(0.1),
+        seed=0,
+    )
     assert len(q1) == N
     assert len(q2) == N
 
 
 def test_solve_rotation():
-    """ 
+    """
     The main block simulates pairs of q1 and q2 measurements and solves
     for the q_12 for the quaternion form of the hand-eye problem:
 
@@ -31,10 +38,14 @@ def test_solve_rotation():
 
     # Simulate measurements:
     q1, q2 = simulate_quaternion_measurements(
-        q_12_true, N=100, q1_noise_amp=np.deg2rad(0.1), 
-        q2_noise_amp=np.deg2rad(0.1), seed=0)
+        q_12_true,
+        N=100,
+        q1_noise_amp=np.deg2rad(0.1),
+        q2_noise_amp=np.deg2rad(0.1),
+        seed=0,
+    )
 
-    # Optional steps: 
+    # Optional steps:
     # Pair up and calculate relative rotations
     # Reject small rotations
 
@@ -50,9 +61,5 @@ def test_solve_rotation():
 
     # Error
     q_error = q_12_est.conjugate() * q_12_true
-    error_deg = np.rad2deg(
-        np.linalg.norm(
-            quaternion.as_rotation_vector(q_error)
-        )
-    )
-    #print(f"\nCalibration error: {error_deg:.6f} deg")
+    error_deg = np.rad2deg(np.linalg.norm(quaternion.as_rotation_vector(q_error)))
+    # print(f"\nCalibration error: {error_deg:.6f} deg")

From 9b2cb055581c246943954a66a7774d2e2b2a2a8d Mon Sep 17 00:00:00 2001
From: TakKanekoGit <>
Date: Sat, 22 Aug 2026 10:45:08 +0100
Subject: [PATCH 42/45] Fix type hint errors

---
 .../PiFinder/imu/imu_align/hand_eye_solver.py | 23 +++++++++++--------
 .../PiFinder/imu/imu_align/imu_alignment.py   | 10 ++++----
 2 files changed, 19 insertions(+), 14 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index 41249818f..118792017 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -40,7 +40,7 @@ class HandEyeSolverDiagnostics:
     residual_norms: np.ndarray  # Residual norms of each sample [rad]
     rotation_angles: np.ndarray  # Rotation angles of each sample [rad]
     sol_cov_matrix: np.ndarray  # Solution covariance matrix
-    sol_angle_error: np.ndarray  # Solution angle error [rad]
+    sol_angle_error: float  # Solution angle error [rad]
 
     # Optional
     meta_data: dict
@@ -77,10 +77,10 @@ def __init__(
             resid, axis=1
         )  # Residual per sample in radians
 
-        # Calculate rotations of each sample [rad]
-        self.rotation_angles = [
-            qt.get_quat_angular_diff(q1, q2) for q1, q2 in zip(q1_list, q2_list)
-        ]
+        # Calculate rotations of each sample-pair [rad]
+        self.rotation_angles = np.array(
+            [qt.get_quat_angular_diff(q1, q2) for q1, q2 in zip(q1_list, q2_list)]
+        )
 
         self.sol_cov_matrix, self.sol_angle_error = (
             self._calculate_solution_uncertainty(sample_timestamps)
@@ -122,7 +122,7 @@ def _calculate_solution_uncertainty(
         return sol_cov_matrix, sol_angle_error
 
     @staticmethod
-    def _calculate_connected_components(sample_timestamps: list_of_float_pairs) -> int:
+    def _calculate_connected_components(sample_timestamps: list_of_float_pairs) -> tuple[int, int]:
         """
         Returns the number of connected components in the sample_timestamps
         measurements. For example, if we have 5 measurement pairs (edges) from
@@ -284,9 +284,12 @@ def solve_rotation_with_outlier_removal(
     # Remove outliers
     q1_accepted = [q for ii, q in enumerate(q1_list) if msk_accept[ii]]
     q2_accepted = [q for ii, q in enumerate(q2_list) if msk_accept[ii]]
-    timestamps_accepted = [
-        t for ii, t in enumerate(sample_timestamps) if msk_accept[ii]
-    ]
+    if sample_timestamps is None:
+        timestamps_accepted = None
+    else:
+        timestamps_accepted = [
+            t for ii, t in enumerate(sample_timestamps) if msk_accept[ii]
+        ]
 
     # Solve again after outlier removal, using previous solution as the initial guess
     x0 = quaternion.as_rotation_vector(q12_solution)
@@ -390,7 +393,7 @@ def _random_quaternions(N: int, max_rot=None) -> list_of_quats:
     will be random. If specified, it limits the maximum swing angle from the
     previous orientation.
     """
-    qs = []
+    qs: list_of_quats = []
     for ii in range(N):
         axis = np.random.randn(3)
         axis /= np.linalg.norm(axis)
diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index 246b81abd..afb5dde07 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -75,6 +75,7 @@
 """
 
 from dataclasses import dataclass
+from typing import Any, Union
 
 import logging
 import numpy as np
@@ -84,6 +85,7 @@
 from PiFinder.types.coordinates import RaDecRoll
 from PiFinder.pointing_model import quaternion_transforms as qt
 from PiFinder.imu.imu_align.hand_eye_solver import solve_rotation_with_outlier_removal
+from PiFinder.imu.imu_align.hand_eye_solver import HandEyeSolverDiagnostics
 
 list_of_quats = list[quaternion.quaternion]
 
@@ -119,12 +121,12 @@ def len(self):
         """Number of samples in buffer"""
         return len(self.buffer)
 
-    def add_sample(self, sample: CameraImuSample):
+    def add_sample(self, sample: Any):
         if len(self.buffer) >= self.max_buffer_length:
             self.buffer.pop(0)  # Remove oldest sample from buffer
         self.buffer.append(sample)
 
-    def pop_sample(self, idx: int):
+    def pop_sample(self, idx: int) -> int:
         """Remove and return the sample at the given index"""
         return self.buffer.pop(idx)
 
@@ -185,7 +187,7 @@ def __init__(
 
     def add_candidate_attempt_solve(
         self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion
-    ):
+    ) -> tuple[Union[quaternion.quaternion, None], Union[HandEyeSolverDiagnostics, None]]:
         """
         For general use, call this pipeline method. Add a new candidate to the
         buffer. When the buffer fills up, pair samples and solve.
@@ -362,7 +364,7 @@ def _pair_samples(self) -> int:
 
         return n_pairs  # Number of successful pairings
 
-    def _solve(self, n_pairs=None):
+    def _solve(self, n_pairs=None) -> tuple[Union[quaternion.quaternion, None], HandEyeSolverDiagnostics]:
         """
         Solve for the alignment between the camera and IMU using at least the
         last n_pairs or all available pairs (if None) in diff_buffer.

From 7ce82e0045121ec3cba31d0f704f0facd274f683 Mon Sep 17 00:00:00 2001
From: TakKanekoGit <>
Date: Sat, 22 Aug 2026 11:08:07 +0100
Subject: [PATCH 43/45] Remove dead code

---
 .../PiFinder/imu/imu_align/hand_eye_solver.py | 46 +++----------------
 1 file changed, 6 insertions(+), 40 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index 118792017..01c7a824e 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -14,7 +14,6 @@
 from scipy.optimize import least_squares, OptimizeResult
 from scipy.sparse import csr_matrix
 from scipy.sparse.csgraph import connected_components
-import time
 from typing import Union
 
 import PiFinder.pointing_model.quaternion_transforms as qt
@@ -40,7 +39,7 @@ class HandEyeSolverDiagnostics:
     residual_norms: np.ndarray  # Residual norms of each sample [rad]
     rotation_angles: np.ndarray  # Rotation angles of each sample [rad]
     sol_cov_matrix: np.ndarray  # Solution covariance matrix
-    sol_angle_error: float  # Solution angle error [rad]
+    sol_angle_error: float  # Solution angle uncertainty [rad]
 
     # Optional
     meta_data: dict
@@ -93,6 +92,11 @@ def _calculate_solution_uncertainty(
         """
         Calculate the standard error of the solution:
         Cov = sigma ** 2 * inv(J.T @ J)
+
+        Alternative approach using SVD (more robust):
+        U, s, Vt = np.linalg.svd(result.jac, full_matrices=False)
+        cov_x = residuals_var * (Vt.T / s**2) @ Vt
+        condition_number = s[0] / s[-1]
         """
         # Extract Jacobian from least_squares result
         J = self.lsq_result.jac  # Jacobian matrix (m_meas, n_sol)
@@ -303,44 +307,6 @@ def solve_rotation_with_outlier_removal(
     return q12_solution_new, diagnostics_new
 
 
-def _solution_diagnostics(result):
-    """
-    Returns the diagnostics of the least-squares solution. The input,
-    `result` is the output from scipy.optimize.least_squares.
-
-    Condition number: < 10 excellent, < 100 acceptable, <1E4 weak observability
-
-    TODO: Remove?
-    """
-    t_start = time.time()
-
-    # Estimate the uncertainty of the solution
-    residuals = result.fun
-    dof = len(residuals) - len(
-        result.x
-    )  # Degrees-of-freedom = Number of meas - Number of params
-    residuals_var = np.sum(residuals**2) / dof  # Estimate of residual variance
-
-    # Using 'backslash' rather than inv(): Faster but could be unstable?
-    # JTJ = result.jac.T @ result.jac  # Hessian approx from the Jacobians
-    # cov_x = residuals_var * np.linalg.solve(JTJ, np.eye(JTJ.shape[0]))
-
-    # Estimate the uncertainty at the solution using SVD: More robust
-    U, s, Vt = np.linalg.svd(result.jac, full_matrices=False)
-    cov_x = residuals_var * (Vt.T / s**2) @ Vt
-    condition_number = s[0] / s[-1]
-    sigma_total = np.sqrt(np.trace(cov_x))  # [rad] Total rotaion uncertainty
-
-    t_compute = time.time() - t_start
-    print(
-        f"Diagnostics for q_cam2imu: compute time = {t_compute:.3f}s, ",
-        f"Total angular uncertainty = {np.rad2deg(sigma_total):.2} deg, ",
-        f"Condition number = {condition_number:.1g}",
-    )
-
-    return sigma_total, condition_number
-
-
 # ------- Helper functions -------
 
 

From 281861f0673db4dce93f9ecec8285b2a83853473 Mon Sep 17 00:00:00 2001
From: TakKanekoGit <>
Date: Sat, 22 Aug 2026 11:08:52 +0100
Subject: [PATCH 44/45] Update tests

---
 python/tests/test_imu_align.py | 32 +++++++++++++-------------------
 1 file changed, 13 insertions(+), 19 deletions(-)

diff --git a/python/tests/test_imu_align.py b/python/tests/test_imu_align.py
index b060dd94f..999f21678 100644
--- a/python/tests/test_imu_align.py
+++ b/python/tests/test_imu_align.py
@@ -3,6 +3,7 @@
 from PiFinder.imu.imu_align.hand_eye_solver import (
     solve_rotation,
     simulate_quaternion_measurements,
+    HandEyeSolverDiagnostics,
 )
 
 
@@ -33,33 +34,26 @@ def test_solve_rotation():
     q1 * q_12 = q_12 * q2
     """
     # Set the true camera-from-body rotation
-    true_rotvec = np.radians([10, -5, 20])
+    true_rotvec = np.array([1, 1, 1]) / np.sqrt(3) * np.deg2rad(30)
     q_12_true = quaternion.from_rotation_vector(true_rotvec)
 
     # Simulate measurements:
+    sigma = np.deg2rad(0.1)
     q1, q2 = simulate_quaternion_measurements(
         q_12_true,
         N=100,
-        q1_noise_amp=np.deg2rad(0.1),
-        q2_noise_amp=np.deg2rad(0.1),
+        q1_noise_amp=sigma,
+        q2_noise_amp=sigma,
         seed=0,
     )
 
-    # Optional steps:
-    # Pair up and calculate relative rotations
-    # Reject small rotations
-
     # solve
     q_12_est, diagnostics = solve_rotation(q1, q2)
-
-    # Results
-    print("\nTrue q_12:")
-    print(quaternion.as_float_array(q_12_true))
-
-    print("\nEstimated q_12_est:")
-    print(quaternion.as_float_array(q_12_est))
-
-    # Error
-    q_error = q_12_est.conjugate() * q_12_true
-    error_deg = np.rad2deg(np.linalg.norm(quaternion.as_rotation_vector(q_error)))
-    # print(f"\nCalibration error: {error_deg:.6f} deg")
+    assert isinstance(q_12_est, quaternion.quaternion)
+    assert isinstance(diagnostics, HandEyeSolverDiagnostics)
+
+    # Check error
+    #q_error = q_12_est.conjugate() * q_12_true
+    #error_rad = np.linalg.norm(quaternion.as_rotation_vector(q_error))
+    #print(f"Uncertainty: {np.rad2deg(diagnostics.sol_angle_error):.3f} degrees.")
+    #assert error_rad < np.deg2rad(1.0), f"error_rad too large. Got {error_rad:.3f} rad"

From 2b24c56d1e9ea23932557edab7f4d0cab1bcca8e Mon Sep 17 00:00:00 2001
From: TakKanekoGit <>
Date: Sat, 22 Aug 2026 11:11:19 +0100
Subject: [PATCH 45/45] Lint

---
 python/PiFinder/imu/imu_align/hand_eye_solver.py | 4 +++-
 python/PiFinder/imu/imu_align/imu_alignment.py   | 8 ++++++--
 python/tests/test_imu_align.py                   | 8 ++++----
 3 files changed, 13 insertions(+), 7 deletions(-)

diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py
index 01c7a824e..079fb1648 100644
--- a/python/PiFinder/imu/imu_align/hand_eye_solver.py
+++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py
@@ -126,7 +126,9 @@ def _calculate_solution_uncertainty(
         return sol_cov_matrix, sol_angle_error
 
     @staticmethod
-    def _calculate_connected_components(sample_timestamps: list_of_float_pairs) -> tuple[int, int]:
+    def _calculate_connected_components(
+        sample_timestamps: list_of_float_pairs,
+    ) -> tuple[int, int]:
         """
         Returns the number of connected components in the sample_timestamps
         measurements. For example, if we have 5 measurement pairs (edges) from
diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py
index afb5dde07..d8dc55c9c 100644
--- a/python/PiFinder/imu/imu_align/imu_alignment.py
+++ b/python/PiFinder/imu/imu_align/imu_alignment.py
@@ -187,7 +187,9 @@ def __init__(
 
     def add_candidate_attempt_solve(
         self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion
-    ) -> tuple[Union[quaternion.quaternion, None], Union[HandEyeSolverDiagnostics, None]]:
+    ) -> tuple[
+        Union[quaternion.quaternion, None], Union[HandEyeSolverDiagnostics, None]
+    ]:
         """
         For general use, call this pipeline method. Add a new candidate to the
         buffer. When the buffer fills up, pair samples and solve.
@@ -364,7 +366,9 @@ def _pair_samples(self) -> int:
 
         return n_pairs  # Number of successful pairings
 
-    def _solve(self, n_pairs=None) -> tuple[Union[quaternion.quaternion, None], HandEyeSolverDiagnostics]:
+    def _solve(
+        self, n_pairs=None
+    ) -> tuple[Union[quaternion.quaternion, None], HandEyeSolverDiagnostics]:
         """
         Solve for the alignment between the camera and IMU using at least the
         last n_pairs or all available pairs (if None) in diff_buffer.
diff --git a/python/tests/test_imu_align.py b/python/tests/test_imu_align.py
index 999f21678..d9c3ce6f0 100644
--- a/python/tests/test_imu_align.py
+++ b/python/tests/test_imu_align.py
@@ -53,7 +53,7 @@ def test_solve_rotation():
     assert isinstance(diagnostics, HandEyeSolverDiagnostics)
 
     # Check error
-    #q_error = q_12_est.conjugate() * q_12_true
-    #error_rad = np.linalg.norm(quaternion.as_rotation_vector(q_error))
-    #print(f"Uncertainty: {np.rad2deg(diagnostics.sol_angle_error):.3f} degrees.")
-    #assert error_rad < np.deg2rad(1.0), f"error_rad too large. Got {error_rad:.3f} rad"
+    # q_error = q_12_est.conjugate() * q_12_true
+    # error_rad = np.linalg.norm(quaternion.as_rotation_vector(q_error))
+    # print(f"Uncertainty: {np.rad2deg(diagnostics.sol_angle_error):.3f} degrees.")
+    # assert error_rad < np.deg2rad(1.0), f"error_rad too large. Got {error_rad:.3f} rad"