#!/usr/bin/env python3 """Penetration audit for RTK--IMU motion excitation. This is read-only diagnostics. It never applies a lever prior, solves a lever arm, or changes continuity/acceptance thresholds. Gyro trajectory integrals are the primary excitation metrics; start/end Euler differences are deliberately absent. """ from __future__ import annotations import argparse import json import math import sys from dataclasses import asdict from pathlib import Path from typing import Iterable import numpy as np ROOT = Path(__file__).resolve().parents[1] if str(ROOT) not in sys.path: sys.path.insert(0, str(ROOT)) from imu_lidar.imu_audit import audit_imu from imu_lidar.rtk_imu_engineering import ( MIN_SEGMENT_DURATION_S, MIN_SEGMENT_NODE_COUNT, _all_hpr, _height_reference, _hpr_factor_observation, _node_interval_threshold_s, _trajectory_continuity_reasons, _nearest_index, _nodes, _position_valid, _segments, _source_nodes, ) from imu_lidar.rtk_imu_multisource import _f, _truth, load_unified_sessions RAW_GAP_S = 1.5 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 hasattr(value, "__dataclass_fields__"): return {key: _jsonable(item) for key, item in asdict(value).items()} if isinstance(value, dict): return {str(key): _jsonable(item) for key, item in value.items()} if isinstance(value, (tuple, list)): return [_jsonable(item) for item in value] return value def _trapz(values: np.ndarray, t_s: np.ndarray) -> np.ndarray: if t_s.size < 2: return np.zeros(values.shape[1], dtype=float) return np.trapezoid(values, t_s, axis=0) def _interval_imu(session, start_s: float, end_s: float) -> tuple[np.ndarray, np.ndarray]: mask = (session.imu.t_s >= start_s) & (session.imu.t_s <= end_s) return session.imu.t_s[mask], session.imu.gyro_rad_s[mask] def _gyro_metrics(session, start_s: float, end_s: float, gyro_offset_rad_s: np.ndarray | None = None) -> dict[str, object]: t_s, gyro = _interval_imu(session, start_s, end_s) if gyro_offset_rad_s is not None: gyro = gyro - np.asarray(gyro_offset_rad_s, dtype=float).reshape(1, 3) if t_s.size < 2: nan = np.full(3, np.nan) return { "sample_count": int(t_s.size), "net_rotation_xyz_deg": nan, "unwrap_rotation_range_xyz_deg": nan, "cumulative_absolute_rotation_xyz_deg": nan, "gyro_integral_squared_xyz_rad2_s": nan, "gyro_rms_xyz_deg_s": nan, "gyro_peak_xyz_deg_s": nan, } dt = np.diff(t_s) midpoint = 0.5 * (gyro[:-1] + gyro[1:]) trajectory = np.vstack([np.zeros(3), np.cumsum(midpoint * dt[:, None], axis=0)]) # The integrated trajectory is continuous. Explicit unwrap documents that # the yaw range is never inferred from a wrapped heading/Euler endpoint. trajectory[:, 2] = np.unwrap(trajectory[:, 2]) duration = float(t_s[-1] - t_s[0]) return { "sample_count": int(t_s.size), "net_rotation_xyz_deg": np.degrees(trajectory[-1]), "unwrap_rotation_range_xyz_deg": np.degrees(np.ptp(trajectory, axis=0)), "cumulative_absolute_rotation_xyz_deg": np.degrees(_trapz(np.abs(gyro), t_s)), "gyro_integral_squared_xyz_rad2_s": _trapz(gyro * gyro, t_s), "gyro_rms_xyz_deg_s": np.degrees(np.sqrt(_trapz(gyro * gyro, t_s) / duration)), "gyro_peak_xyz_deg_s": np.degrees(np.max(np.abs(gyro), axis=0)), } def _interval_summary(session, start_s: float, end_s: float, *, label: str, best_rows: Iterable[dict[str, str]], hpr) -> dict[str, object]: best = list(best_rows) in_range = [row for row in best if start_s <= _f(row, "t_device_s") <= end_s] doppler = [ row for row in in_range if _truth(row, "doppler_velocity_valid") and np.all(np.isfinite([ _f(row, "velocity_east_m_s"), _f(row, "velocity_north_m_s"), _f(row, "vertical_speed_m_s"), ])) ] q4 = hpr.valid & (hpr.t_s >= start_s) & (hpr.t_s <= end_s) return { "label": label, "start_s": float(start_s), "end_s": float(end_s), "duration_s": float(max(0.0, end_s - start_s)), "bestnava_count": len(in_range), "doppler_count": len(doppler), "q4_hpr_count": int(np.count_nonzero(q4)), "gyro": _gyro_metrics(session, start_s, end_s), } def _coalesce(records: list[dict[str, object]], *, include: bool, label: str, session, best_rows, hpr) -> list[dict[str, object]]: result: list[dict[str, object]] = [] current: list[dict[str, object]] = [] key: tuple[str, ...] | None = None for record in records: active = bool(record["accepted"]) == include reasons = tuple(record["reasons"]) same = ( current and active and key == reasons and float(record["t_s"]) - float(current[-1]["t_s"]) <= RAW_GAP_S ) if active and (not current or same): current.append(record) key = reasons continue if current: summary = _interval_summary( session, float(current[0]["t_s"]), float(current[-1]["t_s"]), label=label, best_rows=best_rows, hpr=hpr, ) if not include: summary["cut_reason"] = list(key or ()) result.append(summary) current = [record] if active else [] key = reasons if active else None if current: summary = _interval_summary( session, float(current[0]["t_s"]), float(current[-1]["t_s"]), label=label, best_rows=best_rows, hpr=hpr, ) if not include: summary["cut_reason"] = list(key or ()) result.append(summary) return result def _raw_records(session) -> list[dict[str, object]]: """Raw-valid is position/IMU validity; HPR support remains a separate factor audit.""" hpr = _all_hpr(session) rows = session.rtk_by_type.get("BESTNAVA", []) ordered = sorted(rows, key=lambda row: _f(row, "t_device_s")) records: list[dict[str, object]] = [] last_t = -np.inf for row in ordered: t_s = _f(row, "t_device_s") reasons: list[str] = [] if not np.isfinite(t_s): reasons.append("position_device_time_invalid") elif t_s <= last_t: reasons.append("position_device_time_nonmonotonic") if np.isfinite(t_s): last_t = max(last_t, t_s) if not _truth(row, "checksum_valid"): reasons.append("position_checksum_invalid") if not _truth(row, "position_fixed"): reasons.append("position_not_fixed") if not _position_valid(row, "BESTNAVA"): reasons.append("position_required_field_invalid") imu_index = _nearest_index(session.imu.t_s, t_s, 0.03) if np.isfinite(t_s) else None if imu_index is None: reasons.append("imu_missing_near") _, _, hpr_factor_valid, hpr_method, hpr_gap = _hpr_factor_observation(hpr, t_s) doppler_ok = bool( _truth(row, "doppler_velocity_valid") and np.all(np.isfinite([ _f(row, "velocity_east_m_s"), _f(row, "velocity_north_m_s"), _f(row, "vertical_speed_m_s"), ])) ) records.append({ "t_s": t_s, "row": row, "accepted": not reasons, "reasons": sorted(set(reasons)), "doppler_valid": doppler_ok, "hpr_factor_valid": hpr_factor_valid, "hpr_factor_method": hpr_method, "hpr_support_gap_s": hpr_gap, }) return records def _r0_runs_and_cuts(session, nodes, hpr, best_rows, period_s: float) -> tuple[list[dict[str, object]], list[dict[str, object]], list[dict[str, object]]]: runs: list[list] = [] cuts: list[dict[str, object]] = [] intervals: list[dict[str, object]] = [] if not nodes: return [], [], [] current = [nodes[0]] threshold = _node_interval_threshold_s(period_s) for previous, node in zip(nodes[:-1], nodes[1:]): dt = float(node.t_s - previous.t_s) structural_reasons = list(_trajectory_continuity_reasons(session, previous.t_s, node.t_s, period_s)) continuity_break = node.continuity_id != previous.continuity_id reasons = structural_reasons or (["position_source_quality_or_merge_break"] if continuity_break else []) intervals.append({ "left_t_s": float(previous.t_s), "right_t_s": float(node.t_s), "dt_s": dt, "threshold_s": threshold, "trajectory_continuous": not structural_reasons, "continuity_id_changed": continuity_break, "cut_reason": reasons, "left_hpr_factor": {"valid": previous.hpr_factor_valid, "method": previous.hpr_factor_method, "support_gap_s": previous.hpr_support_gap_s}, "right_hpr_factor": {"valid": node.hpr_factor_valid, "method": node.hpr_factor_method, "support_gap_s": node.hpr_support_gap_s}, }) if not continuity_break: current.append(node) continue runs.append(current) cuts.append({ **_interval_summary(session, previous.t_s, node.t_s, label="r0_cut", best_rows=best_rows, hpr=hpr), "dt_s": dt, "threshold_s": threshold, "cut_reason": reasons, }) current = [node] runs.append(current) summaries = [ _interval_summary(session, run[0].t_s, run[-1].t_s, label="R0_after_cuts", best_rows=best_rows, hpr=hpr) | { "node_count": len(run), "continuity_id": int(run[0].continuity_id), "bestnava_count": sum(node.source == "BESTNAVA" for node in run), "doppler_count": sum(node.velocity_enu_m_s is not None for node in run), "hpr_factor_count": sum(node.hpr_factor_valid for node in run), "hpr_factor_rejected_count": sum(not node.hpr_factor_valid for node in run), } for run in runs ] return summaries, cuts, intervals def _qualified_summary(session, segments, hpr, best_rows) -> list[dict[str, object]]: return [ _interval_summary(session, segment.nodes[0].t_s, segment.nodes[-1].t_s, label="qualified_segment", best_rows=best_rows, hpr=hpr) | { "segment_id": segment.segment_id, "node_count": len(segment.nodes), "bestnava_count": sum(node.source == "BESTNAVA" for node in segment.nodes), "doppler_count": sum(node.velocity_enu_m_s is not None for node in segment.nodes), "hpr_factor_count": sum(node.hpr_factor_valid for node in segment.nodes), "hpr_factor_rejected_count": sum(not node.hpr_factor_valid for node in segment.nodes), } for segment in segments ] def _dropped_r0_runs(session, r0_nodes, qualified, hpr, best_rows) -> list[dict[str, object]]: qualified_ranges = [(s.nodes[0].t_s, s.nodes[-1].t_s) for s in qualified] result: list[dict[str, object]] = [] by_id: dict[int, list] = {} for node in r0_nodes: by_id.setdefault(node.continuity_id, []).append(node) for run in by_id.values(): start_s, end_s = run[0].t_s, run[-1].t_s retained = any(abs(start_s - left) < 1e-6 and abs(end_s - right) < 1e-6 for left, right in qualified_ranges) if retained: continue reasons = [] if len(run) < MIN_SEGMENT_NODE_COUNT: reasons.append("qualified_min_node_count") if end_s - start_s < MIN_SEGMENT_DURATION_S: reasons.append("qualified_min_duration") if not reasons: reasons.append("preintegration_or_segment_validation") result.append({ **_interval_summary(session, start_s, end_s, label="dropped_before_qualified", best_rows=best_rows, hpr=hpr), "node_count": len(run), "cut_reason": reasons, }) return result def _interval_overlap(left: dict[str, object], right: dict[str, object]) -> float: return max(0.0, min(float(left["end_s"]), float(right["end_s"])) - max(float(left["start_s"]), float(right["start_s"]))) def _interval_penetration(raw_intervals, r0_intervals, qualified_intervals, cut_intervals): """Link every raw-valid dynamic interval to its downstream R0/qualified survivors.""" result = [] for raw in raw_intervals: r0 = [item for item in r0_intervals if _interval_overlap(raw, item) > 0.0 or ( item["start_s"] == item["end_s"] and raw["start_s"] <= item["start_s"] <= raw["end_s"] )] qualified = [item for item in qualified_intervals if _interval_overlap(raw, item) > 0.0] cuts = [item for item in cut_intervals if _interval_overlap(raw, item) > 0.0] raw_best = max(int(raw["bestnava_count"]), 1) raw_doppler = max(int(raw["doppler_count"]), 1) r0_duration = sum(_interval_overlap(raw, item) for item in r0) qualified_duration = sum(_interval_overlap(raw, item) for item in qualified) cut_reasons = sorted({reason for item in cuts for reason in item.get("cut_reason", [])}) result.append({ "raw_start_s": raw["start_s"], "raw_end_s": raw["end_s"], "raw_duration_s": raw["duration_s"], "raw_bestnava_count": raw["bestnava_count"], "raw_doppler_count": raw["doppler_count"], "raw_gyro": raw["gyro"], "R0_overlap_duration_s": r0_duration, "qualified_overlap_duration_s": qualified_duration, "R0_bestnava_count": sum(int(item["bestnava_count"]) for item in r0), "R0_doppler_count": sum(int(item["doppler_count"]) for item in r0), "qualified_bestnava_count": sum(int(item["bestnava_count"]) for item in qualified), "qualified_doppler_count": sum(int(item["doppler_count"]) for item in qualified), "retention": { "raw_to_R0_bestnava": sum(int(item["bestnava_count"]) for item in r0) / raw_best, "raw_to_R0_doppler": sum(int(item["doppler_count"]) for item in r0) / raw_doppler, "raw_to_R0_duration": r0_duration / max(float(raw["duration_s"]), 1e-9), "raw_to_qualified_bestnava": sum(int(item["bestnava_count"]) for item in qualified) / raw_best, "raw_to_qualified_doppler": sum(int(item["doppler_count"]) for item in qualified) / raw_doppler, "raw_to_qualified_duration": qualified_duration / max(float(raw["duration_s"]), 1e-9), }, "cut_reason": cut_reasons, }) return result def _union_time_intervals(intervals: list[dict[str, object]]) -> list[tuple[float, float]]: ordered = sorted( (float(item["start_s"]), float(item["end_s"])) for item in intervals if np.isfinite(float(item["start_s"])) and np.isfinite(float(item["end_s"])) ) merged: list[list[float]] = [] for start_s, end_s in ordered: if end_s < start_s: continue if not merged or start_s > merged[-1][1]: merged.append([start_s, end_s]) else: merged[-1][1] = max(merged[-1][1], end_s) return [(start_s, end_s) for start_s, end_s in merged] def _stage_statistics(session, intervals: list[dict[str, object]]) -> dict[str, object]: """Audit each stage on unique IMU samples over the union of its time ranges.""" total = _stage_total(intervals) union = _union_time_intervals(intervals) selected_count = 0 unique_mask = np.zeros(session.imu.t_s.size, dtype=bool) for item in intervals: mask = (session.imu.t_s >= float(item["start_s"])) & (session.imu.t_s <= float(item["end_s"])) selected_count += int(np.count_nonzero(mask)) unique_mask |= mask unique_count = int(np.count_nonzero(unique_mask)) input_duration = float(sum(max(0.0, float(item["end_s"]) - float(item["start_s"])) for item in intervals)) union_duration = float(sum(end_s - start_s for start_s, end_s in union)) net = np.zeros(3) unwrap_range_sum = np.zeros(3) cumulative_abs = np.zeros(3) energy = np.zeros(3) peak = np.zeros(3) metric_duration = 0.0 for start_s, end_s in union: gyro = _gyro_metrics(session, start_s, end_s) current_net = np.asarray(gyro["net_rotation_xyz_deg"], dtype=float) if not np.all(np.isfinite(current_net)): continue net += current_net unwrap_range_sum += np.asarray(gyro["unwrap_rotation_range_xyz_deg"], dtype=float) cumulative_abs += np.asarray(gyro["cumulative_absolute_rotation_xyz_deg"], dtype=float) energy += np.asarray(gyro["gyro_integral_squared_xyz_rad2_s"], dtype=float) peak = np.maximum(peak, np.asarray(gyro["gyro_peak_xyz_deg_s"], dtype=float)) metric_duration += max(0.0, end_s - start_s) total["unique_imu_coverage"] = { "input_interval_count": len(intervals), "union_interval_count": len(union), "input_duration_s": input_duration, "union_duration_s": union_duration, "overlap_duration_s": max(0.0, input_duration - union_duration), "selected_imu_sample_count_before_dedup": selected_count, "unique_imu_sample_count": unique_count, "duplicate_imu_sample_count": selected_count - unique_count, } total["gyro"] = { "net_rotation_xyz_deg": net, "sum_interval_unwrap_rotation_range_xyz_deg": unwrap_range_sum, "cumulative_absolute_rotation_xyz_deg": cumulative_abs, "gyro_integral_squared_xyz_rad2_s": energy, "gyro_rms_xyz_deg_s": np.degrees(np.sqrt(energy / max(metric_duration, 1e-9))), "gyro_peak_xyz_deg_s": peak, } return total def _stage_total(intervals: list[dict[str, object]]) -> dict[str, object]: total = {"interval_count": len(intervals), "duration_s": 0.0, "bestnava_count": 0, "doppler_count": 0, "q4_hpr_count": 0} for item in intervals: for key in ("duration_s", "bestnava_count", "doppler_count", "q4_hpr_count"): total[key] += item[key] return total def _hpr_chain_diagnostics(hpr) -> dict[str, object]: if hpr.t_s.size < 2: return {"sample_count": int(hpr.t_s.size), "pair_count": 0} dt = np.diff(hpr.t_s) finite_vector = np.all(np.isfinite(hpr.baseline_enu), axis=1) dot = np.sum(hpr.baseline_enu[:-1] * hpr.baseline_enu[1:], axis=1) jump_deg = np.degrees(np.arccos(np.clip(dot, -1.0, 1.0))) rate = jump_deg / np.maximum(dt, 1e-12) return { "sample_count": int(hpr.t_s.size), "q4_valid_sample_count": int(np.count_nonzero(hpr.valid)), "pair_count": int(dt.size), "pair_with_invalid_endpoint_count": int(np.count_nonzero(~(hpr.valid[:-1] & hpr.valid[1:]))), "dt_s_p50_p95_max": np.percentile(dt[np.isfinite(dt)], [50.0, 95.0, 100.0]), "dt_too_short_count": int(np.count_nonzero(dt < 0.03)), "dt_too_long_count": int(np.count_nonzero(dt > 0.25)), "baseline_jump_rate_over_45deg_s_count": int(np.count_nonzero( finite_vector[:-1] & finite_vector[1:] & (rate > 45.0) )), } def _hpr_axis_mapping(session, hpr) -> dict[str, object]: valid = hpr.valid & np.isfinite(hpr.t_s) if np.count_nonzero(valid) < 8: return {"available": False, "reason": "fewer_than_8_q4_hpr_samples"} t = hpr.t_s[valid] dt = np.diff(t) keep = np.r_[True, (dt > 0.03) & (dt <= 0.25)] t = t[keep] # hpr arrays preserve GNHPR order after time sorting; heading must unwrap. hpr_rows = sorted(session.rtk_by_type.get("GNHPR", []), key=lambda row: _f(row, "t_device_s")) heading = np.unwrap(np.deg2rad(np.asarray([_f(row, "heading_deg") for row in hpr_rows])))[valid][keep] pitch = np.asarray([_f(row, "pitch_deg") for row in hpr_rows])[valid][keep] if t.size < 8: return {"available": False, "reason": "insufficient_contiguous_q4_hpr"} heading_rate = np.gradient(heading, t) gyro = np.column_stack([np.interp(t, session.imu.t_s, session.imu.gyro_rad_s[:, axis]) for axis in range(3)]) correlation = [] for axis in range(3): value = np.corrcoef(heading_rate, gyro[:, axis])[0, 1] correlation.append(float(value) if np.isfinite(value) else np.nan) best_axis = int(np.nanargmax(np.abs(correlation))) if np.any(np.isfinite(correlation)) else None return { "available": best_axis is not None, "hpr_heading_unwrapped_range_deg": float(np.degrees(np.ptp(heading))), "hpr_heading_net_rotation_deg": float(np.degrees(heading[-1] - heading[0])), "hpr_heading_cumulative_absolute_rotation_deg": float(np.degrees(np.sum(np.abs(np.diff(heading))))), "hpr_pitch_range_deg": float(np.ptp(pitch)), "hpr_pitch_cumulative_absolute_change_deg": float(np.sum(np.abs(np.diff(pitch)))), "heading_rate_to_imu_gyro_correlation_xyz": np.asarray(correlation), "best_correlated_imu_axis": best_axis, "expected_z_axis_correlation": correlation[2], "note": "heading is unwrapped; sign depends on GNHPR clockwise-from-north convention", } def _bias_absorption_check(session, intervals: list[dict[str, object]]) -> dict[str, object]: report = audit_imu(session.imu) checks = [] for item in intervals: if item["duration_s"] < 1.0: continue start_s, end_s = item["start_s"], item["end_s"] raw = _gyro_metrics(session, start_s, end_s) static_corrected = _gyro_metrics(session, start_s, end_s, report.gyro_bias_rad_s) t, gyro = _interval_imu(session, start_s, end_s) mean = np.mean(gyro, axis=0) if gyro.size else np.zeros(3) mean_removed = _gyro_metrics(session, start_s, end_s, mean) raw_abs = np.asarray(raw["cumulative_absolute_rotation_xyz_deg"]) removed_abs = np.asarray(mean_removed["cumulative_absolute_rotation_xyz_deg"]) ratio = removed_abs / np.maximum(raw_abs, 1e-9) checks.append({ "start_s": start_s, "end_s": end_s, "static_bias_rad_s": report.gyro_bias_rad_s, "segment_mean_gyro_rad_s": mean, "segment_mean_removed_to_raw_abs_rotation_ratio_xyz": ratio, "static_bias_corrected": static_corrected, "mean_removal_would_absorb_motion": bool(np.any(ratio < 0.5)), }) return {"imu_static_audit": report, "interval_checks": checks, "note": "Engineering audit integrates raw gyro; it does not subtract a segment mean."} def _unit_check(session) -> dict[str, object]: gyro = session.imu.gyro_rad_s norm = np.linalg.norm(gyro, axis=1) p99 = float(np.percentile(norm, 99.0)) if norm.size else np.nan return { "gyro_p99_norm_rad_s": p99, "gyro_p99_norm_deg_s": float(np.degrees(p99)), "gyro_peak_norm_rad_s": float(np.max(norm)) if norm.size else np.nan, "suspect_deg_per_second_stored_as_rad_per_second": bool(np.isfinite(p99) and p99 > 20.0), "suspect_near_zero_gyro_scale": bool(np.isfinite(p99) and p99 < 1e-4), "unit_contract": "unified imu.npz gyro_rad_s is radians per second", } def _candidate_scores(session_audit: dict[str, object]) -> dict[str, float]: raw = session_audit["raw_valid_intervals"] if not raw: return {"circle": 0.0, "left_right": 0.0, "slope": 0.0} cumulative = np.zeros(3) net = np.zeros(3) for interval in raw: gyro = interval["gyro"] value = np.asarray(gyro["cumulative_absolute_rotation_xyz_deg"], dtype=float) signed = np.asarray(gyro["net_rotation_xyz_deg"], dtype=float) if np.all(np.isfinite(value)): cumulative += value if np.all(np.isfinite(signed)): net += signed axis = session_audit["axis_mapping"] heading_range = abs(float(axis.get("hpr_heading_unwrapped_range_deg", 0.0) or 0.0)) heading_abs = abs(float(axis.get("hpr_heading_cumulative_absolute_rotation_deg", 0.0) or 0.0)) heading_net = abs(float(axis.get("hpr_heading_net_rotation_deg", 0.0) or 0.0)) pitch_range = abs(float(axis.get("hpr_pitch_range_deg", 0.0) or 0.0)) pitch_abs = abs(float(axis.get("hpr_pitch_cumulative_absolute_change_deg", 0.0) or 0.0)) return { "circle": max(heading_range, cumulative[2]), "left_right": max(0.0, heading_abs - heading_net, cumulative[2] - abs(net[2])), "slope": max(cumulative[0], cumulative[1]) ** 2 / max(1.0, cumulative[2]), } def _select_candidates(audits: list[dict[str, object]]) -> dict[str, dict[str, object] | None]: remaining = list(audits) chosen: dict[str, dict[str, object] | None] = {} for kind in ("circle", "left_right", "slope"): ranked = sorted(remaining, key=lambda item: item["candidate_scores"][kind], reverse=True) choice = ranked[0] if ranked and ranked[0]["candidate_scores"][kind] > 0.0 else None chosen[kind] = None if choice is None else { "session_id": choice["session_id"], "score_deg": choice["candidate_scores"][kind], "selection_metric": { "circle": "max(unwrapped HPR heading range, raw gyro-z net/absolute rotation)", "left_right": "unwrapped HPR heading cumulative change minus net change, cross-checked with gyro-z", "slope": "tilt-dominance: max(raw gyro-x/y cumulative rotation)^2 / raw gyro-z cumulative rotation", }[kind], } if choice is not None: remaining.remove(choice) return chosen def _audit_session(session, period_s: float) -> dict[str, object]: reference = _height_reference([session]) hpr = _all_hpr(session) best_rows = session.rtk_by_type.get("BESTNAVA", []) records = _raw_records(session) raw_valid = _coalesce(records, include=True, label="raw_valid", session=session, best_rows=best_rows, hpr=hpr) raw_rejected = _coalesce(records, include=False, label="dropped_before_raw_valid", session=session, best_rows=best_rows, hpr=hpr) r0_nodes = [] if reference is None else _nodes(session, reference, period_s) r0_intervals, r0_cuts, r0_node_intervals = _r0_runs_and_cuts( session, r0_nodes, hpr, best_rows, period_s ) all_qualified = _segments([session], period_s) qualified = [segment for segment in all_qualified if segment.session_id == session.session_id] qualified_intervals = _qualified_summary(session, qualified, hpr, best_rows) dropped_r0 = _dropped_r0_runs(session, r0_nodes, qualified, hpr, best_rows) r0_selected_times = np.asarray([node.t_s for node in r0_nodes]) decimated = [] for record in records: if not record["accepted"]: continue t_s = float(record["t_s"]) selected = r0_selected_times.size and np.min(np.abs(r0_selected_times - t_s)) < 1e-8 if not selected: decimated.append({**record, "accepted": False, "reasons": ["sample_period_decimation"]}) decimation_cuts = _coalesce(decimated, include=False, label="dropped_raw_to_R0", session=session, best_rows=best_rows, hpr=hpr) raw_total, r0_total, qualified_total = map(_stage_total, (raw_valid, r0_intervals, qualified_intervals)) retention = { "raw_to_R0": { "bestnava_count_ratio": r0_total["bestnava_count"] / max(raw_total["bestnava_count"], 1), "doppler_count_ratio": r0_total["doppler_count"] / max(raw_total["doppler_count"], 1), "duration_ratio": r0_total["duration_s"] / max(raw_total["duration_s"], 1e-9), }, "R0_to_qualified": { "bestnava_count_ratio": qualified_total["bestnava_count"] / max(r0_total["bestnava_count"], 1), "doppler_count_ratio": qualified_total["doppler_count"] / max(r0_total["doppler_count"], 1), "duration_ratio": qualified_total["duration_s"] / max(r0_total["duration_s"], 1e-9), }, } audit = { "session_id": session.session_id, "batch_id": session.batch_id, "raw_valid_intervals": raw_valid, "R0_after_cuts_intervals": r0_intervals, "qualified_segments": qualified_intervals, "stage_statistics": { "raw_valid": _stage_statistics(session, raw_valid), "R0_after_cuts": _stage_statistics(session, r0_intervals), "qualified": _stage_statistics(session, qualified_intervals), }, "retention": retention, "cut_intervals": [*raw_rejected, *decimation_cuts, *r0_cuts, *dropped_r0], "r0_consecutive_node_intervals": r0_node_intervals, "interval_penetration": _interval_penetration( raw_valid, r0_intervals, qualified_intervals, [*raw_rejected, *decimation_cuts, *r0_cuts, *dropped_r0], ), "hpr_chain_diagnostics": _hpr_chain_diagnostics(hpr), "axis_mapping": _hpr_axis_mapping(session, hpr), "unit_check": _unit_check(session), "bias_absorption_check": _bias_absorption_check(session, raw_valid), } audit["candidate_scores"] = _candidate_scores(audit) return audit def main(argv: list[str] | None = None) -> 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("--sample-period-s", type=float, default=1.0) parser.add_argument("--session", action="append", help="Optional session id; may repeat.") parser.add_argument( "--inventory-only", action="store_true", help="Only scan raw-valid gyro/HPR excitation; skip R0 and qualified-segment work.", ) args = parser.parse_args(argv) sessions = load_unified_sessions( args.manifest, selected_session_ids=None if args.session is None else set(args.session), ) if args.inventory_only: audits = [] for session in sessions: hpr = _all_hpr(session) best_rows = session.rtk_by_type.get("BESTNAVA", []) records = _raw_records(session) raw_valid = _coalesce(records, include=True, label="raw_valid", session=session, best_rows=best_rows, hpr=hpr) audit = { "session_id": session.session_id, "batch_id": session.batch_id, "raw_valid_intervals": raw_valid, "hpr_chain_diagnostics": _hpr_chain_diagnostics(hpr), "axis_mapping": _hpr_axis_mapping(session, hpr), "unit_check": _unit_check(session), } audit["candidate_scores"] = _candidate_scores(audit) audits.append(audit) scope = "raw-valid motion inventory only; no R0/qualified work or optimisation" else: audits = [_audit_session(session, args.sample_period_s) for session in sessions] scope = "motion-excitation penetration audit only; no lever fit/prior/bootstrap/sensitivity" payload = { "scope": scope, "sample_period_s": args.sample_period_s, "session_count": len(audits), "selected_dynamic_candidates": _select_candidates(audits), "sessions": audits, } args.output.parent.mkdir(parents=True, exist_ok=True) args.output.write_text(json.dumps(_jsonable(payload), ensure_ascii=False, indent=2, allow_nan=False) + "\n", encoding="utf-8") print(json.dumps({ "session_count": len(audits), "selected_dynamic_candidates": payload["selected_dynamic_candidates"], }, ensure_ascii=False, indent=2)) return 0 if __name__ == "__main__": raise SystemExit(main())