重构RTK-IMU标定链路并完成机械先验工程验证

imu_lidar/rtk_attitude.py

@@ -1,4 +1,4 @@
-"""GNHPR attitude conventions and SO(3) interpolation."""
+"""GNHPR dual-antenna baseline conventions and SO(3) interpolation."""
 
 from __future__ import annotations
 
@@ -9,12 +9,14 @@ from scipy.spatial.transform import Rotation, Slerp
 
 @dataclass(frozen=True)
 class GnhprConvention:
-    """Interpretation of GNHPR angles as ``R_ENU_RTK``.
+    """Interpretation of the GNHPR ANT1-to-ANT2 baseline.
 
-    Heading is normally clockwise from north. With ENU and an x-forward RTK
-    frame this becomes yaw ``90 deg - heading``. Aircraft-positive pitch is
-    nose-up, which is the negative mathematical Y rotation in an FLU frame.
-    Alternative signs are retained for empirical protocol validation.
+    Heading is clockwise from north. In this vehicle the RTK ``+X`` axis is
+    the baseline from the main/left antenna (ANT1) to the secondary/right
+    antenna (ANT2), so it points to vehicle right rather than vehicle forward.
+    GNHPR pitch is the elevation of that baseline. A two-antenna receiver
+    cannot observe rotation about the baseline; the reported roll is therefore
+    not treated as a third independent attitude measurement.
     """
 
     name: str
@@ -23,7 +25,7 @@ class GnhprConvention:
     roll_sign: float = 1.0
 
 
-EXPECTED_GNHPR = GnhprConvention("north_cw__pitch_nose_up__roll_right_down")
+EXPECTED_GNHPR = GnhprConvention("ant1_to_ant2__north_cw__elevation_up")
 GNHPR_CANDIDATES = (
     EXPECTED_GNHPR,
     GnhprConvention("north_cw__pitch_opposite", -1.0, 1.0, 1.0),
@@ -38,7 +40,12 @@ def gnhpr_to_rotation_enu_rtk(
     roll_deg: np.ndarray,
     convention: GnhprConvention = EXPECTED_GNHPR,
 ) -> np.ndarray:
-    """Build body-to-ENU matrices with an extrinsic Z-Y-X Euler sequence."""
+    """Build a zero-roll mathematical completion of the baseline frame.
+
+    Only the first column (the ANT1-to-ANT2 unit vector) is physically observed
+    by GNHPR. The remaining columns are a convenient gauge completion and must
+    not be used as a measured full vehicle attitude.
+    """
 
     heading = np.asarray(heading_deg, dtype=float).reshape(-1)
     pitch = np.asarray(pitch_deg, dtype=float).reshape(-1)
@@ -47,11 +54,36 @@ def gnhpr_to_rotation_enu_rtk(
         raise ValueError("heading, pitch and roll must have equal length")
     yaw_rad = np.deg2rad(90.0 + convention.heading_sign * heading)
     pitch_rad = np.deg2rad(convention.pitch_sign * pitch)
-    roll_rad = np.deg2rad(convention.roll_sign * roll)
+    # A dual-antenna baseline has no independent roll observation. Keep the
+    # argument for wire-format compatibility, but never inject it into SO(3).
+    roll_rad = np.zeros_like(roll)
     angles = np.column_stack([yaw_rad, pitch_rad, roll_rad])
     return Rotation.from_euler('ZYX', angles).as_matrix()
 
 
+def gnhpr_to_baseline_enu(
+    heading_deg: np.ndarray,
+    pitch_deg: np.ndarray,
+    convention: GnhprConvention = EXPECTED_GNHPR,
+) -> np.ndarray:
+    """Return ANT1-to-ANT2 unit vectors expressed in ENU.
+
+    The result is independent of the unobservable GNHPR roll field.
+    """
+
+    heading = np.asarray(heading_deg, dtype=float).reshape(-1)
+    pitch = np.asarray(pitch_deg, dtype=float).reshape(-1)
+    if heading.size != pitch.size:
+        raise ValueError("heading and pitch must have equal length")
+    azimuth = np.deg2rad(heading)
+    elevation = np.deg2rad(-convention.pitch_sign * pitch)
+    horizontal = np.cos(elevation)
+    east = np.sin(azimuth) * horizontal
+    north = np.cos(azimuth) * horizontal
+    up = np.sin(elevation)
+    return np.column_stack([east, north, up])
+
+
 def interpolate_rotations(
     source_t_s: np.ndarray,
     rotations: np.ndarray,

imu_lidar/rtk_imu_multisource.py

@@ -0,0 +1,558 @@
+"""Observable RTK--IMU rotation stages using native asynchronous measurements."""
+
+from __future__ import annotations
+
+import csv
+import json
+from dataclasses import asdict, dataclass
+from pathlib import Path
+
+import numpy as np
+from scipy.optimize import least_squares
+from scipy.spatial.transform import Rotation
+
+from .contracts import ImuSeries
+from .geometry import so3_exp, so3_log
+from .imu_preintegration import apply_bias_jacobian_correction, preintegrate_gyro
+from .rtk_attitude import gnhpr_to_baseline_enu
+
+
+@dataclass(frozen=True)
+class UnifiedSession:
+    session_id: str
+    batch_id: str
+    imu: ImuSeries
+    imu_rpy_deg: np.ndarray
+    imu_quaternion_wxyz: np.ndarray
+    imu_host_receive_utc_s: np.ndarray
+    rtk_by_type: dict[str, list[dict[str, str]]]
+
+
+@dataclass(frozen=True)
+class R1bResult:
+    baseline_axis_imu: np.ndarray
+    tilt_yz_deg: np.ndarray
+    pair_count: int
+    residual_rms_deg: float
+    residual_p95_deg: float
+    covariance_deg2: np.ndarray
+    std_deg: np.ndarray
+    information_singular_values: np.ndarray
+    per_session_rms_deg: dict[str, float]
+    gyro_bias_by_session_rad_s: dict[str, np.ndarray]
+    ok: bool
+    notes: tuple[str, ...]
+
+
+@dataclass(frozen=True)
+class CompletedRotationResult:
+    method: str
+    R_RTK_IMU: np.ndarray | None
+    rpy_deg: np.ndarray | None
+    sample_count: int
+    session_count: int
+    residual_rms_deg: float
+    residual_p95_deg: float
+    covariance_deg2: np.ndarray
+    std_deg: np.ndarray
+    per_session_rms_deg: dict[str, float]
+    leave_one_session_delta_deg: dict[str, float]
+    block_out_delta_deg: dict[str, float]
+    convention: str
+    ok: bool
+    notes: tuple[str, ...]
+
+
+@dataclass(frozen=True)
+class R3Result:
+    r1b: R1bResult
+    r2v: CompletedRotationResult
+    r2g: CompletedRotationResult
+    r2v_r2g_delta_deg: float
+    full_rotation_accepted: bool
+    translation_unlocked: bool
+    blockers: tuple[str, ...]
+
+
+@dataclass(frozen=True)
+class _BaselinePair:
+    session_index: int
+    session_id: str
+    world_angle_rad: float
+    delta_R: np.ndarray
+    J_bg: np.ndarray
+
+
+def _f(row: dict[str, str], key: str, default: float = np.nan) -> float:
+    try:
+        return float(row.get(key, ""))
+    except (TypeError, ValueError):
+        return default
+
+
+def _truth(row: dict[str, str], key: str) -> bool:
+    return str(row.get(key, "")).strip().lower() in {"1", "true", "yes"}
+
+
+def load_unified_sessions(
+    manifest_path: Path | str,
+    *,
+    selected_session_ids: set[str] | None = None,
+) -> list[UnifiedSession]:
+    manifest_source = Path(manifest_path)
+    manifest = json.loads(manifest_source.read_text(encoding="utf-8"))
+    sessions: list[UnifiedSession] = []
+    for entry in manifest["sessions"]:
+        session_id = str(entry["session_id"])
+        if selected_session_ids and session_id not in selected_session_ids:
+            continue
+        directory = Path(entry["directory"])
+        if not directory.is_absolute():
+            directory = manifest_source.parent / directory
+        with np.load(directory / "imu.npz") as payload:
+            t = np.asarray(payload["system_time_s"], dtype=float)
+            gyro = np.asarray(payload["gyro_rad_s"], dtype=float)
+            accel = np.asarray(payload["accel_m_s2"], dtype=float)
+            rpy = np.asarray(payload["rpy_deg"], dtype=float)
+            quaternion = np.asarray(payload["quaternion_wxyz"], dtype=float)
+            host = np.asarray(payload["host_receive_utc_s"], dtype=float)
+        by_type: dict[str, list[dict[str, str]]] = {}
+        with (directory / "rtk.csv").open("r", encoding="utf-8", newline="") as stream:
+            for row in csv.DictReader(stream):
+                by_type.setdefault(row["message_type"], []).append(row)
+        sessions.append(
+            UnifiedSession(
+                session_id=session_id,
+                batch_id=str(entry["batch_id"]),
+                imu=ImuSeries(t_s=t, gyro_rad_s=gyro, acc_m_s2=accel),
+                imu_rpy_deg=rpy,
+                imu_quaternion_wxyz=quaternion,
+                imu_host_receive_utc_s=host,
+                rtk_by_type=by_type,
+            )
+        )
+    return sessions
+
+
+def _valid_hpr(session: UnifiedSession) -> tuple[np.ndarray, np.ndarray]:
+    rows = [
+        row for row in session.rtk_by_type.get("GNHPR", [])
+        if _truth(row, "checksum_valid") and int(_f(row, "heading_quality", -1)) == 4
+    ]
+    if not rows:
+        return np.zeros(0), np.zeros((0, 3))
+    t = np.asarray([_f(row, "t_device_s") for row in rows])
+    baseline = gnhpr_to_baseline_enu(
+        np.asarray([_f(row, "heading_deg") for row in rows]),
+        np.asarray([_f(row, "pitch_deg") for row in rows]),
+    )
+    finite = np.isfinite(t) & np.all(np.isfinite(baseline), axis=1)
+    t, baseline = t[finite], baseline[finite]
+    order = np.argsort(t)
+    t, baseline = t[order], baseline[order]
+    unique, indices = np.unique(t, return_index=True)
+    return unique, baseline[indices]
+
+
+def _baseline_pairs(sessions: list[UnifiedSession]) -> list[_BaselinePair]:
+    pairs: list[_BaselinePair] = []
+    for session_index, session in enumerate(sessions):
+        t, baseline = _valid_hpr(session)
+        if t.size < 3:
+            continue
+        dt = np.diff(t)
+        jump = np.degrees(
+            np.arccos(np.clip(np.sum(baseline[:-1] * baseline[1:], axis=1), -1.0, 1.0))
+        )
+        continuous = (dt >= 0.03) & (dt <= 0.25) & (jump / np.maximum(dt, 1e-6) <= 45.0)
+        last_anchor = -np.inf
+        for index, t0 in enumerate(t[:-1]):
+            if t0 - last_anchor < 1.0:
+                continue
+            last_anchor = t0
+            for duration in (0.75, 1.5, 3.0):
+                target = t0 + duration
+                end = int(np.searchsorted(t, target))
+                candidates = [candidate for candidate in (end - 1, end) if index < candidate < t.size]
+                if not candidates:
+                    continue
+                j = min(candidates, key=lambda candidate: abs(t[candidate] - target))
+                if abs((t[j] - t0) - duration) > 0.12 or not np.all(continuous[index:j]):
+                    continue
+                try:
+                    pre = preintegrate_gyro(
+                        session.imu.t_s, session.imu.gyro_rad_s, float(t0), float(t[j])
+                    )
+                except ValueError:
+                    continue
+                world_angle = float(
+                    np.arccos(np.clip(np.dot(baseline[index], baseline[j]), -1.0, 1.0))
+                )
+                if np.degrees(world_angle) < 0.4:
+                    continue
+                pairs.append(
+                    _BaselinePair(
+                        session_index=session_index,
+                        session_id=session.session_id,
+                        world_angle_rad=world_angle,
+                        delta_R=pre.delta_R,
+                        J_bg=pre.J_bg,
+                    )
+                )
+    return pairs
+
+
+def _axis_from_parameters(parameters: np.ndarray) -> np.ndarray:
+    axis = np.asarray([1.0, parameters[0], parameters[1]], dtype=float)
+    return axis / np.linalg.norm(axis)
+
+
+def solve_r1b(sessions: list[UnifiedSession]) -> R1bResult:
+    pairs = _baseline_pairs(sessions)
+    if len(pairs) < 20:
+        raise ValueError("R1b needs at least 20 continuous baseline/gyro motion pairs")
+    session_count = len(sessions)
+    pair_counts = {
+        session.session_id: sum(pair.session_id == session.session_id for pair in pairs)
+        for session in sessions
+    }
+    active_sessions = sum(count > 0 for count in pair_counts.values())
+    target_count = len(pairs) / max(active_sessions, 1)
+
+    def residual(parameters: np.ndarray) -> np.ndarray:
+        axis = _axis_from_parameters(parameters[:2])
+        biases = parameters[2:].reshape(session_count, 3)
+        values = []
+        for pair in pairs:
+            corrected = apply_bias_jacobian_correction(
+                pair.delta_R, pair.J_bg, biases[pair.session_index]
+            )
+            body_angle = np.arccos(
+                np.clip(np.dot(axis, corrected @ axis), -1.0, 1.0)
+            )
+            weight = np.sqrt(target_count / pair_counts[pair.session_id])
+            values.append(weight * (body_angle - pair.world_angle_rad))
+        values.extend((biases / 0.01).reshape(-1))
+        # Weak 20-degree installation prior selects the physically known +X hemisphere.
+        values.extend(np.asarray(parameters[:2]) / np.tan(np.deg2rad(20.0)))
+        return np.asarray(values)
+
+    initial = np.zeros(2 + 3 * session_count)
+    optimum = least_squares(
+        residual, initial, loss="huber", f_scale=np.deg2rad(0.25), max_nfev=120
+    )
+    axis = _axis_from_parameters(optimum.x[:2])
+    biases = optimum.x[2:].reshape(session_count, 3)
+    errors = []
+    per_session_values: dict[str, list[float]] = {}
+    for pair in pairs:
+        corrected = apply_bias_jacobian_correction(
+            pair.delta_R, pair.J_bg, biases[pair.session_index]
+        )
+        body_angle = np.arccos(np.clip(np.dot(axis, corrected @ axis), -1.0, 1.0))
+        error = float(np.degrees(body_angle - pair.world_angle_rad))
+        errors.append(error)
+        per_session_values.setdefault(pair.session_id, []).append(error)
+    errors_array = np.asarray(errors)
+    data_rows = len(pairs)
+    jacobian = optimum.jac[:data_rows, :2]
+    information = jacobian.T @ jacobian
+    residual_variance = float(np.mean(np.deg2rad(errors_array) ** 2))
+    covariance = residual_variance * np.linalg.pinv(information, rcond=1e-12)
+    covariance_deg2 = np.degrees(1.0) ** 2 * covariance
+    std_deg = np.sqrt(np.maximum(np.diag(covariance_deg2), 0.0))
+    singular = np.linalg.svd(information, compute_uv=False)
+    rms = float(np.sqrt(np.mean(errors_array**2)))
+    p95 = float(np.percentile(np.abs(errors_array), 95.0))
+    return R1bResult(
+        baseline_axis_imu=axis,
+        tilt_yz_deg=np.degrees(np.arctan(optimum.x[:2])),
+        pair_count=len(pairs),
+        residual_rms_deg=rms,
+        residual_p95_deg=p95,
+        covariance_deg2=covariance_deg2,
+        std_deg=std_deg,
+        information_singular_values=singular,
+        per_session_rms_deg={
+            key: float(np.sqrt(np.mean(np.asarray(value) ** 2)))
+            for key, value in per_session_values.items()
+        },
+        gyro_bias_by_session_rad_s={
+            session.session_id: biases[index] for index, session in enumerate(sessions)
+        },
+        ok=bool(rms <= 1.0 and p95 <= 2.0 and np.min(singular) >= 1e-3),
+        notes=(
+            "2DoF ANT1-to-ANT2 direction; +X hemisphere selected by installation knowledge",
+            "weak 20 deg prior is reported and prevents sign/gauge branch switching",
+        ),
+    )
+
+
+def _rotation_mean(matrices: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
+    mean = Rotation.from_matrix(matrices).mean().as_matrix()
+    errors = np.asarray(
+        [np.degrees(np.linalg.norm(so3_log(mean.T @ matrix))) for matrix in matrices]
+    )
+    return mean, errors
+
+
+def _result_from_samples(
+    method: str,
+    samples: list[tuple[str, str, np.ndarray]],
+    convention: str,
+    notes: tuple[str, ...],
+) -> CompletedRotationResult:
+    if not samples:
+        return CompletedRotationResult(
+            method, None, None, 0, 0, np.nan, np.nan, np.full((3, 3), np.nan),
+            np.full(3, np.nan),
+            {}, {}, {}, convention, False, notes + ("no qualifying samples",)
+        )
+    matrices = np.asarray([item[2] for item in samples])
+    mean, errors = _rotation_mean(matrices)
+    rotvec = np.asarray([so3_log(mean.T @ matrix) for matrix in matrices])
+    rotvec_deg = np.degrees(rotvec)
+    std = np.std(rotvec_deg, axis=0, ddof=1) if len(samples) > 1 else np.full(3, np.nan)
+    covariance = (
+        np.cov(rotvec_deg, rowvar=False, ddof=1) / len(samples)
+        if len(samples) > 1 else np.full((3, 3), np.nan)
+    )
+    ids = sorted({item[0] for item in samples})
+    per_session = {}
+    loo = {}
+    for session_id in ids:
+        selected = [item[2] for item in samples if item[0] == session_id]
+        _, local_errors = _rotation_mean(np.asarray(selected))
+        per_session[session_id] = float(np.sqrt(np.mean(local_errors**2)))
+        kept = np.asarray([item[2] for item in samples if item[0] != session_id])
+        if kept.size:
+            kept_mean, _ = _rotation_mean(kept)
+            loo[session_id] = float(np.degrees(np.linalg.norm(so3_log(mean.T @ kept_mean))))
+    block_groups = sorted({(item[0], item[1]) for item in samples})
+    block_out = {}
+    for session_id, block_id in block_groups:
+        kept = np.asarray(
+            [item[2] for item in samples if (item[0], item[1]) != (session_id, block_id)]
+        )
+        if kept.size:
+            kept_mean, _ = _rotation_mean(kept)
+            block_out[f"{session_id}:{block_id}"] = float(
+                np.degrees(np.linalg.norm(so3_log(mean.T @ kept_mean)))
+            )
+    rms = float(np.sqrt(np.mean(errors**2)))
+    p95 = float(np.percentile(errors, 95.0))
+    max_loo = max(loo.values(), default=np.inf)
+    ok = bool(
+        len(samples) >= 10 and len(ids) >= 2 and rms <= 2.0 and p95 <= 3.0
+        and np.nanmax(std) <= 1.0 and max_loo <= 1.0
+    )
+    return CompletedRotationResult(
+        method=method,
+        R_RTK_IMU=mean,
+        rpy_deg=Rotation.from_matrix(mean).as_euler("xyz", degrees=True),
+        sample_count=len(samples),
+        session_count=len(ids),
+        residual_rms_deg=rms,
+        residual_p95_deg=p95,
+        covariance_deg2=covariance,
+        std_deg=std,
+        per_session_rms_deg=per_session,
+        leave_one_session_delta_deg=loo,
+        block_out_delta_deg=block_out,
+        convention=convention,
+        ok=ok,
+        notes=notes,
+    )
+
+
+def solve_r2g(
+    sessions: list[UnifiedSession],
+    r1b: R1bResult,
+    *,
+    level_static_session_ids: set[str],
+    block_duration_s: float = 10.0,
+) -> CompletedRotationResult:
+    samples: list[tuple[str, str, np.ndarray]] = []
+    right = r1b.baseline_axis_imu
+    for session in sessions:
+        if session.session_id not in level_static_session_ids:
+            continue
+        gyro_norm = np.linalg.norm(session.imu.gyro_rad_s, axis=1)
+        accel_norm = np.linalg.norm(session.imu.acc_m_s2, axis=1)
+        valid = (gyro_norm <= np.deg2rad(0.35)) & (np.abs(accel_norm - 9.80665) <= 0.15)
+        block = np.floor(
+            (session.imu.t_s - session.imu.t_s[0]) / block_duration_s
+        ).astype(int)
+        for block_id in np.unique(block[valid]):
+            selected = valid & (block == block_id)
+            if np.count_nonzero(selected) < 200:
+                continue
+            up = np.median(session.imu.acc_m_s2[selected], axis=0)
+            up /= np.linalg.norm(up)
+            up -= right * np.dot(up, right)
+            if np.linalg.norm(up) < 0.9:
+                continue
+            up /= np.linalg.norm(up)
+            forward = np.cross(up, right)
+            forward /= np.linalg.norm(forward)
+            C_IMU_RTK = np.column_stack([right, forward, up])
+            samples.append((session.session_id, str(int(block_id)), C_IMU_RTK.T))
+    return _result_from_samples(
+        "R2G_baseline_plus_level_gravity",
+        samples,
+        "accelerometer specific-force points vehicle up on explicit level-static blocks",
+        (
+            "only caller-declared level-static sessions are eligible",
+            "result is level/gravity-prior constrained, not dual-antenna-only",
+        ),
+    )
+
+
+def _nearest_index(t: np.ndarray, value: float, tolerance: float) -> int | None:
+    index = int(np.searchsorted(t, value))
+    candidates = [item for item in (index - 1, index) if 0 <= item < t.size]
+    if not candidates:
+        return None
+    best = min(candidates, key=lambda item: abs(t[item] - value))
+    return best if abs(t[best] - value) <= tolerance else None
+
+
+def solve_r2v(
+    sessions: list[UnifiedSession],
+    *,
+    min_speed_m_s: float = 1.5,
+    max_yaw_rate_deg_s: float = 3.0,
+    max_baseline_course_error_deg: float = 15.0,
+) -> CompletedRotationResult:
+    candidates: dict[str, list[tuple[str, str, np.ndarray]]] = {
+        "HI13_q_body_to_ENU": [],
+        "NED_to_ENU_times_HI13_q": [],
+        "HI13_q_inverse_as_body_to_ENU": [],
+        "NED_to_ENU_times_HI13_q_inverse": [],
+    }
+    NED_TO_ENU = np.asarray([[0.0, 1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, -1.0]])
+    for session in sessions:
+        hpr_t, hpr_baseline = _valid_hpr(session)
+        if hpr_t.size == 0:
+            continue
+        quaternion = session.imu_quaternion_wxyz
+        norm = np.linalg.norm(quaternion, axis=1)
+        valid_quaternion = np.isfinite(norm) & (np.abs(norm - 1.0) <= 0.02)
+        normalized = quaternion / np.maximum(norm[:, None], 1e-12)
+        imu_rotations = Rotation.from_quat(normalized[:, [1, 2, 3, 0]]).as_matrix()
+        for row_index, row in enumerate(session.rtk_by_type.get("BESTNAVA", [])):
+            if not (
+                _truth(row, "checksum_valid")
+                and _truth(row, "position_fixed")
+                and _truth(row, "doppler_velocity_valid")
+                and _f(row, "horizontal_speed_m_s") >= min_speed_m_s
+                and _f(row, "horizontal_speed_std_m_s") <= 0.25
+            ):
+                continue
+            t = _f(row, "t_device_s")
+            hpr_index = _nearest_index(hpr_t, t, 0.15)
+            imu_index = _nearest_index(session.imu.t_s, t, 0.03)
+            if hpr_index is None or imu_index is None or not valid_quaternion[imu_index]:
+                continue
+            if abs(np.degrees(session.imu.gyro_rad_s[imu_index, 2])) > max_yaw_rate_deg_s:
+                continue
+            right = hpr_baseline[hpr_index]
+            forward = np.asarray(
+                [_f(row, "velocity_east_m_s"), _f(row, "velocity_north_m_s"), 0.0]
+            )
+            forward /= np.linalg.norm(forward)
+            course_error = np.degrees(
+                np.arcsin(np.clip(abs(np.dot(right, forward)), 0.0, 1.0))
+            )
+            if course_error > max_baseline_course_error_deg:
+                continue
+            forward -= right * np.dot(right, forward)
+            forward /= np.linalg.norm(forward)
+            up = np.cross(right, forward)
+            if up[2] < 0:
+                forward = -forward
+                up = -up
+            up /= np.linalg.norm(up)
+            R_ENU_RTK = np.column_stack([right, forward, up])
+            q = imu_rotations[imu_index]
+            world_candidates = {
+                "HI13_q_body_to_ENU": q,
+                "NED_to_ENU_times_HI13_q": NED_TO_ENU @ q,
+                "HI13_q_inverse_as_body_to_ENU": q.T,
+                "NED_to_ENU_times_HI13_q_inverse": NED_TO_ENU @ q.T,
+            }
+            block_id = str(row_index // 10)
+            for name, R_ENU_IMU in world_candidates.items():
+                candidates[name].append(
+                    (session.session_id, block_id, R_ENU_RTK.T @ R_ENU_IMU)
+                )
+    diagnostics = {
+        name: _result_from_samples(
+            "R2V_baseline_plus_doppler_velocity",
+            values,
+            name,
+            (
+                "RTK fixed + Doppler velocity + speed + low-yaw + baseline/course gates",
+                "HI13 absolute quaternion may contain magnetic/navigation yaw bias",
+            ),
+        )
+        for name, values in candidates.items()
+    }
+    finite = [result for result in diagnostics.values() if result.sample_count]
+    if not finite:
+        return diagnostics["HI13_q_body_to_ENU"]
+    return min(finite, key=lambda result: result.residual_rms_deg)
+
+
+def solve_r3(
+    sessions: list[UnifiedSession],
+    *,
+    level_static_session_ids: set[str],
+) -> R3Result:
+    dynamic_sessions = [
+        session for session in sessions if session.session_id not in level_static_session_ids
+    ]
+    r1b = solve_r1b(dynamic_sessions)
+    r2v = solve_r2v(dynamic_sessions)
+    r2g = solve_r2g(sessions, r1b, level_static_session_ids=level_static_session_ids)
+    if r2v.R_RTK_IMU is None or r2g.R_RTK_IMU is None:
+        delta = np.nan
+    else:
+        delta = float(
+            np.degrees(np.linalg.norm(so3_log(r2v.R_RTK_IMU.T @ r2g.R_RTK_IMU)))
+        )
+    blockers = []
+    if not r1b.ok:
+        blockers.append("R1b baseline direction failed residual/observability gates")
+    if not r2v.ok:
+        blockers.append("R2V velocity-completed rotation failed stability gates")
+    if not r2g.ok:
+        blockers.append("R2G gravity-completed rotation failed stability gates")
+    if not np.isfinite(delta) or delta > 2.0:
+        blockers.append("R2V and R2G disagree by more than 2 deg")
+    accepted = not blockers
+    return R3Result(
+        r1b=r1b,
+        r2v=r2v,
+        r2g=r2g,
+        r2v_r2g_delta_deg=delta,
+        full_rotation_accepted=accepted,
+        translation_unlocked=accepted,
+        blockers=tuple(blockers),
+    )
+
+
+def result_to_jsonable(result: R3Result) -> dict:
+    def convert(value):
+        if isinstance(value, np.ndarray):
+            return value.tolist()
+        if isinstance(value, np.generic):
+            return value.item()
+        if hasattr(value, "__dataclass_fields__"):
+            return {key: convert(item) for key, item in asdict(value).items()}
+        if isinstance(value, dict):
+            return {str(key): convert(item) for key, item in value.items()}
+        if isinstance(value, (tuple, list)):
+            return [convert(item) for item in value]
+        return value
+    return convert(result)

imu_lidar/rtk_imu_node_graph.py

@@ -0,0 +1,529 @@
+'''Per-GNSS-node RTK/IMU state graph used after legacy propagation deprecation.'''
+from __future__ import annotations
+from dataclasses import dataclass
+import numpy as np
+from scipy.optimize import least_squares
+from scipy.sparse import lil_matrix
+from .geometry import so3_exp, so3_log
+from .imu_preintegration import apply_bias_correction_imu, residual_whiten_matrix
+from .rtk_imu_engineering import (
+    G_ENU, HPR_DIRECT_ANGULAR_SIGMA_RAD, _Segment, _world_rtk)
+
+NODE_DOF = 15
+SIGMA_BG_RW = 1e-5
+SIGMA_BA_RW = 1e-3
+
+@dataclass(frozen=True)
+class NodeGraphProblem:
+    segment: _Segment
+    R_seed_WI: tuple[np.ndarray,...]
+    fixed_l_I_m: np.ndarray
+    R_RTK_IMU: np.ndarray
+    hpr_direct_angular_sigma_rad: float = HPR_DIRECT_ANGULAR_SIGMA_RAD
+
+@dataclass(frozen=True)
+class NodeGraphResult:
+    success: bool
+    message: str
+    node_count: int
+    duration_s: float
+    fixed_l_I_m: np.ndarray
+    initial_cost: float
+    final_cost: float
+    cost_reduction: float
+    nfev: int
+    optimality: float
+    gradient_norm: float
+    residual_dimension: int
+    state_dimension: int
+    statistical_dof: int
+    total_nis: float
+    chi_square_per_dof: float
+    cost_per_dof: float
+    initial_residual_by_factor: dict[str,dict[str,float]]
+    final_residual_by_factor: dict[str,dict[str,float]]
+    final_position_residual_m: dict[str,object]
+    final_velocity_residual_m_s: dict[str,object]
+    max_bg_step_rad_s: float
+    max_ba_step_m_s2: float
+    preintegration_covariance_sigma: dict[str,dict[str,float]]
+
+
+@dataclass(frozen=True)
+class FreeLeverResult:
+    success: bool
+    message: str
+    initial_l_I_m: np.ndarray
+    final_l_I_m: np.ndarray
+    lever_step_norm_m: float
+    initial_cost: float
+    final_cost: float
+    nfev: int
+    optimality: float
+    chi_square_per_dof: float
+    position_residual_m: dict[str,object]
+    velocity_residual_m_s: dict[str,object]
+    residual_by_factor: dict[str,dict[str,float]]
+    lever_covariance_m2: np.ndarray
+    lever_information_singular_values: np.ndarray
+    lever_information_condition_number: float
+    lever_precision_rank: int
+    weakest_lever_direction_I: np.ndarray
+
+def _rotation(problem,index,x):
+    offset = NODE_DOF*index
+    return problem.R_seed_WI[index] @ so3_exp(x[offset:offset+3])
+
+def initial_parameters(problem, lever_override=None):
+    nodes = problem.segment.nodes
+    x = np.zeros(NODE_DOF*len(nodes))
+    lever = (problem.fixed_l_I_m if lever_override is None
+             else np.asarray(lever_override,dtype=float))
+    previous_p, previous_v = np.zeros(3), np.zeros(3)
+    for index,node in enumerate(nodes):
+        offset = NODE_DOF*index
+        R = problem.R_seed_WI[index]
+        previous_p = node.p_enu_m-R@lever
+        if index and not node.position_mask[2]:
+            previous_p[2] = x[offset-NODE_DOF+5]
+        if node.velocity_enu_m_s is not None:
+            previous_v = node.velocity_enu_m_s-R@np.cross(node.gyro_rad_s,lever)
+        x[offset+3:offset+6] = previous_p
+        x[offset+6:offset+9] = previous_v
+    return x
+
+def _stats(values, effective_dof=None):
+    a = np.asarray(values,dtype=float).reshape(-1)
+    if not a.size:
+        return {'count':0,'dof':0,'rms':np.nan,'p50_abs':np.nan,
+                'p95_abs':np.nan,'p99_abs':np.nan,'nis':np.nan,
+                'chi_square_per_dof':np.nan}
+    nis = float(np.dot(a,a))
+    dof = int(a.size if effective_dof is None else effective_dof)
+    return {'count':int(a.size),'dof':dof,
+            'rms':float(np.sqrt(np.mean(a*a))),
+            'p50_abs':float(np.percentile(np.abs(a),50.)),
+            'p95_abs':float(np.percentile(np.abs(a),95.)),
+            'p99_abs':float(np.percentile(np.abs(a),99.)),
+            'nis':nis,'chi_square_per_dof':nis/max(dof,1)}
+
+def _hpr_sigma(problem, node):
+    old = float(node.hpr_angular_sigma_rad)
+    extra_var = max(old*old-HPR_DIRECT_ANGULAR_SIGMA_RAD**2, 0.)
+    return float(np.sqrt(problem.hpr_direct_angular_sigma_rad**2+extra_var))
+
+
+def _distribution(values):
+    a = np.asarray(values,dtype=float).reshape(-1)
+    if not a.size:
+        return {'count':0,'rms':np.nan,'p50':np.nan,'p95':np.nan,'p99':np.nan}
+    return {'count':len(a),'rms':float(np.sqrt(np.mean(a*a))),
+            'p50':float(np.percentile(a,50.)),'p95':float(np.percentile(a,95.)),
+            'p99':float(np.percentile(a,99.))}
+
+def _vector_stats(values):
+    a = np.asarray(values,dtype=float).reshape(-1,3)
+    if not a.size:
+        return {'count':0,'axis_rms':[np.nan]*3,'axis_p95_abs':[np.nan]*3,
+                'vector_rms':np.nan,'vector_p95':np.nan}
+    norm = np.linalg.norm(a,axis=1)
+    return {'count':len(a),'axis_rms':np.sqrt(np.mean(a*a,axis=0)),
+            'axis_p95_abs':np.percentile(np.abs(a),95.,axis=0),
+            'vector_rms':float(np.sqrt(np.mean(norm*norm))),
+            'vector_p95':float(np.percentile(norm,95.))}
+
+def residual(problem,x,details=None,dependencies=None,lever_override=None):
+    nodes, preints = problem.segment.nodes, problem.segment.preintegrations
+    lever = (problem.fixed_l_I_m if lever_override is None
+             else np.asarray(lever_override,dtype=float))
+    baseline_I = problem.R_RTK_IMU.T[:,0]
+    values = []
+    def add(value,label,node_indices,raw=None):
+        a = np.asarray(value,dtype=float).reshape(-1)
+        values.extend(a)
+        if dependencies is not None:
+            dependencies.extend([tuple(node_indices)]*len(a))
+        if details is not None:
+            details.setdefault(label,[]).extend(a.tolist())
+            if raw is not None: details.setdefault(label+'_physical',[]).append(np.asarray(raw))
+    for index,node in enumerate(nodes):
+        offset = NODE_DOF*index
+        R = _rotation(problem,index,x)
+        p, v = x[offset+3:offset+6], x[offset+6:offset+9]
+        bg, ba = x[offset+9:offset+12], x[offset+12:offset+15]
+        p_error = p+R@lever-node.p_enu_m
+        if node.source == 'GGA':
+            add(p_error[:2]/.06,'gga_xy',(index,))
+            if details is not None: details.setdefault('position_physical',[]).append(
+                np.array([p_error[0],p_error[1],np.nan]))
+        else:
+            add(p_error/np.array([.06,.06,.12]),'best_position',(index,),p_error)
+            if details is not None: details.setdefault('position_physical',[]).append(p_error)
+        if node.velocity_enu_m_s is not None:
+            v_error = v+R@np.cross(node.gyro_rad_s-bg,lever)-node.velocity_enu_m_s
+            add(v_error/np.array([.15,.15,.30]),'doppler',(index,),v_error)
+            if details is not None: details.setdefault('velocity_physical',[]).append(v_error)
+        if node.hpr_factor_valid:
+            hpr_error=np.cross(R@baseline_I,node.baseline_enu)
+            add(hpr_error/_hpr_sigma(problem,node),'hpr',(index,),hpr_error)
+        if node.gravity_candidate:
+            gravity = node.accel_m_s2-ba-R.T@(-G_ENU)
+            add(gravity/.12,'gravity',(index,))
+        if index == len(nodes)-1: continue
+        right = index+1
+        right_offset = NODE_DOF*right
+        Rj = _rotation(problem,right,x)
+        pj = x[right_offset+3:right_offset+6]
+        vj = x[right_offset+6:right_offset+9]
+        bgj = x[right_offset+9:right_offset+12]
+        baj = x[right_offset+12:right_offset+15]
+        pre = preints[index]
+        dR,dv,dp = apply_bias_correction_imu(pre,bg,ba)
+        dt = pre.duration_s
+        imu_error = np.concatenate([
+            so3_log(dR.T@R.T@Rj),
+            R.T@(vj-v-G_ENU*dt)-dv,
+            R.T@(pj-p-v*dt-.5*G_ENU*dt*dt)-dp])
+        add(residual_whiten_matrix(pre.cov)@imu_error,'imu_preintegration',
+            (index,right),imu_error)
+        add((bgj-bg)/(SIGMA_BG_RW*np.sqrt(dt)),'gyro_bias_random_walk',(index,right))
+        add((baj-ba)/(SIGMA_BA_RW*np.sqrt(dt)),'accel_bias_random_walk',(index,right))
+    add(x[:3]/np.deg2rad(5.),'initial_attitude_gauge',(0,))
+    add(x[9:12]/.02,'initial_gyro_bias',(0,))
+    add(x[12:15]/.5,'initial_accel_bias',(0,))
+    return np.asarray(values)
+
+def jacobian_sparsity(problem,x):
+    dependencies = []
+    base = residual(problem,x,dependencies=dependencies)
+    sparsity = lil_matrix((len(base),len(x)),dtype=int)
+    for row,node_indices in enumerate(dependencies):
+        for index in node_indices:
+            start = NODE_DOF*index
+            sparsity[row,start:start+NODE_DOF] = 1
+    return sparsity.tocsr()
+
+def _factor_stats(details):
+    return {key:_stats(value,2*len(value)//3 if key=='hpr' else None)
+            for key,value in details.items()
+            if not key.endswith('_physical') and key not in ('position_physical','velocity_physical')}
+
+
+def _effective_residual_dimension(details):
+    return sum(2*len(v)//3 if k=='hpr' else len(v)
+               for k,v in details.items() if not k.endswith('_physical')
+               and k not in ('position_physical','velocity_physical'))
+
+def _preintegration_covariance_stats(problem):
+    blocks = {'rotation_rad':[],'velocity_m_s':[],'position_m':[]}
+    for pre in problem.segment.preintegrations:
+        sigma = np.sqrt(np.maximum(np.diag(pre.cov),0.))
+        blocks['rotation_rad'].extend(sigma[:3])
+        blocks['velocity_m_s'].extend(sigma[3:6])
+        blocks['position_m'].extend(sigma[6:9])
+    return {key:_distribution(value) for key,value in blocks.items()}
+
+def build_problem(segment,R_RTK_IMU,fixed_l_I_m,
+                  hpr_direct_angular_sigma_rad=HPR_DIRECT_ANGULAR_SIGMA_RAD):
+    seeds = []
+    for index,node in enumerate(segment.nodes):
+        if node.hpr_factor_valid:
+            seeds.append(_world_rtk(node.baseline_enu)@R_RTK_IMU)
+        elif index:
+            seeds.append(seeds[-1]@segment.preintegrations[index-1].delta_R)
+        else:
+            seeds.append(segment.R_WRTK_initial@R_RTK_IMU)
+    return NodeGraphProblem(segment,tuple(seeds),np.asarray(fixed_l_I_m,dtype=float),
+                            np.asarray(R_RTK_IMU,dtype=float),
+                            float(hpr_direct_angular_sigma_rad))
+
+def solve_fixed_lever(problem,max_nfev=30):
+    x0 = initial_parameters(problem)
+    initial_detail = {}
+    r0 = residual(problem,x0,initial_detail)
+    fit = least_squares(
+        lambda value:residual(problem,value),x0,jac='2-point',
+        jac_sparsity=jacobian_sparsity(problem,x0),method='trf',
+        tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
+        x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
+    final_detail = {}
+    rf = residual(problem,fit.x,final_detail)
+    statistical_dof = max(_effective_residual_dimension(final_detail)-len(fit.x),1)
+    bg = fit.x.reshape(-1,NODE_DOF)[:,9:12]
+    ba = fit.x.reshape(-1,NODE_DOF)[:,12:15]
+    bg_step = np.diff(bg,axis=0)
+    ba_step = np.diff(ba,axis=0)
+    return NodeGraphResult(
+        success=bool(fit.success),message=str(fit.message),
+        node_count=len(problem.segment.nodes),
+        duration_s=problem.segment.nodes[-1].t_s-problem.segment.nodes[0].t_s,
+        fixed_l_I_m=problem.fixed_l_I_m.copy(),
+        initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
+        cost_reduction=.5*float(np.dot(r0,r0)-np.dot(rf,rf)),
+        nfev=int(fit.nfev),optimality=float(fit.optimality),
+        gradient_norm=float(np.linalg.norm(fit.grad)),
+        residual_dimension=len(rf),state_dimension=len(fit.x),
+        statistical_dof=statistical_dof,total_nis=float(np.dot(rf,rf)),
+        chi_square_per_dof=float(np.dot(rf,rf)/statistical_dof),
+        cost_per_dof=.5*float(np.dot(rf,rf)/statistical_dof),
+        initial_residual_by_factor=_factor_stats(initial_detail),
+        final_residual_by_factor=_factor_stats(final_detail),
+        final_position_residual_m=_vector_stats(final_detail.get('position_physical',[])),
+        final_velocity_residual_m_s=_vector_stats(final_detail.get('velocity_physical',[])),
+        max_bg_step_rad_s=float(np.max(np.linalg.norm(bg_step,axis=1))) if len(bg_step) else 0.,
+        max_ba_step_m_s2=float(np.max(np.linalg.norm(ba_step,axis=1))) if len(ba_step) else 0.,
+        preintegration_covariance_sigma=_preintegration_covariance_stats(problem))
+
+
+def fit_states_at_fixed_lever(problem,l_I_m,max_nfev=50,initial_state_values=None):
+    lever=np.asarray(l_I_m,dtype=float)
+    x0=(initial_parameters(problem,lever) if initial_state_values is None
+        else np.asarray(initial_state_values,dtype=float))
+    r0=residual(problem,x0,lever_override=lever)
+    fit=least_squares(
+        lambda value:residual(problem,value,lever_override=lever),x0,jac='2-point',
+        jac_sparsity=jacobian_sparsity(problem,x0),method='trf',tr_solver='lsmr',
+        loss='linear',max_nfev=max_nfev,x_scale='jac',
+        ftol=1e-6,xtol=1e-6,gtol=1e-6)
+    return fit.x,{'success':bool(fit.success),'message':str(fit.message),
+                  'nfev':int(fit.nfev),'initial_cost':.5*float(r0@r0),
+                  'cost':float(fit.cost)}
+
+
+def summarize_fixed_state_values(problems,state_values,l_I_m):
+    details={}; residuals=[]
+    for problem,value in zip(problems,state_values):
+        local={}
+        residuals.append(residual(problem,np.asarray(value),details=local,
+                                  lever_override=l_I_m))
+        for key,items in local.items(): details.setdefault(key,[]).extend(items)
+    joined=np.concatenate(residuals)
+    state_dimension=sum(len(value) for value in state_values)
+    dof=max(_effective_residual_dimension(details)-state_dimension,1)
+    return {'cost':.5*float(np.dot(joined,joined)),
+        'total_nis':float(np.dot(joined,joined)),
+        'chi_square_per_dof':float(np.dot(joined,joined)/dof),
+        'statistical_dof':dof,'residual_by_factor':_factor_stats(details),
+        'best_position_physical_m':_vector_stats(details.get('best_position_physical',[])),
+        'doppler_physical_m_s':_vector_stats(details.get('doppler_physical',[])),
+        'hpr_physical_rad':_vector_stats(details.get('hpr_physical',[]))}
+
+def solve_fixed_lever_many(problems,max_nfev=30):
+    problems = tuple(problems)
+    sizes = [NODE_DOF*len(problem.segment.nodes) for problem in problems]
+    offsets = np.cumsum([0,*sizes])
+    x0 = np.concatenate([initial_parameters(problem) for problem in problems])
+    def evaluate(value,details=None):
+        chunks = []
+        for index,problem in enumerate(problems):
+            local_details = {} if details is not None else None
+            chunks.append(residual(problem,value[offsets[index]:offsets[index+1]],
+                                   local_details))
+            if details is not None:
+                for key,items in local_details.items():
+                    details.setdefault(key,[]).extend(items)
+        return np.concatenate(chunks)
+    initial_detail = {}
+    r0 = evaluate(x0,initial_detail)
+    sparsity = lil_matrix((len(r0),len(x0)),dtype=int)
+    row = 0
+    for index,problem in enumerate(problems):
+        local_x = x0[offsets[index]:offsets[index+1]]
+        local = jacobian_sparsity(problem,local_x)
+        sparsity[row:row+local.shape[0],offsets[index]:offsets[index+1]] = local
+        row += local.shape[0]
+    fit = least_squares(
+        lambda value:evaluate(value),x0,jac='2-point',jac_sparsity=sparsity.tocsr(),
+        method='trf',tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
+        x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
+    final_detail = {}
+    rf = evaluate(fit.x,final_detail)
+    bg_steps, ba_steps = [], []
+    for index,problem in enumerate(problems):
+        states = fit.x[offsets[index]:offsets[index+1]].reshape(-1,NODE_DOF)
+        bg_steps.extend(np.linalg.norm(np.diff(states[:,9:12],axis=0),axis=1))
+        ba_steps.extend(np.linalg.norm(np.diff(states[:,12:15],axis=0),axis=1))
+    covariance = {'rotation_rad':[],'velocity_m_s':[],'position_m':[]}
+    for problem in problems:
+        for pre in problem.segment.preintegrations:
+            sigma = np.sqrt(np.maximum(np.diag(pre.cov),0.))
+            covariance['rotation_rad'].extend(sigma[:3])
+            covariance['velocity_m_s'].extend(sigma[3:6])
+            covariance['position_m'].extend(sigma[6:9])
+    dof = max(_effective_residual_dimension(final_detail)-len(fit.x),1)
+    return NodeGraphResult(
+        success=bool(fit.success),message=str(fit.message),
+        node_count=sum(len(problem.segment.nodes) for problem in problems),
+        duration_s=sum(problem.segment.nodes[-1].t_s-problem.segment.nodes[0].t_s
+                       for problem in problems),
+        fixed_l_I_m=problems[0].fixed_l_I_m.copy(),
+        initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
+        cost_reduction=.5*float(np.dot(r0,r0)-np.dot(rf,rf)),
+        nfev=int(fit.nfev),optimality=float(fit.optimality),
+        gradient_norm=float(np.linalg.norm(fit.grad)),
+        residual_dimension=len(rf),state_dimension=len(fit.x),
+        statistical_dof=dof,total_nis=float(np.dot(rf,rf)),
+        chi_square_per_dof=float(np.dot(rf,rf)/dof),
+        cost_per_dof=.5*float(np.dot(rf,rf)/dof),
+        initial_residual_by_factor=_factor_stats(initial_detail),
+        final_residual_by_factor=_factor_stats(final_detail),
+        final_position_residual_m=_vector_stats(final_detail.get('position_physical',[])),
+        final_velocity_residual_m_s=_vector_stats(final_detail.get('velocity_physical',[])),
+        max_bg_step_rad_s=float(max(bg_steps,default=0.)),
+        max_ba_step_m_s2=float(max(ba_steps,default=0.)),
+        preintegration_covariance_sigma={key:_distribution(value) for key,value in covariance.items()})
+
+
+def _free_residual(problem,value,details=None):
+    return residual(problem,value[3:],details=details,lever_override=value[:3])
+
+
+def _free_sparsity(problem,value):
+    local = jacobian_sparsity(problem,value[3:])
+    result = lil_matrix((local.shape[0],local.shape[1]+3),dtype=int)
+    result[:,:3] = 1
+    result[:,3:] = local
+    return result.tocsr()
+
+
+def _marginal_lever_information(jacobian):
+    J = jacobian.toarray() if hasattr(jacobian,'toarray') else np.asarray(jacobian)
+    H = J.T@J
+    Hll,Hln,Hnn = H[:3,:3],H[:3,3:],H[3:,3:]
+    marginal = Hll-Hln@np.linalg.pinv(Hnn,rcond=1e-10)@Hln.T
+    return .5*(marginal+marginal.T)
+
+
+def _additive_marginal_lever_information(jacobian,row_offsets,state_offsets):
+    total=np.zeros((3,3))
+    for index in range(len(row_offsets)-1):
+        rows=slice(row_offsets[index],row_offsets[index+1])
+        columns=np.r_[0:3,state_offsets[index]:state_offsets[index+1]]
+        local=jacobian[rows,:][:,columns]
+        total+=_marginal_lever_information(local)
+    return .5*(total+total.T)
+
+
+def linearized_lever_information(problem,l_I_m):
+    lever=np.asarray(l_I_m,dtype=float)
+    value=np.concatenate([lever,initial_parameters(problem,lever)])
+    fit=least_squares(lambda x:_free_residual(problem,x),value,jac='2-point',
+        jac_sparsity=_free_sparsity(problem,value),method='trf',tr_solver='lsmr',
+        loss='linear',max_nfev=1,x_scale='jac')
+    information=_marginal_lever_information(fit.jac)
+    _,singular,Vt=np.linalg.svd(information)
+    covariance=np.linalg.pinv(information,rcond=1e-9)
+    return information,covariance,singular,Vt[-1]
+
+
+def solve_free_lever(problem,initial_l_I_m,max_nfev=120):
+    initial_l = np.asarray(initial_l_I_m,dtype=float)
+    x0 = np.concatenate([initial_l,initial_parameters(problem,initial_l)])
+    r0 = _free_residual(problem,x0)
+    fit = least_squares(
+        lambda value:_free_residual(problem,value),x0,jac='2-point',
+        jac_sparsity=_free_sparsity(problem,x0),method='trf',tr_solver='lsmr',
+        loss='linear',max_nfev=max_nfev,x_scale='jac',
+        ftol=1e-6,xtol=1e-6,gtol=1e-6)
+    detail = {}
+    rf = _free_residual(problem,fit.x,detail)
+    information = _marginal_lever_information(fit.jac)
+    _,singular_values,Vt = np.linalg.svd(information)
+    tolerance = max(singular_values[0]*1e-9,1e-10)
+    rank = int(np.sum(singular_values>tolerance))
+    covariance = np.linalg.pinv(information,rcond=1e-9)
+    dof = max(_effective_residual_dimension(detail)-len(fit.x),1)
+    condition = (float(singular_values[0]/singular_values[-1])
+                 if singular_values[-1]>tolerance else np.inf)
+    return FreeLeverResult(
+        success=bool(fit.success),message=str(fit.message),
+        initial_l_I_m=initial_l,final_l_I_m=fit.x[:3].copy(),
+        lever_step_norm_m=float(np.linalg.norm(fit.x[:3]-initial_l)),
+        initial_cost=.5*float(np.dot(r0,r0)),
+        final_cost=.5*float(np.dot(rf,rf)),nfev=int(fit.nfev),
+        optimality=float(fit.optimality),chi_square_per_dof=float(np.dot(rf,rf)/dof),
+        position_residual_m=_vector_stats(detail.get('position_physical',[])),
+        velocity_residual_m_s=_vector_stats(detail.get('velocity_physical',[])),
+        residual_by_factor=_factor_stats(detail),lever_covariance_m2=covariance,
+        lever_information_singular_values=singular_values,
+        lever_information_condition_number=condition,lever_precision_rank=rank,
+        weakest_lever_direction_I=Vt[-1].copy())
+
+
+def solve_free_lever_many(problems,initial_l_I_m,max_nfev=120,
+                          initial_state_values=None,lever_prior_mean_m=None,
+                          lever_prior_covariance_m2=None,return_state_values=False):
+    problems=tuple(problems)
+    initial_l=np.asarray(initial_l_I_m,dtype=float)
+    sizes=[NODE_DOF*len(problem.segment.nodes) for problem in problems]
+    offsets=np.cumsum([3,*sizes])
+    states=([initial_parameters(problem,initial_l) for problem in problems]
+            if initial_state_values is None else
+            [np.asarray(value,dtype=float) for value in initial_state_values])
+    prior_mean=(None if lever_prior_mean_m is None else
+                np.asarray(lever_prior_mean_m,dtype=float))
+    prior_cov=(None if lever_prior_covariance_m2 is None else
+               np.asarray(lever_prior_covariance_m2,dtype=float))
+    prior_whitener=(None if prior_cov is None else
+                    np.linalg.inv(np.linalg.cholesky(prior_cov)))
+    x0=np.concatenate([initial_l,*states])
+    def evaluate(value,details=None):
+        chunks=[]
+        for index,problem in enumerate(problems):
+            local={} if details is not None else None
+            chunks.append(residual(problem,value[offsets[index]:offsets[index+1]],
+                                   details=local,lever_override=value[:3]))
+            if details is not None:
+                for key,items in local.items(): details.setdefault(key,[]).extend(items)
+        if prior_whitener is not None:
+            prior_error=prior_whitener@(value[:3]-prior_mean)
+            chunks.append(prior_error)
+            if details is not None:
+                details.setdefault('lever_prior',[]).extend(prior_error.tolist())
+        return np.concatenate(chunks)
+    r0=evaluate(x0)
+    sparsity=lil_matrix((len(r0),len(x0)),dtype=int)
+    row=0; row_offsets=[0]
+    for index,problem in enumerate(problems):
+        local=jacobian_sparsity(problem,states[index])
+        sparsity[row:row+local.shape[0],:3]=1
+        sparsity[row:row+local.shape[0],offsets[index]:offsets[index+1]]=local
+        row+=local.shape[0]
+        row_offsets.append(row)
+    if prior_whitener is not None:
+        sparsity[row:row+3,:3]=1
+    fit=least_squares(
+        lambda value:evaluate(value),x0,jac='2-point',jac_sparsity=sparsity.tocsr(),
+        method='trf',tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
+        x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
+    detail={}
+    rf=evaluate(fit.x,detail)
+    information=_additive_marginal_lever_information(
+        fit.jac,row_offsets,offsets)
+    if prior_cov is not None:
+        information+=np.linalg.inv(prior_cov)
+    _,singular_values,Vt=np.linalg.svd(information)
+    tolerance=max(singular_values[0]*1e-9,1e-10)
+    rank=int(np.sum(singular_values>tolerance))
+    covariance=np.linalg.pinv(information,rcond=1e-9)
+    dof=max(_effective_residual_dimension(detail)-len(fit.x),1)
+    condition=(float(singular_values[0]/singular_values[-1])
+               if singular_values[-1]>tolerance else np.inf)
+    result=FreeLeverResult(
+        success=bool(fit.success),message=str(fit.message),
+        initial_l_I_m=initial_l,final_l_I_m=fit.x[:3].copy(),
+        lever_step_norm_m=float(np.linalg.norm(fit.x[:3]-initial_l)),
+        initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
+        nfev=int(fit.nfev),optimality=float(fit.optimality),
+        chi_square_per_dof=float(np.dot(rf,rf)/dof),
+        position_residual_m=_vector_stats(detail.get('position_physical',[])),
+        velocity_residual_m_s=_vector_stats(detail.get('velocity_physical',[])),
+        residual_by_factor=_factor_stats(detail),lever_covariance_m2=covariance,
+        lever_information_singular_values=singular_values,
+        lever_information_condition_number=condition,lever_precision_rank=rank,
+        weakest_lever_direction_I=Vt[-1].copy())
+    if return_state_values:
+        states=[fit.x[offsets[i]:offsets[i+1]].copy()
+                for i in range(len(problems))]
+        return result,states
+    return result

imu_lidar/rtk_imu_replay.py

@@ -16,6 +16,15 @@ from .rtk_imu_translation import TranslationCalibrationResult, solve_rtk_imu_tra
 from .rtk_io import load_rtk_csv
 
 
+DEFAULT_RTK_FRAME_DEFINITION = (
+    "right-handed vehicle-fixed frame: +X ANT1(main,left)->ANT2(secondary,right), "
+    "+Y vehicle forward/IMU +Y, +Z vehicle up/IMU +Z"
+)
+DEFAULT_RTK_REFERENCE_POINT = (
+    "GGA ANT1/main-antenna phase center, 1.916499878 m above ground"
+)
+
+
 @dataclass(frozen=True)
 class InventoryEntry:
     session_id: str
@@ -82,6 +91,7 @@ def dataset_audit(sessions: list[RotationSession]) -> dict[str, Any]:
         rtk = session.rtk
         valid_position = rtk.position_valid
         valid_attitude = rtk.attitude_valid & valid_position
+        float_attitude = rtk.attitude_float & valid_position
         rows.append(
             {
                 "session_id": session.session_id,
@@ -90,6 +100,8 @@ def dataset_audit(sessions: list[RotationSession]) -> dict[str, Any]:
                 "rtk_samples": int(rtk.t_s.size),
                 "fixed_position_ratio": float(np.mean(valid_position)),
                 "fixed_attitude_ratio": float(np.mean(valid_attitude)),
+                "float_attitude_ratio": float(np.mean(float_attitude)),
+                "checksum_valid_ratio": float(np.mean(rtk.checksum_valid)),
                 "common_time_span_s": [
                     float(max(session.imu.t_s[0], rtk.t_s[0])),
                     float(min(session.imu.t_s[-1], rtk.t_s[-1])),
@@ -109,8 +121,8 @@ def run_calibration(
     rotation_only: bool = False,
     compute_loo: bool = True,
     knot_step_s: float = 2.0,
-    rtk_frame_definition: str = '',
-    rtk_reference_point: str = '',
+    rtk_frame_definition: str = DEFAULT_RTK_FRAME_DEFINITION,
+    rtk_reference_point: str = DEFAULT_RTK_REFERENCE_POINT,
 ) -> tuple[RotationCalibrationResult, TranslationCalibrationResult | None]:
     """Run calibration and publish human-readable JSON artifacts."""
 
@@ -118,7 +130,7 @@ def run_calibration(
     output.mkdir(parents=True, exist_ok=True)
     rotation = solve_rtk_imu_rotation(sessions, compute_loo=compute_loo)
     translation = None
-    if not rotation_only:
+    if not rotation_only and rotation.ok:
         translation = solve_rtk_imu_translation(
             sessions,
             rotation,
@@ -144,9 +156,9 @@ def run_calibration(
     )
     blockers = []
     if not rotation.ok:
-        blockers.append('rotation quality gates failed')
+        blockers.append('full RTK-to-IMU rotation is not observable from the lateral dual-antenna baseline')
     if translation is None or not translation.ok:
-        blockers.append('translation quality gates failed or were not run')
+        blockers.append('translation is frozen until a full rotation is observable and accepted')
     if not rtk_frame_definition.strip():
         blockers.append('RTK frame_definition is empty')
     if not rtk_reference_point.strip():

imu_lidar/rtk_imu_rotation.py

@@ -2,7 +2,7 @@
 
 from __future__ import annotations
 
-from dataclasses import dataclass
+from dataclasses import dataclass, replace
 
 import numpy as np
 from scipy.optimize import least_squares
@@ -31,6 +31,28 @@ class TimeOffsetAudit:
     second_best_correlation: float
     evaluated_samples: int
     reliable: bool
+    method: str
+    peak_width_s: tuple[float, float]
+    per_session_offset_s: dict[str, float]
+    per_session_peak_correlation: dict[str, float]
+
+
+@dataclass(frozen=True)
+class BaselineConsistencyAudit:
+    """Two-DOF audit using only the physically observed ANT1-to-ANT2 axis."""
+
+    baseline_axis_imu: np.ndarray
+    pair_count: int
+    residual_rms_deg: float
+    residual_median_deg: float
+    residual_p95_deg: float
+    per_session_rms_deg: dict[str, float]
+    per_session_p95_deg: dict[str, float]
+    per_session_axis_rms_deg: dict[str, np.ndarray]
+    gyro_bias_by_session_rad_s: dict[str, np.ndarray]
+    worst_pairs: tuple[dict[str, object], ...]
+    ok: bool
+    notes: tuple[str, ...]
 
 
 @dataclass(frozen=True)
@@ -50,6 +72,10 @@ class RotationCalibrationResult:
     information_singular_values: np.ndarray
     per_session_rms_deg: dict[str, float]
     loo_delta_deg: dict[str, float]
+    baseline_consistency: BaselineConsistencyAudit
+    observable_rotation_dof: int
+    full_attitude_observable: bool
+    legacy_full_attitude_numeric_ok: bool
     ok: bool
     notes: tuple[str, ...]
 
@@ -62,6 +88,8 @@ class _Pair:
     delta_R_zero_bias: np.ndarray
     J_bg: np.ndarray
     weight: float
+    t0_s: float
+    t1_s: float
 
 
 def _attitude_rows(rtk: RtkSeries, convention: GnhprConvention) -> tuple[np.ndarray, np.ndarray]:
@@ -91,14 +119,25 @@ def _attitude_rows(rtk: RtkSeries, convention: GnhprConvention) -> tuple[np.ndar
     return unique_t, rotations
 
 
-def _rtk_angular_speed(t: np.ndarray, rotations: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
+def _rtk_heading_rate(t: np.ndarray, rotations: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
+    """Return signed vehicle yaw rate from the ANT1-to-ANT2 azimuth.
+
+    ANT1-to-ANT2 points vehicle-right, so its clockwise heading increases when
+    mathematical body yaw decreases. Only this signed heading channel is
+    compared with IMU gyro_z; rotation about the baseline is unobservable.
+    """
+
     dt = np.diff(t)
-    valid = (dt >= 0.03) & (dt <= 0.5)
-    midpoint = 0.5 * (t[:-1] + t[1:])
-    speed = np.array(
-        [np.linalg.norm(so3_log(rotations[i].T @ rotations[i + 1])) for i in range(t.size - 1)]
-    ) / np.maximum(dt, 1e-6)
-    return midpoint[valid], speed[valid]
+    baseline = rotations[:, :, 0]
+    heading = np.unwrap(np.arctan2(baseline[:, 0], baseline[:, 1]))
+    rate = -np.diff(heading) / np.maximum(dt, 1e-6)
+    valid = (
+        (dt >= 0.03)
+        & (dt <= 0.25)
+        & (np.abs(rate) >= np.deg2rad(0.5))
+        & (np.abs(rate) <= np.deg2rad(30.0))
+    )
+    return 0.5 * (t[:-1] + t[1:])[valid], rate[valid]
 
 
 def _correlation(a: np.ndarray, b: np.ndarray) -> float:
@@ -115,45 +154,102 @@ def audit_time_offset(
     search_half_width_s: float = 0.30,
     step_s: float = 0.005,
 ) -> TimeOffsetAudit:
-    """Estimate residual ``t_IMU - t_RTK`` from invariant angular-speed norms."""
+    """Audit residual t_IMU - t_RTK from signed heading rate.
+
+    A broad correlation peak remains diagnostic. It is never applied unless
+    both peak separation and peak width pass.
+    """
 
     offsets = np.arange(-search_half_width_s, search_half_width_s + 0.5 * step_s, step_s)
-    session_series: list[tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]] = []
+    session_series: list[tuple[str, np.ndarray, np.ndarray, np.ndarray, np.ndarray]] = []
     total_samples = 0
     for session in sessions:
         t_rtk, rotations = _attitude_rows(session.rtk, GNHPR_CANDIDATES[0])
-        midpoint, rtk_speed = _rtk_angular_speed(t_rtk, rotations)
-        imu_speed = np.linalg.norm(session.imu.gyro_rad_s, axis=1)
-        motion = rtk_speed > np.deg2rad(0.5)
-        midpoint = midpoint[motion]
-        rtk_speed = rtk_speed[motion]
+        midpoint, rtk_rate = _rtk_heading_rate(t_rtk, rotations)
+        imu_rate = session.imu.gyro_rad_s[:, 2]
         if midpoint.size >= 20:
-            session_series.append((midpoint, rtk_speed, session.imu.t_s, imu_speed))
+            session_series.append(
+                (session.session_id, midpoint, rtk_rate, session.imu.t_s, imu_rate)
+            )
             total_samples += int(midpoint.size)
     if not session_series:
-        return TimeOffsetAudit(0.0, np.nan, np.nan, 0, False)
+        return TimeOffsetAudit(
+            0.0,
+            np.nan,
+            np.nan,
+            0,
+            False,
+            "signed_heading_rate_vs_imu_gyro_z",
+            (np.nan, np.nan),
+            {},
+            {},
+        )
     scores = []
+    per_session_scores: dict[str, list[float]] = {
+        session_id: [] for session_id, *_ in session_series
+    }
     for offset in offsets:
         per_session = []
-        for midpoint, rtk_speed, imu_t, imu_speed in session_series:
+        for session_id, midpoint, rtk_rate, imu_t, imu_rate in session_series:
             query = midpoint + offset
             inside = (query >= imu_t[0]) & (query <= imu_t[-1])
             if np.count_nonzero(inside) < 20:
+                per_session_scores[session_id].append(np.nan)
                 continue
-            interpolated = np.interp(query[inside], imu_t, imu_speed)
-            value = _correlation(rtk_speed[inside], interpolated)
+            interpolated = np.interp(query[inside], imu_t, imu_rate)
+            value = _correlation(rtk_rate[inside], interpolated)
+            per_session_scores[session_id].append(value)
             if np.isfinite(value):
                 per_session.append(value)
         scores.append(float(np.median(per_session)) if per_session else np.nan)
     values = np.asarray(scores, dtype=float)
     if not np.any(np.isfinite(values)):
-        return TimeOffsetAudit(0.0, np.nan, np.nan, total_samples, False)
+        return TimeOffsetAudit(
+            0.0,
+            np.nan,
+            np.nan,
+            total_samples,
+            False,
+            "signed_heading_rate_vs_imu_gyro_z",
+            (np.nan, np.nan),
+            {},
+            {},
+        )
     best_index = int(np.nanargmax(values))
     exclusion = np.abs(offsets - offsets[best_index]) >= 0.03
     second = float(np.nanmax(values[exclusion])) if np.any(np.isfinite(values[exclusion])) else np.nan
     peak = float(values[best_index])
-    reliable = bool(peak >= 0.35 and (not np.isfinite(second) or peak - second >= 0.015))
-    return TimeOffsetAudit(float(offsets[best_index]), peak, second, total_samples, reliable)
+    near_peak = np.flatnonzero(values >= peak - 0.005)
+    peak_width = (
+        (float(offsets[near_peak[0]]), float(offsets[near_peak[-1]]))
+        if near_peak.size
+        else (np.nan, np.nan)
+    )
+    per_session_offset = {}
+    per_session_peak = {}
+    for session_id, session_values in per_session_scores.items():
+        array = np.asarray(session_values, dtype=float)
+        if np.any(np.isfinite(array)):
+            index = int(np.nanargmax(array))
+            per_session_offset[session_id] = float(offsets[index])
+            per_session_peak[session_id] = float(array[index])
+    reliable = bool(
+        peak >= 0.5
+        and (not np.isfinite(second) or peak - second >= 0.015)
+        and np.isfinite(peak_width[0])
+        and peak_width[1] - peak_width[0] <= 0.03
+    )
+    return TimeOffsetAudit(
+        float(offsets[best_index]),
+        peak,
+        second,
+        total_samples,
+        reliable,
+        "signed_heading_rate_vs_imu_gyro_z",
+        peak_width,
+        per_session_offset,
+        per_session_peak,
+    )
 
 
 def _nearest_index(times: np.ndarray, target: float) -> int:
@@ -175,6 +271,17 @@ def _make_pairs(
     cache = {} if preintegration_cache is None else preintegration_cache
     for session_index, session in enumerate(sessions):
         t, rotations = _attitude_rows(session.rtk, convention)
+        dt = np.diff(t)
+        baseline = rotations[:, :, 0]
+        baseline_step = np.arccos(
+            np.clip(np.sum(baseline[:-1] * baseline[1:], axis=1), -1.0, 1.0)
+        )
+        broken_edge = (
+            (dt < 0.03)
+            | (dt > 0.25)
+            | (baseline_step / np.maximum(dt, 1e-6) > np.deg2rad(45.0))
+        )
+        broken_prefix = np.concatenate([[0], np.cumsum(broken_edge.astype(int))])
         next_anchor = float(t[0])
         for i in range(t.size - 1):
             if t[i] + 1e-9 < next_anchor:
@@ -184,6 +291,8 @@ def _make_pairs(
                 j = _nearest_index(t, float(t[i] + duration))
                 if j <= i or abs(float(t[j] - t[i]) - duration) > 0.18:
                     continue
+                if broken_prefix[j] - broken_prefix[i] != 0:
+                    continue
                 imu_t0 = float(t[i] + time_offset_s)
                 imu_t1 = float(t[j] + time_offset_s)
                 if imu_t0 < session.imu.t_s[0] or imu_t1 > session.imu.t_s[-1]:
@@ -212,9 +321,25 @@ def _make_pairs(
                         delta_R_zero_bias=preint.delta_R,
                         J_bg=preint.J_bg,
                         weight=weight,
+                        t0_s=float(t[i]),
+                        t1_s=float(t[j]),
                     )
                 )
-    return pairs
+    # Equalize total influence per session. Pair count and excitation otherwise
+    # let long/high-motion sessions dominate the shared rotation.
+    totals = {
+        session.session_id: sum(
+            pair.weight for pair in pairs if pair.session_id == session.session_id
+        )
+        for session in sessions
+    }
+    nonzero = [value for value in totals.values() if value > 0.0]
+    target = float(np.mean(nonzero)) if nonzero else 1.0
+    return [
+        replace(pair, weight=pair.weight * target / totals[pair.session_id])
+        for pair in pairs
+        if totals[pair.session_id] > 0.0
+    ]
 
 
 def _solve_core(sessions: list[RotationSession], pairs: list[_Pair]) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
@@ -290,6 +415,129 @@ def _solve_core(sessions: list[RotationSession], pairs: list[_Pair]) -> tuple[np
     return r_x, biases, np.asarray(errors), covariance
 
 
+def _solve_baseline_consistency(
+    sessions: list[RotationSession],
+    pairs: list[_Pair],
+) -> BaselineConsistencyAudit:
+    """Audit the two physically observable dual-antenna rotation DOFs.
+
+    The confirmed ANT1-to-ANT2 axis is IMU +X. For every interval, the angle
+    swept by the GNSS baseline must equal the angle swept by IMU +X under gyro
+    preintegration. Rotation about +X cancels from this invariant and is not
+    falsely scored as an RTK attitude residual.
+    """
+
+    baseline_axis = np.array([1.0, 0.0, 0.0])
+    session_count = len(sessions)
+
+    def raw_errors(parameters: np.ndarray) -> np.ndarray:
+        biases = parameters.reshape(session_count, 3)
+        values = []
+        for pair in pairs:
+            corrected = apply_bias_jacobian_correction(
+                pair.delta_R_zero_bias,
+                pair.J_bg,
+                biases[pair.session_index],
+            )
+            observed = np.arccos(np.clip(pair.R_A[0, 0], -1.0, 1.0))
+            predicted = np.arccos(
+                np.clip(baseline_axis @ corrected @ baseline_axis, -1.0, 1.0)
+            )
+            values.append(predicted - observed)
+        return np.asarray(values)
+
+    def residual(parameters: np.ndarray) -> np.ndarray:
+        errors = raw_errors(parameters)
+        weighted = errors * np.sqrt(np.asarray([pair.weight for pair in pairs]))
+        return np.concatenate([weighted, parameters / 0.003])
+
+    initial = np.zeros(3 * session_count)
+    opt = least_squares(
+        residual,
+        initial,
+        loss="huber",
+        f_scale=np.deg2rad(0.25),
+        max_nfev=60,
+    )
+    biases = opt.x.reshape(session_count, 3)
+    errors_deg = np.degrees(raw_errors(opt.x))
+    per_session_rms = {}
+    per_session_p95 = {}
+    per_session_axis_rms = {}
+    for session in sessions:
+        selection = np.asarray(
+            [pair.session_id == session.session_id for pair in pairs], dtype=bool
+        )
+        values = errors_deg[selection]
+        per_session_rms[session.session_id] = (
+            float(np.sqrt(np.mean(values**2))) if values.size else np.nan
+        )
+        per_session_p95[session.session_id] = (
+            float(np.percentile(np.abs(values), 95.0)) if values.size else np.nan
+        )
+        axis_errors = []
+        for pair in np.asarray(pairs, dtype=object)[selection]:
+            corrected = apply_bias_jacobian_correction(
+                pair.delta_R_zero_bias,
+                pair.J_bg,
+                biases[pair.session_index],
+            )
+            axis_errors.append(np.degrees(so3_log(pair.R_A @ corrected.T)))
+        per_session_axis_rms[session.session_id] = (
+            np.sqrt(np.mean(np.asarray(axis_errors) ** 2, axis=0))
+            if axis_errors
+            else np.full(3, np.nan)
+        )
+    worst_indices = np.argsort(np.abs(errors_deg))[-20:][::-1]
+    worst_pairs = tuple(
+        {
+            "session_id": pairs[index].session_id,
+            "t0_s": pairs[index].t0_s,
+            "t1_s": pairs[index].t1_s,
+            "duration_s": pairs[index].t1_s - pairs[index].t0_s,
+            "baseline_angle_residual_deg": float(errors_deg[index]),
+        }
+        for index in worst_indices
+    )
+    rms = float(np.sqrt(np.mean(errors_deg**2)))
+    median = float(np.median(np.abs(errors_deg)))
+    p95 = float(np.percentile(np.abs(errors_deg), 95.0))
+    finite_session_rms = [
+        value for value in per_session_rms.values() if np.isfinite(value)
+    ]
+    finite_session_p95 = [
+        value for value in per_session_p95.values() if np.isfinite(value)
+    ]
+    ok = bool(
+        len(pairs) >= 20
+        and rms <= 1.0
+        and p95 <= 2.0
+        and (not finite_session_rms or max(finite_session_rms) <= 1.5)
+        and (not finite_session_p95 or max(finite_session_p95) <= 3.0)
+    )
+    return BaselineConsistencyAudit(
+        baseline_axis_imu=baseline_axis,
+        pair_count=len(pairs),
+        residual_rms_deg=rms,
+        residual_median_deg=median,
+        residual_p95_deg=p95,
+        per_session_rms_deg=per_session_rms,
+        per_session_p95_deg=per_session_p95,
+        per_session_axis_rms_deg=per_session_axis_rms,
+        gyro_bias_by_session_rad_s={
+            session.session_id: biases[index].copy()
+            for index, session in enumerate(sessions)
+        },
+        worst_pairs=worst_pairs,
+        ok=ok,
+        notes=(
+            "ANT1(main,left)->ANT2(secondary,right) is fixed to IMU +X",
+            "axis residual XYZ labels are baseline-spin(unobservable), baseline-elevation, heading",
+            "full rotation about the baseline is not identifiable from two antennas",
+        ),
+    )
+
+
 def solve_rtk_imu_rotation(
     sessions: list[RotationSession] | tuple[RotationSession, ...],
     *,
@@ -351,41 +599,28 @@ def solve_rtk_imu_rotation(
         per_session[session.session_id] = (
             float(np.sqrt(np.mean(np.asarray(values) ** 2))) if values else np.nan
         )
+    baseline_audit = _solve_baseline_consistency(items, pairs)
     loo = {}
     if compute_loo and len(items) >= 3:
         for omitted in items:
+            kept_items = [item for item in items if item.session_id != omitted.session_id]
+            kept_index = {item.session_id: index for index, item in enumerate(kept_items)}
             kept_pairs = [
-                pair
+                replace(pair, session_index=kept_index[pair.session_id])
                 for pair in pairs
                 if pair.session_id != omitted.session_id
             ]
             if len(kept_pairs) < 6:
                 loo[omitted.session_id] = np.nan
                 continue
-            def loo_residual(rotvec: np.ndarray) -> np.ndarray:
-                candidate = orthonormalize_rotation(so3_exp(rotvec))
-                rows = []
-                for pair in kept_pairs:
-                    corrected = apply_bias_jacobian_correction(
-                        pair.delta_R_zero_bias,
-                        pair.J_bg,
-                        biases[pair.session_index],
-                    )
-                    rows.append(
-                        np.sqrt(pair.weight)
-                        * so3_log(candidate.T @ pair.R_A @ candidate @ corrected.T)
-                    )
-                return np.concatenate(rows)
-
-            loo_opt = least_squares(
-                loo_residual,
-                so3_log(rotation),
-                loss='huber',
-                f_scale=np.deg2rad(0.5),
-                max_nfev=30,
+            try:
+                loo_rotation, _, _, _ = _solve_core(kept_items, kept_pairs)
+            except ValueError:
+                loo[omitted.session_id] = np.nan
+                continue
+            loo[omitted.session_id] = float(
+                np.degrees(np.linalg.norm(so3_log(rotation.T @ loo_rotation)))
             )
-            loo_rotation = orthonormalize_rotation(so3_exp(loo_opt.x))
-            loo[omitted.session_id] = float(np.degrees(np.linalg.norm(so3_log(rotation.T @ loo_rotation))))
     rotation_cov = covariance[:3, :3]
     std_deg = np.degrees(np.sqrt(np.maximum(np.diag(rotation_cov), 0.0)))
     singular_values = np.linalg.svd(np.linalg.pinv(rotation_cov, rcond=1e-12), compute_uv=False)
@@ -395,7 +630,7 @@ def solve_rtk_imu_rotation(
     finite_loo = [value for value in loo.values() if np.isfinite(value)]
     sorted_scores = sorted(scores.values())
     convention_gap = sorted_scores[1] - sorted_scores[0] if len(sorted_scores) > 1 else np.inf
-    ok = bool(
+    legacy_numeric_ok = bool(
         len(pairs) >= 20
         and rms <= 1.0
         and p95 <= 2.0
@@ -403,19 +638,29 @@ def solve_rtk_imu_rotation(
         and (not finite_loo or max(finite_loo) <= 1.0)
         and convention_gap >= 0.05
     )
+    # GNHPR supplies the ANT1-to-ANT2 direction but no independent rotation
+    # about that direction. A completed 3-D attitude is useful diagnostically,
+    # but cannot pass the full extrinsic-rotation gate from this dataset alone.
+    full_attitude_observable = False
+    ok = False
     notes = [
         "transform convention: p_RTK = R_RTK_IMU p_IMU",
         f"residual time convention: t_IMU = t_RTK + {offset:+.6f} s",
         f"GNHPR convention score gap={convention_gap:.4f} deg",
         "GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
-        "LOO is conditional: per-session gyro biases are held at their all-session estimates",
+        "LOO re-optimizes the remaining per-session gyro biases",
+        "legacy full-HPR rotation uses a zero-roll gauge completion and is diagnostic only",
+        "dual antennas do not observe rotation about the ANT1-to-ANT2 baseline",
     ]
     if not time_audit.reliable:
         notes.append("time-offset correlation was ambiguous; held residual offset at zero")
     if convention is not GNHPR_CANDIDATES[0]:
         notes.append("empirical best GNHPR convention differs from protocol expectation; manual verification required")
-    if not ok:
-        notes.append("rotation failed one or more strict acceptance gates")
+    if not baseline_audit.ok:
+        notes.append("the physically observable baseline consistency failed strict gates")
+    if not legacy_numeric_ok:
+        notes.append("the legacy gauge-completed rotation failed one or more numeric gates")
+    notes.append("full rotation is not accepted; translation must remain frozen")
     return RotationCalibrationResult(
         R_RTK_IMU=rotation,
         rpy_deg=rpy_deg_xyz(rotation),
@@ -434,6 +679,10 @@ def solve_rtk_imu_rotation(
         information_singular_values=singular_values,
         per_session_rms_deg=per_session,
         loo_delta_deg=loo,
+        baseline_consistency=baseline_audit,
+        observable_rotation_dof=2,
+        full_attitude_observable=full_attitude_observable,
+        legacy_full_attitude_numeric_ok=legacy_numeric_ok,
         ok=ok,
         notes=tuple(notes),
     )

imu_lidar/rtk_io.py

@@ -22,23 +22,43 @@ class RtkSeries:
     roll_deg: np.ndarray
     fix_quality: np.ndarray
     heading_quality: np.ndarray
+    heading_satellites: np.ndarray
+    heading_age_s: np.ndarray
     hdop: np.ndarray
+    checksum_valid: np.ndarray
     origin_geodetic: tuple[float, float, float]
     source: Path
 
     @property
     def attitude_valid(self) -> np.ndarray:
+        """Strict fixed dual-antenna solutions suitable for calibration."""
+
         return (
             np.isfinite(self.heading_deg)
             & np.isfinite(self.pitch_deg)
             & np.isfinite(self.roll_deg)
-            & np.isin(self.heading_quality, (4.0, 5.0))
+            & (self.heading_quality == 4.0)
+            & self.checksum_valid
+        )
+
+    @property
+    def attitude_float(self) -> np.ndarray:
+        """Float solutions retained for diagnostics but never calibration."""
+
+        return (
+            np.isfinite(self.heading_deg)
+            & np.isfinite(self.pitch_deg)
+            & np.isfinite(self.roll_deg)
+            & (self.heading_quality == 5.0)
+            & self.checksum_valid
         )
 
     @property
     def position_valid(self) -> np.ndarray:
-        return np.all(np.isfinite(self.position_enu_m), axis=1) & np.isin(
-            self.fix_quality, (4.0, 5.0)
+        return (
+            np.all(np.isfinite(self.position_enu_m), axis=1)
+            & (self.fix_quality == 4.0)
+            & self.checksum_valid
         )
 
 
@@ -88,7 +108,10 @@ def load_rtk_csv(path: Path | str) -> RtkSeries:
         roll_deg=_column(data, "roll_deg")[order],
         fix_quality=_column(data, "fix_quality", default=0.0)[order],
         heading_quality=_column(data, "heading_quality", default=0.0)[order],
+        heading_satellites=_column(data, "heading_satellites")[order],
+        heading_age_s=_column(data, "heading_age_s")[order],
         hdop=_column(data, "hdop")[order],
+        checksum_valid=_column(data, "checksum_valid", default=1.0)[order] == 1.0,
         origin_geodetic=origin,
         source=source,
     )