mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 06:31:12 +00:00
[AZ-414] [AZ-415] [AZ-418] Test batch 71: sharp turn + multi-segment + smoothing
- AZ-414 (FT-P-07 + FT-N-02): sharp_turn_detector helper covering AC-1 (gyro_z run detection + synthetic-overlay fallback), AC-2/AC-3 (FT-N-02 during-turn label + monotonic covariance), AC-4/AC-5/AC-6 (FT-P-07 recovery lag/drift/heading); twin scenario files under positive/ and negative/. - AZ-415 (FT-P-08): multi_segment_evaluator helper + scenario. - AZ-418 (FT-P-10): smoothing_evaluator helper covering AC-1 (raw + smoothed pose pairing), AC-2 (improvement rate >= 0.80), AC-3 (mean improvement >= 5 m); scenario file. - All scenarios skip-gated on upstream frame_source_replay / imu_replay / fdr_reader stubs (auto-activate when AZ-441 + AZ-407 leftovers land). - +68 unit tests; full e2e unit suite: 393 passed. See _docs/03_implementation/batch_71_report.md and _docs/03_implementation/reviews/batch_71_review.md. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,483 @@
|
||||
"""Sharp-turn detection + FT-P-07 / FT-N-02 evaluation (AZ-414 / AC-3.2).
|
||||
|
||||
The Derkachi telemetry CSV records ``SCALED_IMU2.{xgyro, ygyro, zgyro}``
|
||||
in milli-degree/s. A "sharp turn" segment is identified by sustained
|
||||
high yaw rate (``|zgyro|``) — ≥``MIN_RUN_LENGTH`` consecutive IMU rows
|
||||
above ``DEFAULT_GYRO_Z_THRESHOLD_MDPS``. The threshold is read from
|
||||
``AC32_SHARP_TURN_GYRO_Z_MDPS`` env var if set (AZ-414 spec note —
|
||||
"reads from the test-spec environment, not from a hardcoded constant").
|
||||
|
||||
FT-N-02 (during turn):
|
||||
* AC-2: source_label ∈ {visual_propagated, dead_reckoned}.
|
||||
* AC-3: cov_semi_major_m non-decreasing across consecutive frames inside
|
||||
the turn.
|
||||
|
||||
FT-P-07 (recovery):
|
||||
* AC-4: next satellite_anchored emission within ≤3 frames after turn end.
|
||||
* AC-5: ``|propagated_centre_at_turn_end − recovery_anchor_centre| ≤ 200 m``.
|
||||
* AC-6: heading delta from pre-turn anchor to post-turn anchor handled
|
||||
in the [0°, 70°] envelope (recovery still occurs within the 3-frame budget).
|
||||
|
||||
Synthetic-overlay fallback: if no natural Derkachi segment meets the
|
||||
threshold, the FT-P-07 / FT-N-02 scenarios fall back to a synthetic
|
||||
gyro overlay. The fallback decision is recorded in the FDR / CSV
|
||||
``evidence_paths`` per the spec (this helper exposes a flag; the
|
||||
scenario writes the marker).
|
||||
|
||||
Public-boundary discipline: this module does NOT import any
|
||||
``src/gps_denied_onboard`` symbol.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import math
|
||||
import os
|
||||
from dataclasses import dataclass, field
|
||||
from pathlib import Path
|
||||
from typing import Sequence
|
||||
|
||||
from .geo import distance_m
|
||||
|
||||
DEFAULT_GYRO_Z_THRESHOLD_MDPS = 30_000 # 30 °/s sustained yaw
|
||||
MIN_RUN_LENGTH = 3 # consecutive IMU rows above threshold
|
||||
SHARP_TURN_ENV_VAR = "AC32_SHARP_TURN_GYRO_Z_MDPS"
|
||||
|
||||
MAX_RECOVERY_FRAMES = 3
|
||||
MAX_RECOVERY_FRAMES_SAFETY_MS = 1100 # 3 frames @ ~3 fps, +100 ms slack
|
||||
MAX_RECOVERY_DRIFT_M = 200.0
|
||||
MAX_HEADING_CHANGE_DEG = 70.0
|
||||
|
||||
ALLOWED_DURING_TURN_LABELS = {"visual_propagated", "dead_reckoned"}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GyroSample:
|
||||
"""One IMU row's yaw-rate sample (z axis) — millidegree/s."""
|
||||
|
||||
monotonic_ms: int
|
||||
time_s: float
|
||||
zgyro_mdps: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TurnSegment:
|
||||
"""A contiguous run of ≥MIN_RUN_LENGTH samples above the gyro threshold."""
|
||||
|
||||
start_index: int
|
||||
end_index: int # inclusive — the last sample still above threshold
|
||||
start_ms: int
|
||||
end_ms: int
|
||||
peak_abs_zgyro_mdps: int
|
||||
sample_count: int
|
||||
|
||||
@property
|
||||
def duration_ms(self) -> int:
|
||||
return self.end_ms - self.start_ms
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TurnDetection:
|
||||
"""Per-flight detection: zero or more segments + synthetic-overlay flag."""
|
||||
|
||||
segments: tuple[TurnSegment, ...]
|
||||
threshold_mdps: int
|
||||
synthetic_overlay: bool = False
|
||||
|
||||
@property
|
||||
def has_natural_turn(self) -> bool:
|
||||
return len(self.segments) > 0 and not self.synthetic_overlay
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TurnFrameSample:
|
||||
"""One outbound estimate observed during replay.
|
||||
|
||||
Same shape as ``multi_segment_evaluator.EstimateSample`` but with
|
||||
the additional ``cov_semi_major_m`` channel FT-N-02 AC-3 needs.
|
||||
"""
|
||||
|
||||
monotonic_ms: int
|
||||
lat_deg: float
|
||||
lon_deg: float
|
||||
source_label: str
|
||||
cov_semi_major_m: float
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FtN02WindowReport:
|
||||
"""FT-N-02 per-window report (during turn)."""
|
||||
|
||||
segment_index: int
|
||||
samples_inside: int
|
||||
label_violations: tuple[str, ...]
|
||||
cov_non_decreasing: bool
|
||||
first_decreasing_at_ms: int | None
|
||||
|
||||
@property
|
||||
def passes_label(self) -> bool:
|
||||
return self.samples_inside > 0 and not self.label_violations
|
||||
|
||||
@property
|
||||
def passes_cov(self) -> bool:
|
||||
return self.cov_non_decreasing
|
||||
|
||||
@property
|
||||
def passes(self) -> bool:
|
||||
return self.passes_label and self.passes_cov
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FtP07WindowReport:
|
||||
"""FT-P-07 per-window report (recovery)."""
|
||||
|
||||
segment_index: int
|
||||
recovery_anchor_ms: int | None
|
||||
recovery_lag_ms: int | None
|
||||
drift_m: float | None
|
||||
heading_change_deg: float | None
|
||||
in_heading_envelope: bool
|
||||
|
||||
@property
|
||||
def passes_recovery_lag(self) -> bool:
|
||||
return (
|
||||
self.recovery_lag_ms is not None
|
||||
and self.recovery_lag_ms <= MAX_RECOVERY_FRAMES_SAFETY_MS
|
||||
)
|
||||
|
||||
@property
|
||||
def passes_drift(self) -> bool:
|
||||
return self.drift_m is not None and self.drift_m <= MAX_RECOVERY_DRIFT_M
|
||||
|
||||
@property
|
||||
def passes_heading(self) -> bool:
|
||||
return self.in_heading_envelope
|
||||
|
||||
@property
|
||||
def passes(self) -> bool:
|
||||
return self.passes_recovery_lag and self.passes_drift and self.passes_heading
|
||||
|
||||
|
||||
def get_threshold_mdps() -> int:
|
||||
"""Read the AC-3.2 threshold from env, defaulting to the project value."""
|
||||
raw = os.environ.get(SHARP_TURN_ENV_VAR)
|
||||
if raw is None or raw.strip() == "":
|
||||
return DEFAULT_GYRO_Z_THRESHOLD_MDPS
|
||||
try:
|
||||
value = int(raw)
|
||||
except ValueError:
|
||||
raise ValueError(
|
||||
f"{SHARP_TURN_ENV_VAR}={raw!r} is not a valid int (millidegree/s)"
|
||||
)
|
||||
if value <= 0:
|
||||
raise ValueError(f"{SHARP_TURN_ENV_VAR}={raw!r} must be > 0")
|
||||
return value
|
||||
|
||||
|
||||
def load_zgyro_samples(csv_path: Path) -> list[GyroSample]:
|
||||
"""Read ``data_imu.csv`` and return per-row yaw-rate samples."""
|
||||
if not csv_path.exists():
|
||||
raise FileNotFoundError(
|
||||
f"data_imu.csv not found at {csv_path} — bind-mount the Derkachi fixture"
|
||||
)
|
||||
samples: list[GyroSample] = []
|
||||
with csv_path.open() as fh:
|
||||
reader = csv.DictReader(fh)
|
||||
if "SCALED_IMU2.zgyro" not in (reader.fieldnames or []):
|
||||
raise ValueError(
|
||||
"data_imu.csv missing required column SCALED_IMU2.zgyro"
|
||||
)
|
||||
for row in reader:
|
||||
ts = row.get("timestamp(ms)", "").strip()
|
||||
if not ts:
|
||||
continue
|
||||
samples.append(
|
||||
GyroSample(
|
||||
monotonic_ms=int(float(ts)),
|
||||
time_s=float(row["Time"]),
|
||||
zgyro_mdps=int(float(row["SCALED_IMU2.zgyro"])),
|
||||
)
|
||||
)
|
||||
return samples
|
||||
|
||||
|
||||
def detect_turn_segments(
|
||||
samples: Sequence[GyroSample],
|
||||
*,
|
||||
threshold_mdps: int | None = None,
|
||||
min_run_length: int = MIN_RUN_LENGTH,
|
||||
) -> TurnDetection:
|
||||
"""Find contiguous runs of |zgyro| ≥ threshold lasting ≥min_run_length samples."""
|
||||
if min_run_length < 1:
|
||||
raise ValueError(f"min_run_length must be ≥1, got {min_run_length}")
|
||||
threshold = threshold_mdps if threshold_mdps is not None else get_threshold_mdps()
|
||||
|
||||
segments: list[TurnSegment] = []
|
||||
run_start: int | None = None
|
||||
run_peak = 0
|
||||
for i, s in enumerate(samples):
|
||||
abs_z = abs(s.zgyro_mdps)
|
||||
if abs_z >= threshold:
|
||||
if run_start is None:
|
||||
run_start = i
|
||||
run_peak = abs_z
|
||||
else:
|
||||
run_peak = max(run_peak, abs_z)
|
||||
else:
|
||||
if run_start is not None:
|
||||
run_len = i - run_start
|
||||
if run_len >= min_run_length:
|
||||
seg_end = i - 1
|
||||
segments.append(
|
||||
TurnSegment(
|
||||
start_index=run_start,
|
||||
end_index=seg_end,
|
||||
start_ms=samples[run_start].monotonic_ms,
|
||||
end_ms=samples[seg_end].monotonic_ms,
|
||||
peak_abs_zgyro_mdps=run_peak,
|
||||
sample_count=run_len,
|
||||
)
|
||||
)
|
||||
run_start = None
|
||||
run_peak = 0
|
||||
# Tail run.
|
||||
if run_start is not None:
|
||||
run_len = len(samples) - run_start
|
||||
if run_len >= min_run_length:
|
||||
seg_end = len(samples) - 1
|
||||
segments.append(
|
||||
TurnSegment(
|
||||
start_index=run_start,
|
||||
end_index=seg_end,
|
||||
start_ms=samples[run_start].monotonic_ms,
|
||||
end_ms=samples[seg_end].monotonic_ms,
|
||||
peak_abs_zgyro_mdps=run_peak,
|
||||
sample_count=run_len,
|
||||
)
|
||||
)
|
||||
|
||||
return TurnDetection(
|
||||
segments=tuple(segments),
|
||||
threshold_mdps=threshold,
|
||||
synthetic_overlay=False,
|
||||
)
|
||||
|
||||
|
||||
def synthesize_overlay_segment(
|
||||
samples: Sequence[GyroSample],
|
||||
*,
|
||||
threshold_mdps: int,
|
||||
anchor_fraction: float = 0.5,
|
||||
duration_samples: int = MIN_RUN_LENGTH * 2,
|
||||
) -> TurnDetection:
|
||||
"""Construct a synthetic-overlay turn anchored at ``anchor_fraction`` of the flight.
|
||||
|
||||
The scenario only needs the segment's time bounds to evaluate the
|
||||
SUT's behaviour — the overlay does NOT modify the IMU stream
|
||||
(that's an injector concern). The detection result is marked
|
||||
``synthetic_overlay=True`` so the scenario writes the flag into
|
||||
the evidence CSV.
|
||||
"""
|
||||
if not samples:
|
||||
raise ValueError("samples must not be empty")
|
||||
if not 0.0 <= anchor_fraction <= 1.0:
|
||||
raise ValueError(f"anchor_fraction must be in [0, 1], got {anchor_fraction}")
|
||||
if duration_samples < MIN_RUN_LENGTH:
|
||||
raise ValueError(
|
||||
f"duration_samples must be ≥{MIN_RUN_LENGTH}, got {duration_samples}"
|
||||
)
|
||||
anchor_idx = int(anchor_fraction * (len(samples) - 1))
|
||||
end_idx = min(len(samples) - 1, anchor_idx + duration_samples - 1)
|
||||
seg = TurnSegment(
|
||||
start_index=anchor_idx,
|
||||
end_index=end_idx,
|
||||
start_ms=samples[anchor_idx].monotonic_ms,
|
||||
end_ms=samples[end_idx].monotonic_ms,
|
||||
peak_abs_zgyro_mdps=threshold_mdps, # synthesised; not natural
|
||||
sample_count=end_idx - anchor_idx + 1,
|
||||
)
|
||||
return TurnDetection(
|
||||
segments=(seg,),
|
||||
threshold_mdps=threshold_mdps,
|
||||
synthetic_overlay=True,
|
||||
)
|
||||
|
||||
|
||||
def detect_or_synthesize(csv_path: Path) -> TurnDetection:
|
||||
"""Convenience: detect a natural turn; if none, synthesise an overlay.
|
||||
|
||||
The scenario uses this to keep the FT-P-07 / FT-N-02 paths covered
|
||||
on the natural fixture even when the data doesn't naturally include
|
||||
a sharp-turn segment.
|
||||
"""
|
||||
samples = load_zgyro_samples(csv_path)
|
||||
natural = detect_turn_segments(samples)
|
||||
if natural.has_natural_turn:
|
||||
return natural
|
||||
return synthesize_overlay_segment(samples, threshold_mdps=natural.threshold_mdps)
|
||||
|
||||
|
||||
def _heading_change_deg(
|
||||
pre_lat: float, pre_lon: float,
|
||||
mid_lat: float, mid_lon: float,
|
||||
post_lat: float, post_lon: float,
|
||||
) -> float:
|
||||
"""Heading delta from (pre→mid) to (mid→post), wrapped to [0, 180]."""
|
||||
def bearing(a_lat: float, a_lon: float, b_lat: float, b_lon: float) -> float:
|
||||
from .geo import delta
|
||||
return delta(a_lat, a_lon, b_lat, b_lon).forward_bearing_deg
|
||||
|
||||
pre_bearing = bearing(pre_lat, pre_lon, mid_lat, mid_lon)
|
||||
post_bearing = bearing(mid_lat, mid_lon, post_lat, post_lon)
|
||||
diff = (post_bearing - pre_bearing) % 360.0
|
||||
if diff > 180.0:
|
||||
diff = 360.0 - diff
|
||||
return diff
|
||||
|
||||
|
||||
def evaluate_ft_n_02(
|
||||
segment: TurnSegment,
|
||||
segment_index: int,
|
||||
samples: Sequence[TurnFrameSample],
|
||||
) -> FtN02WindowReport:
|
||||
"""FT-N-02 per-window evaluation (during-turn label + monotonic covariance)."""
|
||||
inside = [s for s in samples if segment.start_ms <= s.monotonic_ms <= segment.end_ms]
|
||||
label_violations = tuple(
|
||||
sorted({s.source_label for s in inside if s.source_label not in ALLOWED_DURING_TURN_LABELS})
|
||||
)
|
||||
|
||||
cov_non_decreasing = True
|
||||
first_decreasing: int | None = None
|
||||
prev_cov = -math.inf
|
||||
for s in inside:
|
||||
if s.cov_semi_major_m < prev_cov:
|
||||
cov_non_decreasing = False
|
||||
first_decreasing = s.monotonic_ms
|
||||
break
|
||||
prev_cov = s.cov_semi_major_m
|
||||
|
||||
return FtN02WindowReport(
|
||||
segment_index=segment_index,
|
||||
samples_inside=len(inside),
|
||||
label_violations=label_violations,
|
||||
cov_non_decreasing=cov_non_decreasing,
|
||||
first_decreasing_at_ms=first_decreasing,
|
||||
)
|
||||
|
||||
|
||||
def evaluate_ft_p_07(
|
||||
segment: TurnSegment,
|
||||
segment_index: int,
|
||||
samples: Sequence[TurnFrameSample],
|
||||
) -> FtP07WindowReport:
|
||||
"""FT-P-07 per-window evaluation (recovery anchor, drift, heading)."""
|
||||
# Pre-turn anchor: last satellite_anchored at or before start_ms.
|
||||
pre_anchor: TurnFrameSample | None = None
|
||||
for s in samples:
|
||||
if s.monotonic_ms <= segment.start_ms and s.source_label == "satellite_anchored":
|
||||
pre_anchor = s
|
||||
elif s.monotonic_ms > segment.start_ms:
|
||||
break
|
||||
|
||||
# Last inside-window sample (propagated centre at turn end).
|
||||
inside = [s for s in samples if segment.start_ms <= s.monotonic_ms <= segment.end_ms]
|
||||
propagated_at_end = inside[-1] if inside else None
|
||||
|
||||
# Recovery: first satellite_anchored after end_ms.
|
||||
recovery: TurnFrameSample | None = None
|
||||
for s in samples:
|
||||
if s.monotonic_ms > segment.end_ms and s.source_label == "satellite_anchored":
|
||||
recovery = s
|
||||
break
|
||||
|
||||
if recovery is None:
|
||||
return FtP07WindowReport(
|
||||
segment_index=segment_index,
|
||||
recovery_anchor_ms=None,
|
||||
recovery_lag_ms=None,
|
||||
drift_m=None,
|
||||
heading_change_deg=None,
|
||||
in_heading_envelope=False,
|
||||
)
|
||||
|
||||
recovery_lag = recovery.monotonic_ms - segment.end_ms
|
||||
drift: float | None = None
|
||||
if propagated_at_end is not None:
|
||||
drift = distance_m(
|
||||
propagated_at_end.lat_deg, propagated_at_end.lon_deg,
|
||||
recovery.lat_deg, recovery.lon_deg,
|
||||
)
|
||||
|
||||
heading_change: float | None = None
|
||||
in_envelope = True
|
||||
if pre_anchor is not None and propagated_at_end is not None:
|
||||
heading_change = _heading_change_deg(
|
||||
pre_anchor.lat_deg, pre_anchor.lon_deg,
|
||||
propagated_at_end.lat_deg, propagated_at_end.lon_deg,
|
||||
recovery.lat_deg, recovery.lon_deg,
|
||||
)
|
||||
in_envelope = heading_change <= MAX_HEADING_CHANGE_DEG
|
||||
|
||||
return FtP07WindowReport(
|
||||
segment_index=segment_index,
|
||||
recovery_anchor_ms=recovery.monotonic_ms,
|
||||
recovery_lag_ms=recovery_lag,
|
||||
drift_m=drift,
|
||||
heading_change_deg=heading_change,
|
||||
in_heading_envelope=in_envelope,
|
||||
)
|
||||
|
||||
|
||||
def write_csv_evidence(
|
||||
out_path: Path,
|
||||
detection: TurnDetection,
|
||||
n02_reports: Sequence[FtN02WindowReport],
|
||||
p07_reports: Sequence[FtP07WindowReport],
|
||||
) -> Path:
|
||||
"""Write FT-P-07 + FT-N-02 combined evidence CSV.
|
||||
|
||||
The ``synthetic_overlay`` column marks the fallback case per AC-1.
|
||||
"""
|
||||
out_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with out_path.open("w", newline="") as fh:
|
||||
writer = csv.writer(fh)
|
||||
writer.writerow(
|
||||
[
|
||||
"segment_index",
|
||||
"start_ms",
|
||||
"end_ms",
|
||||
"peak_abs_zgyro_mdps",
|
||||
"synthetic_overlay",
|
||||
# FT-N-02 columns
|
||||
"samples_inside",
|
||||
"label_violations",
|
||||
"cov_non_decreasing",
|
||||
# FT-P-07 columns
|
||||
"recovery_lag_ms",
|
||||
"drift_m",
|
||||
"heading_change_deg",
|
||||
"in_heading_envelope",
|
||||
"passes_ft_n_02",
|
||||
"passes_ft_p_07",
|
||||
]
|
||||
)
|
||||
for seg, n02, p07 in zip(detection.segments, n02_reports, p07_reports):
|
||||
writer.writerow(
|
||||
[
|
||||
n02.segment_index,
|
||||
seg.start_ms,
|
||||
seg.end_ms,
|
||||
seg.peak_abs_zgyro_mdps,
|
||||
"true" if detection.synthetic_overlay else "false",
|
||||
n02.samples_inside,
|
||||
"|".join(n02.label_violations) if n02.label_violations else "",
|
||||
"true" if n02.cov_non_decreasing else "false",
|
||||
"" if p07.recovery_lag_ms is None else p07.recovery_lag_ms,
|
||||
"" if p07.drift_m is None else f"{p07.drift_m:.3f}",
|
||||
"" if p07.heading_change_deg is None else f"{p07.heading_change_deg:.3f}",
|
||||
"true" if p07.in_heading_envelope else "false",
|
||||
"true" if n02.passes else "false",
|
||||
"true" if p07.passes else "false",
|
||||
]
|
||||
)
|
||||
return out_path
|
||||
Reference in New Issue
Block a user