#!/usr/bin/env python3 """Read-only artificial GNHPR dropout validation for the engineering R0 bridge. For each requested gap, Q4 HPR samples inside an otherwise 0.1 s contiguous run are withheld. Their true baseline directions are compared with normalized linear interpolation and short-horizon HI13 gyro propagation. No lever-arm solve, prior, bootstrap, sensitivity run, or threshold update is performed. """ from __future__ import annotations import argparse import json import math import sys from pathlib import Path import numpy as np from scipy.spatial.transform import Rotation ROOT = Path(__file__).resolve().parents[1] if str(ROOT) not in sys.path: sys.path.insert(0, str(ROOT)) from rtk_imu.rtk_imu_engineering import _all_hpr, _baseline_angle_deg, _interpolate_baseline, _world_rtk from rtk_imu.rtk_imu_multisource import load_unified_sessions GAPS_S = (0.2, 0.3, 0.4, 0.5, 0.6, 0.8) R2G_RPY_DEG = (0.4543066225, -0.0026392019, 0.0122384129) BASELINE_FACTOR_SIGMA_DEG = float(np.degrees(0.035)) def _jsonable(value): if isinstance(value, np.ndarray): return _jsonable(value.tolist()) if isinstance(value, np.generic): return _jsonable(value.item()) if isinstance(value, float): return value if math.isfinite(value) else None if isinstance(value, dict): return {str(k): _jsonable(v) for k, v in value.items()} if isinstance(value, (tuple, list)): return [_jsonable(v) for v in value] return value def _contiguous_runs(times: np.ndarray, valid: np.ndarray) -> list[np.ndarray]: indices = np.flatnonzero(valid) if indices.size == 0: return [] runs: list[list[int]] = [[int(indices[0])]] for previous, index in zip(indices[:-1], indices[1:]): dt = float(times[index] - times[previous]) if 0.075 <= dt <= 0.125: runs[-1].append(int(index)) else: runs.append([int(index)]) return [np.asarray(run, dtype=int) for run in runs if len(run) >= 3] def _propagate_baseline(session, R_WI_start: np.ndarray, baseline_I: np.ndarray, start_s: float, targets_s: np.ndarray) -> list[np.ndarray]: """Propagate with vectorized gyro interpolation over the short withheld gap.""" targets = np.asarray(targets_s, dtype=float) if targets.size == 0: return [] grid = np.unique(np.concatenate(([start_s], session.imu.t_s[ (session.imu.t_s > start_s) & (session.imu.t_s < targets[-1])], targets))) gyro = np.column_stack([ np.interp(grid, session.imu.t_s, session.imu.gyro_rad_s[:, axis]) for axis in range(3) ]) R_WI = np.asarray(R_WI_start, dtype=float).copy() predicted: list[np.ndarray] = [] target_index = 0 for index, (left, right) in enumerate(zip(grid[:-1], grid[1:])): omega = 0.5 * (gyro[index] + gyro[index + 1]) R_WI = R_WI @ Rotation.from_rotvec(omega * float(right - left)).as_matrix() while target_index < targets.size and abs(targets[target_index] - right) < 1e-9: predicted.append(R_WI @ baseline_I) target_index += 1 return predicted def _session_validation(session, rotation: np.ndarray, max_windows: int) -> dict[str, object]: hpr = _all_hpr(session) runs = _contiguous_runs(hpr.t_s, hpr.valid) baseline_I = rotation.T[:, 0] result: dict[str, object] = {"session_id": session.session_id, "q4_runs": len(runs), "gaps": {}} for requested_gap in GAPS_S: interpolation_errors: list[float] = [] propagation_errors: list[float] = [] actual_gaps: list[float] = [] windows = 0 for run in runs: nominal_dt = float(np.median(np.diff(hpr.t_s[run]))) step_count = max(2, int(round(requested_gap / nominal_dt))) # The endpoints remain observed, every internal Q4 point is withheld. for start in range(0, run.size - step_count, max(1, step_count)): subset = run[start:start + step_count + 1] if subset.size != step_count + 1: continue left, right = int(subset[0]), int(subset[-1]) actual_gap = float(hpr.t_s[right] - hpr.t_s[left]) if abs(actual_gap - requested_gap) > 0.08: continue withheld = subset[1:-1] if withheld.size == 0: continue left_b, right_b = hpr.baseline_enu[left], hpr.baseline_enu[right] targets = hpr.t_s[withheld] predicted_prop = _propagate_baseline( session, _world_rtk(left_b) @ rotation, baseline_I, float(hpr.t_s[left]), targets ) for index, predicted in zip(withheld, predicted_prop): fraction = float((hpr.t_s[index] - hpr.t_s[left]) / actual_gap) interpolation_errors.append(_baseline_angle_deg( _interpolate_baseline(left_b, right_b, fraction), hpr.baseline_enu[index] )) propagation_errors.append(_baseline_angle_deg(predicted, hpr.baseline_enu[index])) actual_gaps.append(actual_gap) windows += 1 if windows >= max_windows: break if windows >= max_windows: break def summary(errors: list[float]) -> dict[str, float | int | None]: finite = np.asarray([e for e in errors if np.isfinite(e)], dtype=float) if finite.size == 0: return {"count": 0, "p50_deg": None, "p95_deg": None, "max_deg": None} return {"count": int(finite.size), "p50_deg": float(np.percentile(finite, 50)), "p95_deg": float(np.percentile(finite, 95)), "max_deg": float(np.max(finite))} interp, prop = summary(interpolation_errors), summary(propagation_errors) choice = "linear_baseline_interpolation" if (interp["p95_deg"] or np.inf) <= (prop["p95_deg"] or np.inf) else "imu_gyro_propagation" selected_p95 = interp["p95_deg"] if choice.startswith("linear") else prop["p95_deg"] result["gaps"][f"{requested_gap:.1f}"] = { "requested_gap_s": requested_gap, "actual_gap_p50_s": float(np.median(actual_gaps)) if actual_gaps else None, "window_count": windows, "linear_baseline_interpolation": interp, "imu_gyro_propagation": prop, "preferred_method_by_p95": choice, "selected_p95_deg": selected_p95, "within_baseline_factor_sigma": bool(selected_p95 is not None and selected_p95 <= BASELINE_FACTOR_SIGMA_DEG), } return result def main() -> int: parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("--manifest", type=Path, required=True) parser.add_argument("--output", type=Path, required=True) parser.add_argument("--session", action="append") parser.add_argument("--max-windows", type=int, default=500) args = parser.parse_args() sessions = load_unified_sessions(args.manifest, selected_session_ids=None if args.session is None else set(args.session)) rotation = Rotation.from_euler("xyz", R2G_RPY_DEG, degrees=True).as_matrix() session_results = [_session_validation(session, rotation, args.max_windows) for session in sessions] recommendation: dict[str, object] = {} for gap in GAPS_S: entries = [item["gaps"][f"{gap:.1f}"] for item in session_results if item["gaps"].get(f"{gap:.1f}")] p95 = [entry["selected_p95_deg"] for entry in entries if entry["selected_p95_deg"] is not None] recommendation[f"{gap:.1f}"] = { "sessions_with_samples": len(p95), "worst_session_selected_p95_deg": float(max(p95)) if p95 else None, "passes_all_sessions_factor_sigma": bool(p95 and max(p95) <= BASELINE_FACTOR_SIGMA_DEG), } passing = [float(key) for key, value in recommendation.items() if value["passes_all_sessions_factor_sigma"]] payload = { "scope": "artificial HPR dropout validation only; no solve/prior/bootstrap/sensitivity/threshold change", "r2g_rotation_rpy_deg": R2G_RPY_DEG, "baseline_factor_sigma_deg": BASELINE_FACTOR_SIGMA_DEG, "sessions": session_results, "bridge_recommendation": recommendation, "largest_gap_passing_all_sessions_s": max(passing) if passing else None, } args.output.parent.mkdir(parents=True, exist_ok=True) args.output.write_text(json.dumps(_jsonable(payload), ensure_ascii=False, indent=2) + "\n", encoding="utf-8") print(json.dumps({"largest_gap_passing_all_sessions_s": payload["largest_gap_passing_all_sessions_s"], "bridge_recommendation": recommendation}, ensure_ascii=False, indent=2)) return 0 if __name__ == "__main__": raise SystemExit(main())