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>
Adds an opt-in C5-internal orthorectifier (`_orthorectifier.py`) that
emits at most one tile-aligned JPEG candidate per nav frame to the
C6 `TileStore.write_tile` API. Quality gates fire before any
OpenCV work: covariance Frobenius, inlier floor, source-label
(`SATELLITE_ANCHORED` only), and once-per-frame rate limit.
Cross-component import rule (AZ-507) is preserved: c5_state never
imports c6_tile_cache. `runtime_root.state_factory` carries a new
`_C6MidFlightIngestAdapter` that builds the canonical
`TileMetadata` (`ONBOARD_INGEST` / `FRESH` / `PENDING`), hashes
the JPEG, and translates `FreshnessRejectionError` to a `None`
return so the orthorectifier silently swallows freshness
rejection per AC-NEW-3.
Wiring is opt-in via `C5StateConfig.orthorectifier.enabled`;
existing tests/binaries default to disabled and are unaffected.
Both `GtsamIsam2StateEstimator` and `EskfStateEstimator`
participate through new `attach_orthorectifier` /
`set_latest_nav_frame` extension methods (Protocol surface
unchanged).
Tests: 22 new unit tests cover AC-1..AC-9 plus inlier-floor
gate plus the composition-root adapter. 216/216 c5_state and
38/38 runtime-root + compose tests pass.
Co-authored-by: Cursor <cursoragent@cursor.com>
Freezes the c1_vio Public API per
_docs/02_document/contracts/c1_vio/vio_strategy_protocol.md v1.0.0:
- VioStrategy Protocol (4 methods: process_frame, reset_to_warm_start,
health_snapshot, current_strategy_label) in
components/c1_vio/interface.py.
- DTOs (VioOutput, VioHealth, FeatureQuality, WarmStartPose) + VioState
enum in _types/nav.py — L1 placement so C5 + C13 consume them without
crossing the components.* boundary (AZ-270 AC-6). The new VioOutput
shape (frame_id: str, relative_pose_T: gtsam.Pose3,
pose_covariance_6x6, imu_bias, feature_quality, emitted_at_ns)
replaces the AZ-263 scaffolding in _types/vio.py, which is now
deleted.
- VioError family (VioInitializingError / VioDegradedError /
VioFatalError) in components/c1_vio/errors.py. Documented
rationale: the degraded-operation path returns a VioOutput with
inflated covariance + VioHealth.state=DEGRADED rather than raising
VioDegradedError — the error type exists only for the rare
degraded->fatal transition.
- C1VioConfig per-component config block (strategy enum,
lost_frame_threshold default 9, warm_start_max_frames default 5)
with constructor-time validation rejecting unknown strategy labels.
- StrategyNotAvailableError added to runtime_root/errors.py;
composition-time error distinct from the VioError family.
- Composition-root factory build_vio_strategy in
runtime_root/vio_factory.py with three BUILD_* gates (BUILD_OKVIS2,
BUILD_VINS_MONO, BUILD_KLT_RANSAC). Concrete strategy modules are
imported lazily via __import__ AFTER the flag check — Tier-0
workstation builds with the flag OFF MUST NOT load the strategy
module (Risk-2 / I-5; verifiable via sys.modules).
- 36 conformance tests cover all 9 ACs + NFR-perf-factory
(p99 build under 200 ms x 1000 calls) + NFR-reliability-error-family.
AC-8 introspects the contract file's Shape table and asserts method
parity against the runtime Protocol; AC-9 asserts the frame_id
annotation is 'str' (PEP-563 stringified).
C5 migration (consumers of the new VioOutput shape):
- gtsam_isam2_estimator.py + eskf_baseline.py: replaced
vio.timestamp -> vio.emitted_at_ns (drops _datetime_to_ns on the
VIO path), vio.pose_se3 -> vio.relative_pose_T (gtsam.Pose3 direct;
drops _pose_se3_to_gtsam / _pose_se3_to_array), vio.covariance_6x6
-> vio.pose_covariance_6x6 (rename).
- key_for_frame signature widened to UUID | int | str to accept the
new str frame_id.
- 4 C5 test files migrated to the new VioOutput shape with helper
fixtures producing ImuBias + FeatureQuality + str frame_id.
- c5_state/interface.py TYPE_CHECKING import path updated.
Bootstrap healthcheck + test_types_importable updated to drop the
deleted _types/vio module and pick up _types/inference (AZ-297) in
the same sweep.
Full unit-test sweep: 884 passed, 2 pre-existing environment skips
(cmake, actionlint).
Co-authored-by: Cursor <cursoragent@cursor.com>
F1 (High/Architecture) from cumulative review of batches 01-22:
`ISam2GraphHandleImpl` did not satisfy C4's `ISam2GraphHandle`
Protocol stub (AZ-355) because it lacked `get_pose_key`.
`pose_factory`'s isinstance gate would have raised at composition.
Two Protocols (C4 minimal consumer cut, C5 richer producer surface)
are intentional per AZ-355 Risk 1 — the impl just needed to expose
the canonical name. Delegates to estimator.key_for_frame.
Added cross-component conformance test asserting the C5 impl
satisfies both Protocols, so future drift trips a unit test.
F2 (Medium/Maintainability): added justifying comments at four
`except: pass` sites in runtime_root, c8_fc_adapter (ap + inav),
and c13_fdr writer. No behavioral change.
Updated cumulative review report verdict from FAIL to PASS and
recorded a post-mortem on the initial misframing
(treated the dual-Protocol design as duplication on first read).
Autodev state: batch 22 done, cumulative-review PASS,
ready for batch 23.
Co-authored-by: Cursor <cursoragent@cursor.com>
Add operator warm-start path to C5 StateEstimator Protocol and both
implementations (GtsamIsam2StateEstimator, EskfStateEstimator), plus
the third clause of the AZ-385 spoof-promotion gate.
- StateEstimator Protocol: set_takeoff_origin(origin, sigma_horiz_m,
sigma_vert_m) -> None.
- iSAM2: PriorFactorPose3 at origin with diagonal sigmas, single
isam2.update().
- ESKF: zero _nominal_pos, overwrite _P position block with sigma**2.
- SourceLabelStateMachine.process_gps_sample bounded-delta clause:
WgsConverter.horizontal_distance_m vs smoother estimate; reject
resets the dwell-time counter so AZ-385 cannot re-promote off bad
GPS.
- New EstimatorAlreadyStartedError (StateEstimatorConfigError
subclass) on late call after first add_*.
- C5StateConfig: spoof_promotion_bounded_delta_m=200,
default_takeoff_origin_sigma_horiz_m=5,
default_takeoff_origin_sigma_vert_m=10.
- New GpsSample DTO + WgsConverter.horizontal_distance_m helper.
- 4 new FDR kinds (cold_start_origin.{set,unavailable},
gps_bounded_delta.{accept,reject}) registered in AZ-272 schema.
- 33 new unit tests cover AC-1..AC-15; full repo 750 passed / 2
skipped (pre-existing CI tooling skips).
Docs synced: protocol contract, C5 component description,
architecture, glossary, system-flows, C10 provisioning description.
Co-authored-by: Cursor <cursoragent@cursor.com>
Implements the mandatory simple-baseline StateEstimator per AC-2.1a
engine-rule at C5 (IT-12 comparative study vs iSAM2). NumPy-only;
no GTSAM dependency so BUILD_STATE_ESKF=ON binaries ship without
GTSAM at all.
- 16-state error vector (pos 3 + vel 3 + rot 3 + ba 3 + bg 3 + dt 1)
over a textbook nominal-state / error-state ESKF split.
- add_fc_imu: full nonlinear IMU integration + linearised F P F^T + Q
covariance propagation per IMU sample.
- add_vio: simplified relative-pose update (snapshot-based; baseline
scope, documented).
- add_pose_anchor: absolute-pose update; integrates BOTH marginals and
jacobian modes (no skip — ESKF has no graph; AC-4).
- AC-9 divergence test: Mahalanobis r^T S^-1 r > 100 (10 sigma) on the
innovation covariance S = H P H^T + R.
- AC-5 SPD: Cholesky-positive enforcement on every emitted covariance;
non-SPD raises EstimatorFatalError and locks state to LOST.
- AC-6 honesty: smoothed_history entries carry smoothed=False; deviation
from C5 contract Invariant 7 documented in module + report.
- AC-7 / AC-10 BUILD_STATE_ESKF gating: works through existing factory
infra (state_factory._STATE_BUILD_FLAGS).
- AC-8: SourceLabelStateMachine + FallbackWatcher auto-wired eagerly
in __init__, same pattern as the iSAM2 estimator.
Tests: 20 new unit tests covering AC-1..AC-10 + robustness checks.
Full suite: 660 passed, 2 skipped (CI-only).
The AZ-386 Jira transition to Done is deferred (Atlassian MCP returned
'Not connected'); recorded in _docs/_process_leftovers/ for replay on
the next autodev invocation per the Leftovers Mechanism.
Co-authored-by: Cursor <cursoragent@cursor.com>
After every successful current_estimate(), emit one
c5.state.smoothed_history FDR record per newly-smoothed past
keyframe from IncrementalFixedLagSmoother. AC-4.5 (revised): the
smoothed stream goes ONLY to FDR; the C8 outbound forward-time
stream is unaffected.
Idempotency via _smoothed_fdr_watermark_s (smoother-native float
seconds); the same pose key is never emitted twice. Hook is
best-effort — internal failures log warnings but do not raise, so
a smoother divergence cannot contaminate the forward-time path.
Cross-task invariants documented:
- AC-3 ESKF no-op — AZ-386 installs an inert hook on the ESKF.
- AC-4 No C8 leak — enforced at the C8 boundary by AZ-261.
8 new unit tests against AC-1/2/5/6 + robustness (no-FDR-client,
marginals failure). Full suite: 640 passed, 2 skipped.
Co-authored-by: Cursor <cursoragent@cursor.com>
Implements Invariants 5 + 8 + AC-NEW-2 / AC-NEW-8: the
EstimatorOutput.source_label now reflects a real state machine
(DEAD_RECKONED → SATELLITE_ANCHORED ↔ VISUAL_PROPAGATED) governed by
a spoof-promotion gate that latches closed on FC SPOOFED GPS health
and re-opens only when BOTH conditions hold — ≥10 s
STABLE_NON_SPOOFED AND next anchor within
spoof_promotion_visual_consistency_tol_m.
Every reject emits a c5.state.spoof_rejected FDR record plus a
subscriber-fan-out STATUSTEXT (severity WARNING, 50-char cap per
MAVLink). FDR and subscriber paths bypass the standard logger so
silencing logs cannot suppress the spoof trail (R07 / AC-6).
GtsamIsam2StateEstimator now eagerly builds the SM from C5StateConfig
in __init__; new public methods notify_gps_health() (delegates to
SM, called by composition root from C8 inbound) and
subscribe_spoof_rejection() (composition root attaches C8's
QgcTelemetryAdapter here). health_snapshot.spoof_promotion_blocked
+ current_estimate.source_label now flow from the live SM.
25 new unit tests across all 12 ACs plus cancellation, subscriber
exception isolation, and estimator wire-up integration cases. One
AZ-384 test renamed + updated to expect DEAD_RECKONED before any
anchor (was VISUAL_PROPAGATED placeholder pre-AZ-385).
Full suite: 632 passed, 2 skipped.
Co-authored-by: Cursor <cursoragent@cursor.com>
Implements Invariant 9 / AC-5.2: when current_estimate cannot return a
fresh output for >= state.no_estimate_fallback_s (default 3.0 s), emit
ONE engagement signal (FDR kind=c5.state.no_estimate_fallback_engaged
+ GCS STATUSTEXT severity CRITICAL); on recovery, ONE recovery signal
(FDR kind=c5.state.no_estimate_fallback_recovered + STATUSTEXT NOTICE).
Rate-limited via single _in_fallback latch (AC-2: 30 s sustained
no-estimate still emits exactly one engagement).
New FallbackWatcher class owns the state machine; estimator wires it
through constructor + current_estimate entry/success hooks. Public
check_fallback_state(now_ns) watchdog (NFR p99 <= 5 us) + subscribe
APIs let C8 outbound react without coupling C5 to a concrete GCS
adapter at construction. Severity enum extended with CRITICAL=2 and
NOTICE=5 to match MAVLink MAV_SEVERITY.
18 new unit tests across all 8 ACs, deterministic synthetic clock,
integration tests patch monotonic_ns through GtsamIsam2StateEstimator
to drive AC-7 iSAM2 leg (ESKF leg deferred to AZ-386).
Full suite: 607 passed, 2 skipped.
Co-authored-by: Cursor <cursoragent@cursor.com>
Replaces the last three NotImplementedError placeholders on
GtsamIsam2StateEstimator with real Marginals + output methods:
- current_estimate(): recovers the 6x6 Marginals covariance for the
most-recently committed pose key, enforces the SPD invariant via
np.linalg.cholesky (Invariant 10), converts the local-ENU pose
translation to WGS84 via the shared WgsConverter, derives a
body->world quaternion, and emits a fresh EstimatorOutput
(smoothed=False, Invariant 4). On SPD failure transitions
isam2_state -> LOST and raises EstimatorFatalError (AC-5.2 path).
- smoothed_history(n): iterates the smoother's active POSE keys via
_smoother.calculateEstimate().keys() (filtered by GTSAM symbol
char) and the smoother timestamps via ts_map.at(key) - workaround
for the pinned gtsam_unstable build's non-iterable
FixedLagSmootherKeyTimestampMap. Bounded by K (Invariant 6); every
entry has smoothed=True (Invariant 7).
- health_snapshot(): cheap O(1) accumulator read; reports
IsamState lifecycle, pose-key count, AC-NEW-8
cov_norm_growing_for_s rolling 60s deque-backed counter, and
spoof_promotion_blocked via the AZ-385 state machine injection
point.
Adds two public injection points for AZ-385/composition root:
set_enu_origin(LatLonAlt) and attach_source_label_state_machine(machine).
Defaults: (0, 0, 0) ENU origin, VISUAL_PROPAGATED source label,
spoof_promotion_blocked=False.
Wires _record_committed_pose_key into the three add_* success paths
so current_estimate only reads keys that have real values in iSAM2.
The JACOBIAN path in add_pose_anchor deliberately skips this call -
Invariant 3 keeps the JACOBIAN pose out of the iSAM2 graph.
Tests: +27 in tests/unit/c5_state/test_az384_marginals_outputs.py
covering all 10 ACs. Three obsolete AZ-382 tests
(test_ac10_*_raises_named_az384) removed. Full suite: 589 passed,
2 skipped.
Co-authored-by: Cursor <cursoragent@cursor.com>
Replaces AZ-382 NotImplementedError placeholders with real GTSAM factor
adds wired against the iSAM2 graph handle:
- add_vio -> BetweenFactorPose3 between consecutive VIO pose keys
(first call primes the chain; AZ-388 owns first-keyframe seeding).
- add_pose_anchor -> mode-dispatch per pose.covariance_mode:
"marginals" -> PriorFactorPose3 + handle.update();
"jacobian" -> skip iSAM2 add per AZ-361 contract.
Both paths bump _last_anchor_ns via time.monotonic_ns().
- add_fc_imu -> shared ImuPreintegrator.integrate_window +
reset_for_new_keyframe; builds a CombinedImuFactor between the
prev/curr (X, V, B) keyframe triple. Introduces new 'v' (velocity)
and 'b' (bias) GTSAM key namespaces decoupled from the VIO/pose
frame_id mapping.
Invariant 2 - non-decreasing timestamps - enforced per call with
EstimatorDegradedError + c5.state.out_of_order log. Every successful
add emits a structured DEBUG *_ok log; every failure emits a
structured ERROR *_failed log and raises through the C5 error
hierarchy (R05).
Contract-vs-reality fix-ups also landed:
- StateEstimator Protocol: add_fc_imu(ImuWindow) - was incorrectly
annotated as ImuTelemetrySample by AZ-381.
- _last_anchor_ns semantics switched to monotonic_ns() to match
last_anchor_age_ms.
- create() factory back-wires the ISam2GraphHandle to the estimator
via the new attach_handle() method.
Tests: +21 in tests/unit/c5_state/test_az383_factor_adds.py covering
all 8 ACs with mock ISam2GraphHandle instances. Three obsolete
AZ-382 tests (test_ac10_add_*_raises_named_az383) removed. Full
suite: 565 passed, 2 skipped.
Co-authored-by: Cursor <cursoragent@cursor.com>
Oleksandr Bezdieniezhnykh