668 lines
31 KiB
Python
668 lines
31 KiB
Python
#!/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 rtk_imu.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 rtk_imu.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()) |