mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 12:51:12 +00:00
[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>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,96 @@
|
||||
# Batch 14 — Cycle 1 Implementation Report
|
||||
|
||||
**Batch**: 14 of N
|
||||
**Tasks landed**: AZ-383 (`GtsamIsam2StateEstimator` — VIO + Pose + IMU factor adds)
|
||||
**Cycle**: 1
|
||||
**Date**: 2026-05-11
|
||||
|
||||
## Scope
|
||||
|
||||
| Task | Component | Purpose |
|
||||
|------|-----------|---------|
|
||||
| AZ-383 | C5 state estimator | Replaces the three AZ-382 `NotImplementedError` placeholders on `GtsamIsam2StateEstimator` (`add_vio`, `add_pose_anchor`, `add_fc_imu`) with real factor-add bodies wired against the AZ-382 `ISam2GraphHandle`. `add_vio` emits a `BetweenFactorPose3` between consecutive VIO pose keys (first call primes the chain only). `add_pose_anchor` mode-dispatches per `pose.covariance_mode`: `"marginals"` → `PriorFactorPose3` + `handle.update()`; `"jacobian"` → SKIP the iSAM2 add per the AZ-361 cross-task contract (the running estimate still consumes the pose downstream, the graph stops growing under throttle). Both paths bump `_last_anchor_ns` via `time.monotonic_ns()` so `last_anchor_age_ms` and the spoof-promotion gate (AZ-385) see a recent satellite anchor. `add_fc_imu` feeds the shared `ImuPreintegrator`, closes the PIM with `reset_for_new_keyframe()`, and builds a `CombinedImuFactor` between the previous and current `(X, V, B)` keyframe triple — first call primes the chain without emitting a factor. Every successful add emits a DEBUG `*_ok` log; every failure emits an ERROR `*_failed` log and raises through the C5 error hierarchy (R05). Invariant 2 — non-decreasing timestamps — is enforced per call with an `EstimatorDegradedError` + `c5.state.out_of_order` log on violation. The remaining three Protocol output methods (`current_estimate` / `smoothed_history` / `health_snapshot`) still raise `NotImplementedError` naming AZ-384. |
|
||||
|
||||
## Decision record
|
||||
|
||||
The task started as a "~5pt drop-in", but inspecting the contracts surfaced four contract-vs-reality conflicts that the user chose to resolve in full alignment with the C5 contract rather than paper over (re-estimate to ~7–8pt, one batch):
|
||||
|
||||
1. **Protocol `add_fc_imu` type mismatch** — AZ-381's Protocol annotated `add_fc_imu(ImuTelemetrySample)` (a single sample), but the C5 contract has always specified `ImuWindow` because the shared `ImuPreintegrator` operates on batches. The composition root will batch the C8 `ImuTelemetrySample` stream into `ImuWindow`s before calling `add_fc_imu`. Fixed `interface.py` here with a docstring note.
|
||||
2. **`ImuPreintegrator` API mismatch** — task spec assumed `preintegrate(window) -> CombinedImuFactor`. Real API per AZ-276 is `integrate_window(window)` + `reset_for_new_keyframe() -> PreintegratedCombinedMeasurements`. C5 builds the `CombinedImuFactor` itself from the closed PIM + the (X, V, B) key triple — that's not the preintegrator's job.
|
||||
3. **Anchor freshness semantics** — task spec said `_last_anchor_ns = pose.emitted_at`, but `PoseEstimate` has no `emitted_at` field and the contract describes the value as a **monotonic age** (consumed by `last_anchor_age_ms` which uses `time.monotonic_ns()`). Switched to `_last_anchor_ns = time.monotonic_ns()`.
|
||||
4. **`CombinedImuFactor` key management** — the factor needs a `(prev_X, prev_V, prev_B, curr_X, curr_V, curr_B)` 6-tuple of keys. Introduced two new GTSAM key namespaces (`'v'` for velocity, `'b'` for bias) with an independent `_imu_keyframe_counter`. The `'x'` (pose) namespace is shared with VIO / pose-anchor via `_next_key_counter`, but the IMU chain steps it independently per IMU keyframe — the composition root will arrange the higher-level scheduling that fuses the chains.
|
||||
|
||||
## Files added / modified
|
||||
|
||||
### Modified (prod)
|
||||
|
||||
- `src/gps_denied_onboard/components/c5_state/interface.py` — corrected `add_fc_imu` Protocol annotation from `ImuTelemetrySample` to `ImuWindow`; added a docstring note explaining the AZ-381 → AZ-383 fix-up.
|
||||
- `src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py` — replaced the three `NotImplementedError` placeholders with real factor-add bodies; added `_DEFAULT_POSE_SIGMA = 0.1` module constant; added new state fields: `_log`, `_isam2_handle` (back-reference to the handle), `_last_added_ts_ns` (Invariant 2 monotonic guard), `_prev_vio` (between-factor wiring), `_imu_keyframe_counter` + `_prev_imu_x_key/_prev_imu_v_key/_prev_imu_b_key` (IMU chain); added `attach_handle(handle)` method; extended `key_for_frame` signature to `UUID | int`; added internal helpers `_require_handle`, `_guard_timestamp`, `_reset_staging`; added module-level pure helpers `_datetime_to_ns`, `_pose_se3_to_gtsam`, `_build_pose_noise`, `_make_timestamp_map`; updated `create(...)` to back-wire the handle to the estimator via `attach_handle(...)`.
|
||||
|
||||
### Added (tests)
|
||||
|
||||
- `tests/unit/c5_state/test_az383_factor_adds.py` — 21 new tests covering all 8 ACs using mock `ISam2GraphHandle` instances (real iSAM2 would require seeded initial values which AZ-388 owns — see "Test strategy" below).
|
||||
|
||||
### Modified (tests)
|
||||
|
||||
- `tests/unit/c5_state/test_az382_isam2_smoother_wiring.py` — removed the three now-obsolete `test_ac10_add_*_raises_named_az383` tests; their behaviour is now governed by AZ-383's AC tests. Replaced with a comment block pointing readers to `test_az383_factor_adds.py`. The three remaining `test_ac10_*_raises_named_az384` tests (the marginals/outputs methods) were retained.
|
||||
|
||||
## Test strategy — why mock handles
|
||||
|
||||
A smoke test of `add_vio` against the real `ISam2GraphHandleImpl` failed with `iSAM2.update failed: Attempting to … the key "x0", which does not exist in the Values`. The reason: a between-factor between `x0` and `x1` requires both keys to already be in `gtsam.Values`. The first `add_vio` call would need to ALSO seed `x0` with a prior or an initial value, which is the responsibility of AZ-388 (AC-5.2 fallback). Rather than smuggle that responsibility into AZ-383, the AC tests verify only what AZ-383 owns:
|
||||
|
||||
- The correct GTSAM factor TYPE is constructed (`BetweenFactorPose3` / `PriorFactorPose3` / `CombinedImuFactor`).
|
||||
- The correct keys are passed.
|
||||
- `handle.add_factor` and `handle.update` are called the expected number of times.
|
||||
- The mode-dispatch in `add_pose_anchor` correctly SKIPS the add on the JACOBIAN path.
|
||||
- Invariant 2 enforcement raises `EstimatorDegradedError` + emits the structured `c5.state.out_of_order` log.
|
||||
- Every success path emits a `*_ok` DEBUG log; every failure path emits a `*_failed` ERROR log and raises through the C5 error hierarchy.
|
||||
- `_last_anchor_ns` is updated on BOTH pose-anchor paths.
|
||||
|
||||
The full end-to-end iSAM2 convergence will be tested in AZ-388 once the chain is seedable.
|
||||
|
||||
## Architectural notes
|
||||
|
||||
- **`_FRAME_PERIOD_S` constant** — inherited from AZ-382 (D-C5-3 fixes the keyframe rate at 3 Hz). AZ-383 did not touch the smoother window logic.
|
||||
- **(X, V, B) chain is independent of the frame-id mapping** — IMU keyframes are NOT VIO keyframes; the contract documents this. The composition root will fuse the chains; AZ-383's per-window factor add is self-contained.
|
||||
- **Defensive logging is mandatory (R05)** — every successful add: `c5.state.add_vio_ok` / `c5.state.add_pose_anchor_ok` / `c5.state.add_fc_imu_ok`. Every failure: `c5.state.add_vio_failed` / `c5.state.add_pose_anchor_failed` / `c5.state.add_fc_imu_failed` / `c5.state.add_fc_imu_preintegrate_failed`. JACOBIAN-path skip emits INFO `c5.state.skip_isam2_jacobian_path`. Out-of-order: ERROR `c5.state.out_of_order`. The C5 contract documents this surface because silent factor-add failures bit the prototype.
|
||||
- **First-call priming** — both `add_vio` and `add_fc_imu` SHORT-CIRCUIT the first call (`_prev_vio is None` / `_prev_imu_x_key is None`) because there is no previous frame to relate to. This is documented in both method docstrings.
|
||||
- **`_reset_staging()` after every successful flush** — the staging `_graph` / `_values` are cleared post-flush so the next `add_*` builds a fresh delta. This matches the iSAM2 incremental-update idiom (the handle's `update(graph, values, …)` accepts the DELTA, not the cumulative graph).
|
||||
|
||||
## Test counts
|
||||
|
||||
| Suite | Before (B13) | After (B14) | Delta |
|
||||
|-------|--------------|-------------|-------|
|
||||
| Total passing | 547 | 565 | +18 |
|
||||
| Skipped | 2 | 2 | 0 |
|
||||
| AZ-383 (new) | 0 | 21 | +21 |
|
||||
| AZ-382 (preserved) | 27 | 24 | −3 (obsolete `test_ac10_add_*_raises_named_az383` tests removed; behaviour now lives in AZ-383 tests) |
|
||||
|
||||
Run command: `PYTHONPATH=src pytest tests/ -q` → `565 passed, 2 skipped in ~20s`.
|
||||
|
||||
## Lint / type
|
||||
|
||||
- `ruff check src/gps_denied_onboard/components/c5_state/ tests/unit/c5_state/` — clean (1 `RUF002` ambiguous-Unicode in a docstring auto-fixed during the cycle).
|
||||
- `ruff format src/gps_denied_onboard/components/c5_state/ tests/unit/c5_state/` — clean (1 file reformatted).
|
||||
- `ReadLints` on `src/gps_denied_onboard/components/c5_state/` + `tests/unit/c5_state/` — 0 errors.
|
||||
|
||||
## Acceptance evidence
|
||||
|
||||
| AC | Test(s) | Status |
|
||||
|----|---------|--------|
|
||||
| AC-1 VIO factor add | `test_ac1_vio_first_call_records_without_factor`, `test_ac1_vio_second_call_adds_between_factor` | PASS |
|
||||
| AC-2 Pose-anchor MARGINALS path | `test_ac2_pose_anchor_marginals_adds_prior_factor_and_updates`, `test_ac2_pose_anchor_marginals_bumps_last_anchor_ns` | PASS |
|
||||
| AC-3 Pose-anchor JACOBIAN skip | `test_ac3_pose_anchor_jacobian_skips_factor_add`, `test_ac3_pose_anchor_jacobian_still_bumps_last_anchor_ns` | PASS |
|
||||
| AC-4 IMU factor add via ImuPreintegrator | `test_ac4_imu_first_window_primes_chain_no_factor`, `test_ac4_imu_second_window_adds_combined_imu_factor` | PASS |
|
||||
| AC-5 Timestamp ordering | `test_ac5_vio_out_of_order_raises_degraded`, `test_ac5_pose_anchor_out_of_order_raises_degraded`, `test_ac5_imu_out_of_order_raises_degraded`, `test_ac5_emits_out_of_order_log` | PASS |
|
||||
| AC-6 Defensive logging | `test_ac6_vio_success_emits_debug_log`, `test_ac6_pose_anchor_success_emits_debug_log`, `test_ac6_vio_failure_emits_error_log_and_raises` | PASS |
|
||||
| AC-7 Update exactly once | `test_ac7_vio_triggers_update_once`, `test_ac7_pose_anchor_marginals_triggers_update_once`, `test_ac7_pose_anchor_jacobian_triggers_no_update`, `test_ac7_imu_second_window_triggers_update_once` | PASS |
|
||||
| AC-8 `_last_anchor_ns` accuracy | `test_ac8_last_anchor_age_ms_after_anchor_is_small` | PASS |
|
||||
| Defensive — no-handle | `test_no_handle_raises_state_estimator_config_error` | PASS |
|
||||
|
||||
## Known forward actions (not in scope this batch)
|
||||
|
||||
- **AZ-388 (AC-5.2 fallback)** — first-keyframe seeding (prior factor + initial values on x0) so an end-to-end smoke test of `add_vio` → real iSAM2 update converges. The current AZ-383 tests use mock handles to avoid coupling the factor-add semantics to that scheduling.
|
||||
- **AZ-384** — `current_estimate` / `smoothed_history` / `health_snapshot` bodies; will read marginals from the handle and translate them into the C5 `EstimatorOutput` / `EstimatorHealth` DTOs.
|
||||
- **Composition root batching** — turn the C8 `ImuTelemetrySample` stream into `ImuWindow` instances before calling `add_fc_imu`. Owned by the composition-root binding task.
|
||||
- **Cross-stream timestamp merge** — AZ-383 enforces per-call monotonicity but does not order the three streams against each other; the composition root will arrange that.
|
||||
@@ -8,7 +8,7 @@ status: in_progress
|
||||
sub_step:
|
||||
phase: 6
|
||||
name: implement-tasks
|
||||
detail: "batch 13 of N committed (AZ-382 c5 GtsamIsam2StateEstimator skeleton + ISam2GraphHandleImpl real bodies + defensive logging)"
|
||||
detail: "batch 14 of N committed (AZ-383 c5 factor adds: add_vio BetweenFactorPose3 + add_pose_anchor mode-dispatch + add_fc_imu CombinedImuFactor with v/b key namespaces)"
|
||||
retry_count: 0
|
||||
cycle: 1
|
||||
tracker: jira
|
||||
|
||||
Reference in New Issue
Block a user