mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-21 21:01:12 +00:00
Oleksandr Bezdieniezhnykh
94d2358c8b
Derkachi e2e Tier-2 divergence had three stacked root causes; this
commit ships fixes for all three plus the IMU prerequisite they
depend on, plus a baseline cheirality gate for cv2.recoverPose.
AZ-918 MAVLink IMU adapters now convert raw mG/mrad-s + FRD body to
SI m/s^2 + rad/s + FLU body via helpers.imu_units. Without
this the ESKF receives values ~1000x too small with wrong-
sign Y/Z and cannot function at all.
AZ-919 Composition root wires EskfNominalAltitudeProvider into the
KLT/RANSAC strategy via the AZ-331 factory introspect path;
OKVIS2 and VINS-Mono are unaffected.
AZ-920 KLT/RANSAC recovers metric translation via Ground Sampling
Distance when AGL is available; otherwise falls through with
scale_quality=direction_only/unknown (no fake scale invented).
AZ-921 VioOutput.scale_quality signal; ESKF add_vio adapts R_meas
position block based on the flag (1e6 inflation when scale is
direction_only/unknown to keep the filter consistent).
AZ-922 KLT/RANSAC cheirality gate rejects single-frame rotations
beyond a config threshold (default 30 deg), catching
cv2.recoverPose twisted-pair flips that cause immediate ESKF
divergence on low-parallax aerial scenes.
Verification:
- Tier-1 (macOS) unit suite: 2346 passed, 0 failed.
- Tier-2 (Jetson) Derkachi e2e: divergence moves from frame 5
(mahalanobis^2 3757) to frame 233 (mahalanobis^2 212). Remaining
drift is open-loop attitude accumulation, not cheirality.
Follow-up tickets filed:
- AZ-923 closed as misdiagnosed: EskfNominalAltitudeProvider was
already correct (nominal_pos.z IS the AGL when takeoff origin sits
at ground level); the early-frame AGL near zero reflects the drone
being stationary on the ground, not a provider bug.
- AZ-942 filed: cross-check VIO rotation against IMU preintegrator
(consistency gate) - more physically grounded than the coarse
AZ-922 threshold and likely required to absorb the frame-233 drift.
Co-authored-by: Cursor <cursoragent@cursor.com>
108 lines
3.3 KiB
Python
108 lines
3.3 KiB
Python
"""AZ-919 — `AltitudeProvider` Protocol + `EskfNominalAltitudeProvider` contract.
|
|
|
|
Pins the gating rules that the GSD scale-recovery work (AZ-920) and the
|
|
degraded-mode signal (AZ-921) build on top of.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
from typing import Any
|
|
|
|
import numpy as np
|
|
|
|
from gps_denied_onboard._types.state import IsamState
|
|
from gps_denied_onboard.helpers.altitude_provider import (
|
|
AltitudeProvider,
|
|
EskfNominalAltitudeProvider,
|
|
)
|
|
|
|
|
|
class _FakeEstimator:
|
|
"""Minimal ESKF stand-in exposing only the attrs the provider reads."""
|
|
|
|
def __init__(
|
|
self,
|
|
*,
|
|
nominal_pos_z: float,
|
|
takeoff_origin_set: bool,
|
|
isam2_state: IsamState,
|
|
) -> None:
|
|
self._nominal_pos = np.array([0.0, 0.0, nominal_pos_z], dtype=np.float64)
|
|
self._takeoff_origin_set: tuple[Any, float, float] | None = (
|
|
("origin-sentinel", 1.0, 1.0) if takeoff_origin_set else None
|
|
)
|
|
self._isam2_state = isam2_state
|
|
|
|
|
|
def test_provider_is_runtime_checkable_protocol() -> None:
|
|
provider = EskfNominalAltitudeProvider(estimator_supplier=lambda: None)
|
|
|
|
assert isinstance(provider, AltitudeProvider)
|
|
|
|
|
|
def test_returns_none_when_supplier_yields_none() -> None:
|
|
provider = EskfNominalAltitudeProvider(estimator_supplier=lambda: None)
|
|
|
|
assert provider.agl_m(now_ns=1_000_000_000) is None
|
|
|
|
|
|
def test_returns_none_before_takeoff_origin_is_anchored() -> None:
|
|
# Arrange — origin not yet set, even though nominal_pos has a value.
|
|
estimator = _FakeEstimator(
|
|
nominal_pos_z=42.0,
|
|
takeoff_origin_set=False,
|
|
isam2_state=IsamState.INIT,
|
|
)
|
|
provider = EskfNominalAltitudeProvider(estimator_supplier=lambda: estimator)
|
|
|
|
assert provider.agl_m(now_ns=1_000_000_000) is None
|
|
|
|
|
|
def test_returns_nominal_pos_z_after_origin_is_anchored() -> None:
|
|
# Arrange
|
|
estimator = _FakeEstimator(
|
|
nominal_pos_z=83.5,
|
|
takeoff_origin_set=True,
|
|
isam2_state=IsamState.TRACKING,
|
|
)
|
|
provider = EskfNominalAltitudeProvider(estimator_supplier=lambda: estimator)
|
|
|
|
# Act + Assert
|
|
assert provider.agl_m(now_ns=1_000_000_000) == 83.5
|
|
|
|
|
|
def test_returns_none_when_estimator_is_lost() -> None:
|
|
# Arrange — origin anchored and altitude is non-zero, but the
|
|
# filter has flipped to LOST; the AGL signal is no longer trustworthy.
|
|
estimator = _FakeEstimator(
|
|
nominal_pos_z=120.0,
|
|
takeoff_origin_set=True,
|
|
isam2_state=IsamState.LOST,
|
|
)
|
|
provider = EskfNominalAltitudeProvider(estimator_supplier=lambda: estimator)
|
|
|
|
assert provider.agl_m(now_ns=1_000_000_000) is None
|
|
|
|
|
|
def test_supplier_is_re_resolved_per_call() -> None:
|
|
# Arrange — the composition root builds C1 before C5, so the
|
|
# supplier must read from a mutable container at call time.
|
|
holder: dict[str, _FakeEstimator] = {}
|
|
provider = EskfNominalAltitudeProvider(
|
|
estimator_supplier=lambda: holder.get("c5_state"),
|
|
)
|
|
|
|
# Act — first call: C5 not yet built.
|
|
first = provider.agl_m(now_ns=1_000_000_000)
|
|
# Then C5 lands in the dict.
|
|
holder["c5_state"] = _FakeEstimator(
|
|
nominal_pos_z=37.0,
|
|
takeoff_origin_set=True,
|
|
isam2_state=IsamState.TRACKING,
|
|
)
|
|
second = provider.agl_m(now_ns=2_000_000_000)
|
|
|
|
# Assert
|
|
assert first is None
|
|
assert second == 37.0
|