fd848266d1
[AZ-383] C5 add_vio/add_pose_anchor/add_fc_imu factor adds
...
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 >
2026-05-11 06:07:45 +03:00
8b394a98c6
[AZ-382] C5 GtsamIsam2StateEstimator skeleton + real iSAM2 handle bodies
...
- Add GtsamIsam2StateEstimator owning the GTSAM substrate:
gtsam.ISAM2(ISAM2Params()) + gtsam_unstable.IncrementalFixedLagSmoother
(K * 1/3 s window per D-C5-3) + NonlinearFactorGraph + Values.
- Module-level create(...) factory + register() helper for
register_state_estimator("gtsam_isam2", create). Opt-in registration
per ADR-002 — no auto-import.
- Key-management policy: key_for_frame(UUID) -> int via
gtsam.symbol('x', counter); idempotent re-lookup.
- Replace all four NotImplementedError bodies in _isam2_handle.py with
real GTSAM calls:
* add_factor → estimator._graph.add(factor); R05 defensive logging
on success/failure; EstimatorDegradedError on failure.
* update → _isam2.update + _smoother.update; empty
FixedLagSmootherKeyTimestampMap substituted for timestamps=None;
EstimatorFatalError on either failure.
* compute_marginals → gtsam.Marginals(getFactorsUnsafe(),
calculateEstimate()).
* last_anchor_age_ms → (monotonic_ns - _last_anchor_ns) // 1e6.
- StateEstimator Protocol methods on the estimator still raise
NotImplementedError naming AZ-383 (factor adds) / AZ-384
(marginals + outputs).
- AZ-382 AC tests: 27 cases covering 10/10 ACs + factory integration.
- AZ-381 test_ac8_handle_methods_raise_named_task removed (obsolete:
bodies are real now); test_ac8_handle_is_isam2_graph_handle retained.
- Full suite: 547 passed (+26 vs B12), 2 skipped.
- Impl report: _docs/03_implementation/batch_13_cycle1_report.md.
Co-authored-by: Cursor <cursoragent@cursor.com >
2026-05-11 05:51:23 +03:00
beed43724f
[AZ-381] C5 StateEstimator protocol + factory + C8 DTO reshape
...
- Add StateEstimator Protocol (6 methods, @runtime_checkable) + DTOs
(EstimatorOutput, EstimatorHealth, IsamState, PoseSourceLabel, Quat)
in _types/state.py per state_estimator_protocol.md v1.0.0.
- Add C5 error hierarchy (StateEstimatorError + 3 subclasses) and
C5StateConfig (strategy, keyframe_window, spoof gates,
no_estimate_fallback_s) with __post_init__ validation.
- Add ISam2GraphHandle Protocol + ISam2GraphHandleImpl skeleton (all
4 methods raise NotImplementedError naming AZ-382 as owner).
- Add build_state_estimator factory + bind_state_ingest_thread for
single-writer enforcement; ADR-002 build-flag gating
(BUILD_STATE_<variant>); INFO log on success.
- Strict reshape of legacy EstimatorOutput / EstimatorHealth across
all 6 C8 production files (_outbound_provenance,
_covariance_projector, pymavlink_ardupilot_adapter,
msp2_inav_adapter, mavlink_gcs_adapter, interface) + 6 C8 test
files (UUID frame_id, LatLonAlt position_wgs84, Quat orientation,
PoseSourceLabel enum source_label). Remove ad-hoc DTOs from
_types/pose.py and from C4's public __init__ (EstimatorOutput is a
C5 concept, not a C4 one).
- 20 AZ-381 AC tests (10 ACs + 4 config range + NFR + conformance).
- Full suite: 521 passed, 2 skipped (+20 vs Batch 11).
- Contracts: state_estimator_protocol.md v1.0.0 -> active;
composition_root_protocol.md v1.2.0 -> v1.3.0 (additive state
block + factory + ingest-thread binding).
- Impl report: _docs/03_implementation/batch_12_cycle1_report.md.
Co-authored-by: Cursor <cursoragent@cursor.com >
2026-05-11 05:35:20 +03:00
b12db61444
[AZ-263] Bootstrap: repo skeleton + Docker + CI + Alembic + Tier-1 tests
...
Implements the AZ-263 / E-BOOT initial structure task:
- Python src/-layout package `gps_denied_onboard/` with per-component
interface stubs (14 components), type-only DTOs under `_types/`,
shared helpers under `helpers/` (R14 LightGlue ownership), structured
JSON logging, runtime composition root with env-var fail-fast gate,
healthcheck module shared by Docker and CI smoke.
- CMake top-level + `cmake/{build_options,dependencies,strategies}.cmake`
with the BUILD_* per-binary flags (ADR-002) and pinned external git
refs for OKVIS2 / VINS-Mono / GTSAM / FAISS / OpenCV >=4.12.0.
- Three Dockerfiles (companion-tier1, operator-tooling,
mock-suite-sat-service) + two compose files (dev + Tier-1 test).
- Four GitHub Actions workflows: ci.yml (lint/unit/integration/dual
binary build/SBOM diff/security), ci-tier2.yml (self-hosted Jetson
AC-bound NFTs), release.yml, cve-rescan.yml.
- Two CI gate scripts: `ci/sbom_diff.py` (deployment SBOM subset +
R02 exclusion), `ci/opencv_pin_gate.py` (>=4.12.0 enforcement,
D-CROSS-CVE-1).
- Alembic-driven Postgres 16 initial migration `0001_initial.py`
mirroring satellite-provider tiles + flights + sector_classifications
+ manifests + engine_cache_entries (data_model.md s 2).
- Tier-1 test scaffolding: 95 passing unit tests covering every AC,
per-component smoke tests, structured logging JSON output check,
env-var gate check, healthcheck import check. Two CI-gated tests
(cmake configure, actionlint) skip locally with explicit reasons.
- Batch report + code review report under `_docs/03_implementation/`.
Verdict: PASS_WITH_WARNINGS (two Low findings, both informational).
Co-authored-by: Cursor <cursoragent@cursor.com >
2026-05-11 01:00:28 +03:00
Oleksandr Bezdieniezhnykh