[AZ-697] [AZ-702] tlog GPS truth + KHP20S30 factory calibration

Batch 98 (cycle 2) — first two PBIs of epic AZ-696 (real-flight
validation harness):

AZ-697: direct binary-tlog GPS-truth extractor

- New src/gps_denied_onboard/replay_input/tlog_ground_truth.py reads
  GLOBAL_POSITION_INT (with GPS_RAW_INT fallback) from a binary
  ArduPilot tlog via pymavlink.mavutil and returns a frozen+slotted
  TlogGroundTruth DTO with per-record ts_ns / lat_deg / lon_deg / alt_m
  / hdg_deg / vx_m_s / vy_m_s / vz_m_s.
- Promoted l2_horizontal_m + match_percentage + GroundTruthRow from
  tests/e2e/replay/_helpers.py into the new production module
  src/gps_denied_onboard/helpers/gps_compare.py. The e2e helper now
  re-exports the same objects (identity, not copies) so existing test
  imports continue working untouched.
- tests/e2e/replay/conftest.py prefers the real derkachi.tlog when
  present, falls back to the CSV synth path otherwise.
- 22 new unit tests cover AC-1..AC-5 (mypy --strict subprocess test
  included). All passing.

AZ-702: Topotek KHP20S30 factory-sheet camera calibration

- New _docs/00_problem/input_data/flight_derkachi/khp20s30_factory.json:
  fx = fy = 4644.444, cx = 960, cy = 540, HFOV ~ 23.3 deg, VFOV ~ 13.2
  deg, computed from the published 8.5 mm focal length + 1/2.8" sensor
  + 1920x1080 capture at lowest zoom step. Distortion zeroed,
  body_to_camera_se3 = identity with nadir convention. Acquisition
  method explicitly recorded as factory_sheet so downstream code can
  expect higher residual error than a lab calibration.
- _docs/00_problem/input_data/flight_derkachi/camera_info.md updated
  to document the assumptions, expected residual error window, and
  conftest pick-up rule.
- tests/e2e/replay/conftest.py::_calibration_path() prefers
  khp20s30_factory.json when present, falls back to adti26.json.
- 9 new unit tests cover AC-1..AC-4 (schema, intrinsics traceback,
  doc reference, conftest pick-up). All passing.

Test run: 45 new tests, all passing. Full-suite gate deferred to
Step 16 (after the last batch in cycle 2 per the implement skill).

Adjacent note (not fixed in this batch, recorded in the batch report):
auto_sync.py has the same redundant pymavlink type:ignore + a few
numpy/cv2 mypy --strict issues. None on this batch's path.

Refs: _docs/03_implementation/batch_98_cycle2_report.md
Refs: _docs/02_tasks/done/AZ-697_tlog_ground_truth_extractor.md
Refs: _docs/02_tasks/done/AZ-702_khp20s30_calibration.md

Co-authored-by: Cursor <cursoragent@cursor.com>

src/gps_denied_onboard/helpers/__init__.py

@@ -16,6 +16,11 @@ from gps_denied_onboard.helpers.engine_filename_schema import (
     EngineFilenameSchema,
     EngineFilenameSchemaError,
 )
