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>
96 lines
3.2 KiB
Python
96 lines
3.2 KiB
Python
"""AZ-919 — `_c1_vio_wrapper` builds and forwards an `EskfNominalAltitudeProvider`.
|
|
|
|
Tests the composition-root seam: the AZ-919 plumbing is only useful if
|
|
the wrapper actually constructs the provider and passes it to
|
|
``build_vio_strategy``. The provider's supplier must close over the same
|
|
mutable ``constructed`` dict that the bootstrap loop populates so it
|
|
sees the C5 estimator after the topo order builds it.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
from typing import Any
|
|
|
|
import pytest
|
|
|
|
from gps_denied_onboard.config import Config
|
|
from gps_denied_onboard.helpers.altitude_provider import (
|
|
AltitudeProvider,
|
|
EskfNominalAltitudeProvider,
|
|
)
|
|
from gps_denied_onboard.runtime_root import airborne_bootstrap
|
|
from gps_denied_onboard.runtime_root.airborne_bootstrap import _c1_vio_wrapper
|
|
|
|
|
|
def _minimal_config() -> Config:
|
|
return Config.with_blocks(c1_vio={"strategy": "klt_ransac"})
|
|
|
|
|
|
def test_wrapper_forwards_altitude_provider_kwarg(
|
|
monkeypatch: pytest.MonkeyPatch,
|
|
) -> None:
|
|
# Arrange — capture the kwargs handed to build_vio_strategy.
|
|
captured: dict[str, Any] = {}
|
|
|
|
def _capture(_config: Config, **kwargs: Any) -> object:
|
|
captured.update(kwargs)
|
|
return object()
|
|
|
|
monkeypatch.setattr(airborne_bootstrap, "build_vio_strategy", _capture)
|
|
constructed: dict[str, Any] = {"c13_fdr": object()}
|
|
|
|
# Act
|
|
_c1_vio_wrapper(_minimal_config(), constructed)
|
|
|
|
# Assert
|
|
assert "altitude_provider" in captured
|
|
assert isinstance(captured["altitude_provider"], EskfNominalAltitudeProvider)
|
|
assert isinstance(captured["altitude_provider"], AltitudeProvider)
|
|
|
|
|
|
def test_provider_supplier_resolves_c5_estimator_lazily(
|
|
monkeypatch: pytest.MonkeyPatch,
|
|
) -> None:
|
|
# Arrange — capture the provider, then mutate the constructed dict
|
|
# after the wrapper returns to simulate C5 landing in the topo order.
|
|
captured: dict[str, Any] = {}
|
|
|
|
def _capture(_config: Config, **kwargs: Any) -> object:
|
|
captured.update(kwargs)
|
|
return object()
|
|
|
|
monkeypatch.setattr(airborne_bootstrap, "build_vio_strategy", _capture)
|
|
constructed: dict[str, Any] = {"c13_fdr": object()}
|
|
_c1_vio_wrapper(_minimal_config(), constructed)
|
|
provider: EskfNominalAltitudeProvider = captured["altitude_provider"]
|
|
|
|
# Pre-C5 the supplier returns None.
|
|
pre_c5 = provider._estimator_supplier()
|
|
|
|
# The bootstrap loop later assigns the c5_state slot.
|
|
sentinel_estimator = object()
|
|
constructed["c5_state"] = sentinel_estimator
|
|
post_c5 = provider._estimator_supplier()
|
|
|
|
# Assert
|
|
assert pre_c5 is None
|
|
assert post_c5 is sentinel_estimator
|
|
|
|
|
|
def test_wrapper_still_requires_c13_fdr(
|
|
monkeypatch: pytest.MonkeyPatch,
|
|
) -> None:
|
|
# Arrange — AZ-919 must not silently relax the existing AC-618-3
|
|
# invariant: an absent c13_fdr key still raises AirborneBootstrapError.
|
|
monkeypatch.setattr(
|
|
airborne_bootstrap,
|
|
"build_vio_strategy",
|
|
lambda *_a, **_kw: object(),
|
|
)
|
|
|
|
# Act + Assert
|
|
with pytest.raises(airborne_bootstrap.AirborneBootstrapError) as exc:
|
|
_c1_vio_wrapper(_minimal_config(), constructed={})
|
|
assert "c13_fdr" in str(exc.value)
|
|
assert "c1_vio" in str(exc.value)
|