mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-06-22 16:01:14 +00:00

Files

T

Oleksandr Bezdieniezhnykh b3ad94c155 [AZ-384] C5 marginals + current_estimate/smoothed_history/health_snapshot

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>

2026-05-11 06:20:01 +03:00

5.6 KiB

Raw Permalink Blame History

C5 GtsamIsam2StateEstimator — Marginals + current_estimate / smoothed_history / health_snapshot

Task: AZ-384_c5_marginals_outputs Name: C5 GtsamIsam2StateEstimator — Marginals + output methods Description: Implement current_estimate(), smoothed_history(n), and health_snapshot() on GtsamIsam2StateEstimator. current_estimate(): get the current pose key from the most-recent frame; recover the 6×6 covariance via _isam2_handle.compute_marginals().marginalCovariance(pose_key); convert local-tangent-plane pose to WGS84 via WgsConverter; assemble EstimatorOutput(smoothed=False, source_label = <gate state>, last_satellite_anchor_age_ms = handle.last_anchor_age_ms()). smoothed_history(n): return up to min(n, K) smoothed past keyframes from IncrementalFixedLagSmoother.calculateEstimate() projection; each entry has smoothed=True. health_snapshot(): report IsamState (INIT/TRACKING/DEGRADED/LOST) based on convergence quality; keyframe_count = len(_smoother.timestamps()); cov_norm_growing_for_s (rolling counter incremented when frame-to-frame cov norm rises monotonically per AC-NEW-8); spoof_promotion_blocked (queries the source-label state machine — owned by AZ-385; this task introduces a stub that returns False until AZ-385 lands). SPD-invariant defensive check on every emitted covariance. Complexity: 3 points Dependencies: AZ-383 (graph populated with factors), AZ-382 / AZ-381 (handle + Protocol), AZ-279 (WgsConverter), AZ-277 (SE3Utils), AZ-263, AZ-269, AZ-266, AZ-272 Component: c5_state (epic AZ-260 / E-C5) Tracker: AZ-384 Epic: AZ-260 (E-C5)

Document Dependencies

_docs/02_document/contracts/c5_state/state_estimator_protocol.md — Invariants 4 (current_estimate fresh), 6/7 (smoothed history bounded + flagged), 10 (SPD).
_docs/02_document/components/07_c5_state/description.md — § 2 outputs; § 7 Marginals dominant cost.

Problem

Without this task, the system has no way to read the posterior pose; downstream C8 cannot emit FC corrections; FDR has no smoothed history.

Outcome

current_estimate() body: get current pose + Marginals 6×6; WGS84-convert via helper; assemble EstimatorOutput.
smoothed_history(n) body: iterate smoother's active keyframes; build EstimatorOutput(smoothed=True) for each.
health_snapshot() body: IsamState derivation + keyframe_count + cov_norm_growing_for_s rolling counter + spoof_promotion_blocked (from injected source-label state machine; default False until AZ-79).
_cov_norm_window private rolling-window counter (60 s lazy-pruned) for AC-NEW-8 monotonicity check.
SPD-invariant defensive check before every EstimatorOutput emission; on failure raise EstimatorFatalError.
Constructor extension: optional source_label_state_machine arg (default None; AZ-79 wires it up).

Scope

Included

All three method bodies.
_cov_norm_window rolling-window counter.
SPD defensive check.
Source-label state machine injection point (Optional, default None).
Unit tests: synthetic graph with known pose → current_estimate() returns expected pose+covariance; smoothed_history(20) bounded by K=15; SPD invariant; IsamState derivation; cov_norm_growing_for_s monotonicity counter accuracy.

Excluded

Source-label state machine impl — owned by AZ-79.
Spoof gate logic body — owned by AZ-79.
AC-5.2 fallback — owned by AZ-81.
ESKF baseline — owned by AZ-80.

Acceptance Criteria

AC-1: current_estimate returns fresh EstimatorOutput — every call returns a new instance with smoothed=False.

AC-2: SPD covariance — np.linalg.cholesky(out.covariance_6x6) succeeds for every emitted output; non-SPD raises EstimatorFatalError.

AC-3: WGS84 conversion — uses shared WgsConverter; output matches helper test vectors.

AC-4: smoothed_history(n) bounded by K — len(smoothed_history(100)) <= K=15; each has smoothed=True.

AC-5: current_estimate has smoothed=False — distinguishes from history.

AC-6: health_snapshot.isam2_state matches convergence quality — INIT before first factor; TRACKING after; DEGRADED on inflated cov; LOST on EstimatorFatalError.

AC-7: keyframe_count accuracy — matches IncrementalFixedLagSmoother.timestamps().size().

AC-8: cov_norm_growing_for_s — increments while consecutive frames show monotone-rising cov norm; resets to 0 on a non-rising frame.

AC-9: spoof_promotion_blocked via injected state machine — queries source_label_state_machine.is_spoof_promotion_blocked(); default False if no state machine wired.

AC-10: last_satellite_anchor_age_ms pass-through — every EstimatorOutput.last_satellite_anchor_age_ms == handle.last_anchor_age_ms().

Non-Functional Requirements

current_estimate p95 ≤ 60 ms (Marginals dominant).
smoothed_history(K) p99 ≤ 20 ms.
health_snapshot p99 ≤ 5 µs (O(1) accumulator reads).

Constraints

Single-writer thread.
SPD defensive check is mandatory.
WgsConverter use is mandatory (no inline math).

Risks & Mitigation

Risk: IncrementalFixedLagSmoother.calculateEstimate() returns full Values not just keyframe poses — filter by key; verify against pinned GTSAM API.
Risk: SPD invariant fails under iSAM2 numerical instability — defensive raise (AC-2) maps to EstimatorFatalError; AC-5.2 fallback then triggers.

Runtime Completeness

Named capability: posterior pose + covariance recovery + smoothed history.
Production code: real Marginals, real WGS84 conversion, real rolling-window counter, real SPD defensive check.
Unacceptable substitutes: synthetic Marginals; inline WGS84 math.

5.6 KiB Raw Permalink Blame History Unescape Escape