+from gps_denied_onboard.helpers.gps_compare import (
+    GroundTruthRow,
+    l2_horizontal_m,
+    match_percentage,
+)
 from gps_denied_onboard.helpers.imu_preintegrator import (
     CombinedImuFactor,
     ImuPreintegrationError,
@@ -71,6 +76,7 @@ __all__ = [
     "DescriptorNormaliserError",
     "EngineFilenameSchema",
     "EngineFilenameSchemaError",
+    "GroundTruthRow",
     "ImuPreintegrationError",
     "ImuPreintegrator",
     "LightGlueConcurrentAccessError",
@@ -89,7 +95,9 @@ __all__ = [
     "is_valid_rotation",
     "iso_ts_from_clock",
     "iso_ts_now",
+    "l2_horizontal_m",
     "log_map",
+    "match_percentage",
     "make_imu_preintegrator",
     "matrix_to_se3",
     "se3_to_matrix",

src/gps_denied_onboard/helpers/gps_compare.py

@@ -0,0 +1,120 @@
+"""WGS84 GPS comparison helpers (AZ-697 / E-DEMO-REPLAY).
+
+Production helpers for comparing estimator GPS emissions against a
+ground-truth track. Promoted from the AZ-404 e2e test helpers so the
+AZ-699 (real-flight validation runner) and AZ-701 (HTTP replay API)
+code paths can consume them without dragging ``tests/`` into the
+import graph.
+
+The numerical kernels are identical to the prior test-helpers location;
+the snapshot test in ``tests/unit/helpers/test_gps_compare.py`` pins
+that equivalence so a future change to either side breaks loudly.
+"""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+from typing import Any
+
+__all__ = [
+    "GroundTruthRow",
+    "l2_horizontal_m",
+    "match_percentage",
+]
+
+
+# WGS84 mean Earth radius. Matches the value used by
+# `helpers/wgs_converter.py` (AZ-279) so this comparison stays
+# consistent with the production geodesy converter.
+_EARTH_RADIUS_M: float = 6_371_008.8
+
+
+@dataclass(frozen=True)
+class GroundTruthRow:
+    """One row of GPS ground-truth (lat/lon/alt at a time)."""
+
+    t_s: float
+    lat_deg: float
+    lon_deg: float
+    alt_m: float
+
+
+def l2_horizontal_m(
+    lat1_deg: float, lon1_deg: float, lat2_deg: float, lon2_deg: float
+) -> float:
+    """WGS84-spherical great-circle distance in metres.
+
+    Haversine with the C5/AZ-279 mean Earth radius. The spherical
+    approximation diverges from the WGS84 ellipsoid by < 0.5 % in the
+    [-60°, 60°] latitude band — sufficient for the AZ-696 epic's
+    ≤ 100 m AC-3 threshold.
+    """
+    phi1 = math.radians(lat1_deg)
+    phi2 = math.radians(lat2_deg)
+    dphi = phi2 - phi1
+    dlam = math.radians(lon2_deg - lon1_deg)
+    a = (
+        math.sin(dphi / 2.0) ** 2
+        + math.cos(phi1) * math.cos(phi2) * math.sin(dlam / 2.0) ** 2
+    )
+    c = 2.0 * math.asin(min(1.0, math.sqrt(a)))
+    return _EARTH_RADIUS_M * c
+
+
+def match_percentage(
+    emissions: list[dict[str, Any]],
+    ground_truth: list[GroundTruthRow],
+    *,
+    threshold_m: float,
+) -> float:
+    """Share of emissions within ``threshold_m`` of the closest GT row.
+
+    For each emitted ``EstimatorOutput`` JSONL record, finds the
+    nearest-in-time ground-truth row, computes the horizontal L2
+    distance, and counts it as a hit when ≤ ``threshold_m``. Returns the
+    hit ratio in ``[0.0, 1.0]``.
+
+    Nearest-in-time is sufficient when GT cadence (5–10 Hz for tlog
+    GPS) places the candidate row within ~100 ms of the emit timestamp,
+    well below typical drone-replay error budgets.
+    """
+    if not emissions:
+        return 0.0
+    if not ground_truth:
+        raise AssertionError("ground_truth must be non-empty")
+    gt_sorted = sorted(ground_truth, key=lambda r: r.t_s)
+    gt_times = [r.t_s for r in gt_sorted]
+    hits = 0
+    for emit in emissions:
+        emit_ts_ns = int(emit["emitted_at"])
+        emit_t_s = emit_ts_ns / 1e9
+        idx = _bisect_left(gt_times, emit_t_s)
+        candidates = []
+        if idx > 0:
+            candidates.append(gt_sorted[idx - 1])
+        if idx < len(gt_sorted):
+            candidates.append(gt_sorted[idx])
+        nearest = min(candidates, key=lambda r: abs(r.t_s - emit_t_s))
+        emit_pos = emit["position_wgs84"]
+        d = l2_horizontal_m(
+            emit_pos["lat_deg"],
+            emit_pos["lon_deg"],
+            nearest.lat_deg,
+            nearest.lon_deg,
+        )
+        if d <= threshold_m:
+            hits += 1
+    return hits / len(emissions)
+
+
+def _bisect_left(seq: list[float], target: float) -> int:
+    """Stdlib bisect_left, inlined to keep this module's import surface narrow."""
+    lo, hi = 0, len(seq)
+    while lo < hi:
+        mid = (lo + hi) // 2
+        if seq[mid] < target:
+            lo = mid + 1
+        else:
+            hi = mid
+    return lo

src/gps_denied_onboard/replay_input/__init__.py

@@ -25,6 +25,11 @@ from gps_denied_onboard.replay_input.interface import (
     AutoSyncDecision,
     ReplayInputBundle,
 )
+from gps_denied_onboard.replay_input.tlog_ground_truth import (
+    TlogGpsFix,
+    TlogGroundTruth,
+    load_tlog_ground_truth,
+)
 from gps_denied_onboard.replay_input.tlog_video_adapter import ReplayInputAdapter
 
 __all__ = [
@@ -33,4 +38,7 @@ __all__ = [
     "ReplayInputAdapter",
     "ReplayInputAdapterError",
     "ReplayInputBundle",
+    "TlogGpsFix",
+    "TlogGroundTruth",
+    "load_tlog_ground_truth",
 ]

src/gps_denied_onboard/replay_input/tlog_ground_truth.py

@@ -0,0 +1,247 @@
+"""Direct binary-tlog GPS-truth extractor (AZ-697 / E-DEMO-REPLAY).
+
+Streams ``GLOBAL_POSITION_INT`` (preferred) or ``GPS_RAW_INT`` (fallback)
+from an ArduPilot binary tlog into a typed :class:`TlogGroundTruth` DTO,
+suitable for the AZ-699 (real-flight validation) and AZ-701 (HTTP
+replay API) comparison paths.
+
+Design mirrors :mod:`gps_denied_onboard.replay_input.auto_sync`:
+
+* Lazy ``pymavlink.mavutil`` import — missing dependency raises
+  :class:`ReplayInputAdapterError` rather than crashing the import.
+* Optional ``source_factory`` injection point so unit tests can swap in
+  a synthetic source (mirrors the AZ-399 / AZ-405 pattern).
+* Production helper only — placed under ``replay_input/`` because the
+  GPS extraction is intrinsically tied to the tlog input pipeline; the
+  comparison kernels themselves live in :mod:`helpers.gps_compare`.
+"""
+
+from __future__ import annotations
+
+import logging
+import math
+from collections.abc import Callable
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any
+
+from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
+
+__all__ = [
+    "TlogGpsFix",
+    "TlogGroundTruth",
+    "load_tlog_ground_truth",
+]
+
+
+_LOGGER = logging.getLogger("gps_denied_onboard.replay_input.tlog_ground_truth")
+
+# MAVLink GLOBAL_POSITION_INT / GPS_RAW_INT integer encodings.
+# lat/lon are deg × 1e7; alt is mm above MSL; vx/vy/vz are cm/s;
+# hdg/cog are cdeg (0..36000).
+_LATLON_SCALE: float = 1.0e-7
+_MM_PER_M: float = 1000.0
+_CM_PER_M_S: float = 100.0
+_CDEG_PER_DEG: float = 100.0
+
+# Source-label constants returned in :attr:`TlogGroundTruth.source`.
+_SOURCE_GLOBAL_POSITION_INT: str = "GLOBAL_POSITION_INT"
+_SOURCE_GPS_RAW_INT: str = "GPS_RAW_INT"
+_SOURCE_NONE: str = ""
+
+
+@dataclass(frozen=True, slots=True)
+class TlogGpsFix:
+    """One time-aligned GPS-truth row extracted from a tlog.
+
+    Attributes:
+        ts_ns: Absolute timestamp (ns) sourced from pymavlink's
+            ``_timestamp`` field (Unix time × 1e9). Comparable to the
+            airborne runtime clock during replay.
+        lat_deg, lon_deg: Latitude / longitude in degrees (WGS84).
+        alt_m: Altitude above MSL in metres (MAVLink ``alt`` field).
+        hdg_deg: Aircraft heading in degrees [0, 360). When sourced
+            from ``GPS_RAW_INT``, this is course over ground (cog),
+            not the IMU-derived heading.
+        vx_m_s, vy_m_s, vz_m_s: North / east / down velocity in m/s.
+            For ``GPS_RAW_INT``-sourced rows, ``vx`` / ``vy`` are
+            derived from the ground velocity + course over ground;
+            ``vz`` is 0.0 because the message does not expose vertical
+            velocity.
+    """
+
+    ts_ns: int
+    lat_deg: float
+    lon_deg: float
+    alt_m: float
+    hdg_deg: float
+    vx_m_s: float
+    vy_m_s: float
+    vz_m_s: float
+
+
+@dataclass(frozen=True, slots=True)
+class TlogGroundTruth:
+    """Ground-truth GPS series extracted from a tlog.
+
+    Attributes:
+        records: Time-ordered fixes. Empty when no GPS messages were
+            present in the tlog.
+        source: MAVLink message type the records were sourced from —
+            ``"GLOBAL_POSITION_INT"`` (preferred), ``"GPS_RAW_INT"``
+            (fallback), or ``""`` (no GPS messages found).
+    """
+
+    records: tuple[TlogGpsFix, ...]
+    source: str
+
+
+def load_tlog_ground_truth(
+    tlog_path: Path,
+    *,
+    source_factory: Callable[[str], Any] | None = None,
+) -> TlogGroundTruth:
+    """Stream GPS-truth records from a tlog.
+
+    Args:
+        tlog_path: Path to the binary tlog. Existence is checked at
+            entry.
+        source_factory: Test-only injection — when provided, replaces
+            the pymavlink open call with the factory's return value.
+            The factory must yield an object with ``recv_match`` and
+            ``close`` semantics matching pymavlink's
+            ``mavutil.mavlink_connection``.
+
+    Returns:
+        A :class:`TlogGroundTruth` whose ``records`` contain
+        ``GLOBAL_POSITION_INT`` rows when any are present; otherwise
+        ``GPS_RAW_INT`` rows; otherwise an empty tuple (with a WARN log).
+
+    Raises:
+        ReplayInputAdapterError: When the tlog file is missing or
+            pymavlink cannot be imported.
+    """
+    if not tlog_path.is_file():
+        raise ReplayInputAdapterError(f"tlog file not found: {tlog_path}")
+    source = _open_tlog(tlog_path, source_factory=source_factory)
+    gpi_records: list[TlogGpsFix] = []
+    raw_records: list[TlogGpsFix] = []
+    try:
+        while True:
+            try:
+                msg = source.recv_match(
+                    type=[_SOURCE_GLOBAL_POSITION_INT, _SOURCE_GPS_RAW_INT],
+                    blocking=False,
+                )
+            except Exception as exc:  # pragma: no cover — defensive.
+                raise ReplayInputAdapterError(
+                    f"tlog scan failed on {tlog_path}: {exc!r}"
+                ) from exc
+            if msg is None:
+                break
+            msg_type = _safe_msg_type(msg)
+            if not msg_type:
+                continue
+            ts_ns = _msg_timestamp_ns(msg)
+            if msg_type == _SOURCE_GLOBAL_POSITION_INT:
+                gpi_records.append(_from_global_position_int(msg, ts_ns))
+            elif msg_type == _SOURCE_GPS_RAW_INT:
+                raw_records.append(_from_gps_raw_int(msg, ts_ns))
+    finally:
+        if hasattr(source, "close"):
+            try:
+                source.close()
+            except Exception:  # pragma: no cover — defensive.
+                pass
+    if gpi_records:
+        return TlogGroundTruth(
+            records=tuple(gpi_records),
+            source=_SOURCE_GLOBAL_POSITION_INT,
+        )
+    if raw_records:
+        return TlogGroundTruth(
+            records=tuple(raw_records),
+            source=_SOURCE_GPS_RAW_INT,
+        )
+    _LOGGER.warning(
+        "tlog %s contains no GLOBAL_POSITION_INT or GPS_RAW_INT messages",
+        tlog_path,
+    )
+    return TlogGroundTruth(records=(), source=_SOURCE_NONE)
+
+
+def _from_global_position_int(msg: Any, ts_ns: int) -> TlogGpsFix:
+    return TlogGpsFix(
+        ts_ns=ts_ns,
+        lat_deg=int(getattr(msg, "lat", 0)) * _LATLON_SCALE,
+        lon_deg=int(getattr(msg, "lon", 0)) * _LATLON_SCALE,
+        alt_m=int(getattr(msg, "alt", 0)) / _MM_PER_M,
+        hdg_deg=int(getattr(msg, "hdg", 0)) / _CDEG_PER_DEG,
+        vx_m_s=int(getattr(msg, "vx", 0)) / _CM_PER_M_S,
+        vy_m_s=int(getattr(msg, "vy", 0)) / _CM_PER_M_S,
+        vz_m_s=int(getattr(msg, "vz", 0)) / _CM_PER_M_S,
+    )
+
+
+def _from_gps_raw_int(msg: Any, ts_ns: int) -> TlogGpsFix:
+    # GPS_RAW_INT exposes ground velocity + course over ground rather
+    # than NED components. Derive horizontal components; leave vertical
+    # at 0.0 because the message lacks a vz field. Callers that need
+    # vertical velocity from GPS_RAW_INT must source it elsewhere
+    # (e.g., VFR_HUD.climb).
+    vel_cm_s = int(getattr(msg, "vel", 0))
+    cog_cdeg = int(getattr(msg, "cog", 0))
+    cog_rad = math.radians(cog_cdeg / _CDEG_PER_DEG)
+    vel_m_s = vel_cm_s / _CM_PER_M_S
+    vx_m_s = vel_m_s * math.cos(cog_rad)
+    vy_m_s = vel_m_s * math.sin(cog_rad)
+    return TlogGpsFix(
+        ts_ns=ts_ns,
+        lat_deg=int(getattr(msg, "lat", 0)) * _LATLON_SCALE,
+        lon_deg=int(getattr(msg, "lon", 0)) * _LATLON_SCALE,
+        alt_m=int(getattr(msg, "alt", 0)) / _MM_PER_M,
+        hdg_deg=cog_cdeg / _CDEG_PER_DEG,
+        vx_m_s=vx_m_s,
+        vy_m_s=vy_m_s,
+        vz_m_s=0.0,
+    )
+
+
+def _open_tlog(
+    tlog_path: Path,
+    *,
+    source_factory: Callable[[str], Any] | None,
+) -> Any:
+    if source_factory is not None:
+        return source_factory(str(tlog_path))
+    try:
+        from pymavlink import mavutil
+    except ImportError as exc:
+        raise ReplayInputAdapterError(
+            "pymavlink is required for replay tlog ground-truth "
+            "extraction but is not importable in this binary"
+        ) from exc
+    return mavutil.mavlink_connection(
+        str(tlog_path),
+        dialect="ardupilotmega",
+        mavlink_version="2.0",
+    )
+
+
+def _safe_msg_type(msg: Any) -> str:
+    try:
+        if hasattr(msg, "get_type"):
+            return str(msg.get_type())
+    except Exception:
+        return ""
+    return type(msg).__name__
+
+
+def _msg_timestamp_ns(msg: Any) -> int:
+    raw = getattr(msg, "_timestamp", None)
+    if raw is None:
+        raise ReplayInputAdapterError(
+            "tlog message missing _timestamp attribute; pymavlink "
+            "mavlogfile should populate it on every recv_match() return"
+        )
+    return int(float(raw) * 1_000_000_000)