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>
Adds the C8 inbound producer side:
- TelemetryRing[T]: bounded drop-oldest ring; first-overflow INFO log
+ monotonic dropped_count.
- SubscriptionBus + SubscriptionHandle: synchronous fan-out, lock-
released-before-callback to avoid deadlock; subscriber crash caught
+ DEBUG-logged so one bad subscriber cannot kill the decode loop.
- PymavlinkInboundDecoder: pymavlink-based AP decoder for RAW_IMU,
SCALED_IMU2, ATTITUDE, GPS_RAW_INT, GPS2_RAW, HEARTBEAT, STATUSTEXT.
Out-of-order drop (Invariant 7) per-kind WARN. STATUSTEXT spoofing
sentinel promotes subsequent GPS to GpsStatus.SPOOFED within 5 s.
AC-5.1 warm-start hint cached on first 3D+ fix; embedded into
every FlightStateSignal.
- Msp2InavInboundDecoder: YAMSPy-based iNav polling decoder for IMU /
attitude / GPS / flight-state. signed=False always (RESTRICT-COMM-2);
GpsStatus.SPOOFED is unreachable on iNav.
Adds yamspy>=0.3.3 + pyserial>=3.5 to pyproject.toml.
Tests: 443 pass / 2 skip / 0 fail (+33 in batch 9).
Contract: no drift on fc_adapter_protocol.md v1.0.0; this batch
implements the inbound producer side without changing signatures.
Co-authored-by: Cursor <cursoragent@cursor.com>
AZ-294: MidFlightTileSnapshotSink writes orthorectified tile JPEGs
atomically to flight_root/<flight_id>/tiles/<tile_id>.jpg, emits a
kind="mid_flight_tile_snapshot" pointer record, and evicts the oldest
tile when the per-flight 64 MiB cap is exceeded. Adds optional
frame_id to the snapshot payload (fdr_record_schema bump).
AZ-295: RecordKindPolicy with two paired gates:
- enforce_or_raise (producer-side) raises RawFrameWriteForbiddenError
for raw_nav_frame / raw_ai_cam_frame at the call site, defending
AC-8.5 / RESTRICT-UAV-4.
- gate_for_writer (writer-side) tumbling-window rate-caps
failed_tile_thumbnail records at <= 0.1 Hz; over-cap drops are
coalesced into kind="overrun" records with the originating
producer slug.
AZ-296: take_off() composition-root sequence with strict ordering
(writer.__init__ -> start -> open_flight -> fc_adapter.__init__ ->
fc_adapter.open). On FdrOpenError, logs ERROR record, calls
writer.stop(), prints the documented FATAL line to stderr, and
sys.exit(EXIT_FDR_OPEN_FAILURE=2). composition_root_protocol bumped
to v1.1.0 with the new constants + takeoff-sequence section.
29 new tests; full suite 356 passed / 2 skipped / 0 failures.
No new dependencies (stdlib only).
Co-authored-by: Cursor <cursoragent@cursor.com>
AZ-291 — FileFdrWriter: single writer thread draining every registered
FdrClient SPSC ring buffer to per-flight segment files; per-segment
size rotation; cross-process fcntl.flock filelock on flight_root;
ENOSPC degraded mode with rate-capped ERROR logs and one GCS alert.
AZ-292 — FlightHeader/FlightFooter dataclasses + open_flight /
close_flight lifecycle methods; four per-flight monotonic counters
(records_written, records_dropped_overrun, bytes_written,
rollover_count) reported by the footer; flight_id mismatch and
close-without-open are typed errors.
AZ-293 — CapacityCapPolicy (post-rotation hook): walks the flight
directory, drops the oldest CLOSED segment when total > cap (default
64 GiB), emits a kind="segment_rollover" record per drop. Never drops
the currently-open segment or segment 0 alone; cap_misconfigured path
logs ERROR + GCS alert. No config flag disables emission (C13-ST-01).
Schema: bumped fdr_record_schema flight_header / flight_footer payload
key sets to match the AZ-292 task spec (effective 1.0.0 -> 1.1.0; no
prior producer); KNOWN_PAYLOAD_KEYS updated. Added FdrWriterConfig
nested in FdrConfig (segment_size_bytes, batch_size, flight_cap_bytes,
debug_log_per_record).
Tests: 29 new unit tests (8 AC + 1 invariant per task); full suite
323 passed, 2 pre-existing skips, 0 regressions.
Co-authored-by: Cursor <cursoragent@cursor.com>
E-CC-HELPERS closes with the three remaining Layer-1 helpers and
E-CC-CONF closes with the env > YAML > defaults precedence test
gate. All four tickets ship with frozen public surfaces, hermetic
unit tests, and no upward (components.*) imports.
* AZ-271 — tests/unit/shared/config/test_precedence.py (5 ACs + smoke
test + helper that names the layer in failure messages).
* AZ-282 — helpers/ransac_filter.py: static RansacFilter +
RansacResult; cv2.setRNGSeed(0) for byte-equal determinism;
median residual semantics pinned by contract.
* AZ-276 — helpers/imu_preintegrator.py + make_imu_preintegrator;
GTSAM PreintegratedCombinedMeasurements; strict-monotonic ts_ns
guard runs before any state mutation. Adjacent hygiene:
_types/nav.py ImuSample/ImuWindow now use ts_ns:int and the
spec-mandated ImuBias dataclass.
* AZ-278 — helpers/lightglue_runtime.py: structural R14 fix.
LightGlueRuntime + non-blocking concurrent-access guard that
raises rather than serialising. EngineHandle Protocol in
_types/manifests.py + KeypointSet/CorrespondenceSet in
_types/matching.py (Protocol surface adds approved by spec).
Dependency conflict (Finding 1, user-approved): gtsam 4.2 (PyPI) is
numpy-1.x-ABI only; opencv-python>=4.12 needs numpy>=2 at runtime.
Resolution: opencv-python pin relaxed to >=4.11.0.86,<4.12. The
D-CROSS-CVE-1 ratchet at ci/opencv_pin_gate.py is held at 4.11.0
with the original 4.12.0 floor restored once a numpy-2-compatible
gtsam wheel ships. Full replay procedure in
_docs/_process_leftovers/2026-05-11_d_cross_cve_1_opencv_pin_deferred.md.
Tests: 294 passed, 2 skipped (cmake/actionlint env-skips,
pre-existing). 43 new tests added for batch 5. Ruff check + format
clean.
Co-authored-by: Cursor <cursoragent@cursor.com>
AZ-273: lock-free SPSC ring buffer with pre-allocated slots, power-of-
two capacity, opt-in SPSC guard, and EnqueueResult / FdrSpscViolationError
on the public surface. make_fdr_client caches one client per producer_id
and reads capacity from config.fdr.per_producer_capacity with fallback
to queue_size.
AZ-274: default_overrun_policy implements drop-oldest + retry + immediate
marker emission, with prior-marker dropped_count folding via _evict_one
so user-loss info is never lost across iterations. ERROR diagnostic is
rate-limited to <=1/sec per producer.
AZ-275: FakeFdrSink mirrors the FdrClient public surface and reuses the
production default_overrun_policy via a duck-typed _PolicyAdapter. The
test-only records/all_records_ever properties let component tests assert
both in-buffer and lifetime state. tests/conftest.py registers the
fake_fdr_sink fixture and an AST architecture lint forbids production
imports of fakes.
AZ-267: FdrLogBridgeHandler installs on the root logger via wire_log_bridge
and forwards only WARN+ERROR records into the FDR with kind="log".
Thread-local recursion guard short-circuits internal logging; saturated-
queue diagnostics go to stderr every N=1000 drops.
AZ-268: tests/contract/log_schema.py covers every row of the schema's
Test Cases table plus the "DEBUG+INFO never reach FDR" invariant.
pyproject.toml registers the contract pytest marker and the
contract-mandated log_schema.py file-name.
251 unit + contract tests pass (48 new). Review verdict:
PASS_WITH_WARNINGS; findings are NFR-perf deferrals + documented
relaxation of AZ-274 AC-2 coalescing under permanently-stalled consumer.
Co-authored-by: Cursor <cursoragent@cursor.com>
AZ-270: composition root with strategy registry, tier-gated lookup,
topo-order construction, all-or-nothing teardown, StrategyNotLinkedError
payload.
AZ-272: orjson-backed FdrRecord serialise/parse with forward-compat for
unknown payload + top-level fields and canonical overrun-record shape.
AZ-279: pyproj-backed WGS84/ECEF/ENU + OSM slippy-map tile math with
WgsConversionError for shape/range/zoom guards.
AZ-281: strict EngineFilenameSchema build/parse/matches_host with
anchored regex + enum validation; round-trip identity by construction.
AZ-283: dtype-preserving (fp16/fp32) single + batch L2 normaliser with
zero-norm safety and descriptor_metric() source-of-truth.
pyproject.toml pins pyproj>=3.6 and orjson>=3.9 (named-backend deps per
the AZ-272 / AZ-279 contracts). New DTOs LatLonAlt + BoundingBox and
EngineCacheKey + HostCapabilities land in _types/ to back the helper
contracts.
203 unit tests pass (64 new). Review verdict: PASS_WITH_WARNINGS;
findings are perf-NFR deferrals + dep amendment + minor docstring polish.
Co-authored-by: Cursor <cursoragent@cursor.com>
Closes out greenfield Step 6 (Decompose) for all 14 components
(C1-C13 + cross-cutting helpers/replay). Covers tasks AZ-266..AZ-446
plus the _dependencies_table.md and component contract documents.
State file updated to greenfield Step 7 (Implement), not_started.
Co-authored-by: Cursor <cursoragent@cursor.com>
- Changed the autodev state to reflect the new phase and task name for remediation related to AZ-243.
- Updated the dependencies table to include the new task AZ-243 and adjusted dependencies for AZ-233.
- Added a section in the implementation completeness report to document the creation of the AZ-243 remediation task aimed at integrating the production native VIO runtime.
Confirm the existing blackbox test task set is ready after product
remediation and advance autodev to test implementation.
Co-authored-by: Cursor <cursoragent@cursor.com>
- Refined task decomposition steps to ensure implementation tasks are atomic and complexity does not exceed 5 points.
- Enhanced the product implementation process with a completeness gate to verify task outcomes against architecture promises before proceeding to testing.
- Updated dependencies table to reflect new tasks and their relationships, ensuring all test tasks are linked to product remediation tasks.
- Adjusted workflow documentation to clarify entry points for task decomposition and implementation contexts.
Co-authored-by: Cursor <cursoragent@cursor.com>
Keep generated tiles auditable and untrusted onboard while preserving
covariance, quality, and sidecar metadata for post-flight sync.
Co-authored-by: Cursor <cursoragent@cursor.com>
Implement the first runtime component boundaries around the shared
contracts so downstream batches can consume typed frame, MAVLink, tile,
and FDR behavior with focused tests and batch evidence.
Co-authored-by: Cursor <cursoragent@cursor.com>
Provide deterministic geometry/time-sync helpers and structured config, error, health, and telemetry primitives for downstream runtime components.
Co-authored-by: Cursor <cursoragent@cursor.com>
Implement the shared DTO contract surface with validation so runtime components consume one public model set instead of duplicating shapes.
Co-authored-by: Cursor <cursoragent@cursor.com>
Keep VIO package and native bridge paths backend-neutral so BASALT remains an implementation choice rather than a component boundary.
Co-authored-by: Cursor <cursoragent@cursor.com>
Add the initial source, test, infrastructure, CI, configuration, and evidence-path scaffold so dependent implementation tasks have stable package and runtime boundaries.
Co-authored-by: Cursor <cursoragent@cursor.com>
Oleksandr Bezdieniezhnykh