gps-denied-onboard/_docs/02_document/components/07_c5_state/description.md

C5 — State Estimator

1. High-Level Overview

Purpose: own the GTSAM iSAM2 + IncrementalFixedLagSmoother (K=10–20 keyframes per D-C5-3) state. Fuse VioOutput (C1), PoseEstimate (C4), and FC IMU/attitude windows (C8 inbound) into the posterior pose with native 6×6 covariance via Marginals (D-C5-5 = (c)). Emit the smoothed corrected current frame to C8 for FC delivery; emit smoothed past-keyframes to C13 (FDR only — AC-4.5 internal smoothing, NOT FC retroactive correction).

Architectural Pattern: Strategy with two concrete implementations: GtsamIsam2StateEstimator (production-default) and EskfStateEstimator (mandatory simple-baseline). Selection at startup (ADR-001), BUILD_* gating (ADR-002), composition-root wired (ADR-009).

Upstream dependencies:

• C1 → VioOutput (relative pose + IMU bias).
• C4 → PoseEstimate (absolute satellite-anchored pose); C4 adds factors directly to C5's iSAM2 graph (shared substrate).
• C8 inbound side → FC ImuWindow + AttitudeWindow + GpsHealth (for warm-start AC-5.1, blackout AC-NEW-8, spoofing-promotion AC-NEW-2 / F7).

Downstream consumers:

• C8 outbound side (per-FC encoder) → EmittedExternalPosition (5 Hz periodic to FC).
• C6 (mid-flight tile gen via orthorectifier; C5 supplies the PoseEstimate + quality_metadata for tile emission).
• C13 FDR (smoothed past-keyframe estimates, source-set switch events, spoofing-rejection events).

2. Internal Interfaces

Interface: StateEstimator

Method	Input	Output	Async	Error Types
set_takeoff_origin (AZ-490, ADR-010)	origin: LatLonAlt, sigma_horiz_m: float, sigma_vert_m: float	None	No	StateEstimatorConfigError, EstimatorAlreadyStartedError
add_vio	VioOutput	None	No	EstimatorDegradedError, EstimatorFatalError
add_pose_anchor	PoseEstimate	None	No	EstimatorDegradedError, EstimatorFatalError
add_fc_imu	ImuWindow	None	No	EstimatorDegradedError
current_estimate	()	EstimatorOutput (smoothed current keyframe)	No	—
smoothed_history	n_keyframes: int	list[EstimatorOutput]	No	—
health_snapshot	()	EstimatorHealth	No	—

Input DTOs: see C1, C4, C8.

Output DTOs:

EstimatorOutput:
  frame_id:                       uuid
  position_wgs84:                 LatLonAlt
  orientation_world_T_body:       Quat (w, x, y, z)
  velocity_world:                 Vector3 (m/s)
  covariance_6x6:                 Matrix6
  source_label:                   enum {satellite_anchored, visual_propagated, dead_reckoned}
  last_satellite_anchor_age_ms:   int
  smoothed:                       bool — true for entries from `smoothed_history`
  emitted_at:                     monotonic_ns

EstimatorHealth:
  isam2_state:                   enum {INIT, TRACKING, DEGRADED, LOST}
  keyframe_count:                int
  cov_norm_growing_for_s:        float — AC-NEW-8 monotonicity check
  spoof_promotion_blocked:       bool — AC-NEW-2 / AC-NEW-8 gate state

3. External API Specification

Not applicable.

4. Data Access Patterns

C5 holds the GTSAM iSAM2 state in memory; persistent storage is only via FDR writes (C13 owns the file). No DB queries.

Storage Estimates

Table/Collection	Est. Row Count (1yr)	Row Size	Total Size	Growth Rate
In-memory keyframe window	up to 20 keyframes resident	~2 KB / keyframe (factors + values)	~40 KB	bounded by IncrementalFixedLagSmoother K=10–20

C5 is bounded by design — no unbounded growth.

5. Implementation Details

Algorithmic Complexity:

• iSAM2 update on factor add: amortised O(K) in keyframe count for the typical case; O(K^2) worst-case on relinearisation.
• Marginals.marginalCovariance(pose_key): O(K^3) in keyframe-window size; the dominant per-frame cost (~30–90 ms steady-state).
• IncrementalFixedLagSmoother keeps the active window bounded — older keyframes are marginalised out.

State Management:

• iSAM2 graph + Values + Marginals lifecycle for the flight.
• Cold-start ladder (ADR-010, AZ-490): set_takeoff_origin(origin, sigma_horiz_m, sigma_vert_m) MUST be invoked before any add_vio / add_fc_imu / add_pose_anchor call. The cold-start window closes on the first add_* call. iSAM2 attaches a PriorFactorPose3 at Pose3.Identity() (operator origin BECOMES local-ENU (0,0,0)) with diagonal sigmas [5°, 5°, 5°, sigma_horiz_m, sigma_horiz_m, sigma_vert_m]; ESKF seeds the nominal position to (0,0,0) and writes the position-block of the error covariance to diag(sigma_horiz_m², sigma_horiz_m², sigma_vert_m²). The method is strictly idempotent on identical args — re-invocation with byte-equal (origin, sigma_horiz_m, sigma_vert_m) is a no-op; re-invocation with different args raises StateEstimatorConfigError. Once the cold-start window closes, further calls raise EstimatorAlreadyStartedError (subclass of StateEstimatorConfigError). Defaults default_takeoff_origin_sigma_horiz_m = 5.0, default_takeoff_origin_sigma_vert_m = 10.0 live in C5StateConfig.
• Source-label state machine: tracks the AC-NEW-2 / AC-NEW-8 spoofing-promotion gate (≥10 s + visual consistency check + ≤ 200 m bounded-delta before re-promoting a previously-spoofed FC GPS source).
• Last-anchor-age timer for AC-1.3 binning.

Key Dependencies:

Library	Version	Purpose
GTSAM (Python + C++)	per Plan-phase pin	iSAM2 + CombinedImuFactor + BetweenFactorPose3 + GenericProjectionFactorCal3DS2 + Marginals
gtsam_unstable.IncrementalFixedLagSmoother	per Plan-phase pin	Bounded keyframe window (D-C5-3 K=10–20)
Eigen	matches GTSAM	Lie-algebra math

Error Handling Strategy:

• StateEstimatorConfigError: set_takeoff_origin called with a malformed LatLonAlt (out of WGS-84 bounds / non-finite) OR with non-positive / non-finite sigmas, OR re-called inside the cold-start window with conflicting args. EstimatorAlreadyStartedError (a StateEstimatorConfigError subclass): set_takeoff_origin called after the first add_* call sealed the cold-start window. Caller must surface to operator; takeoff blocked.
• EstimatorDegradedError: factor add yielded poor convergence; covariance inflated; emit EstimatorOutput with degraded label.
• EstimatorFatalError: iSAM2 numerical failure, KEYFRAME_LIMIT exceeded, etc.; emit no EstimatorOutput for this tick. AC-5.2 fallback (3 s no estimate → FC IMU-only) applies.
• Spoof-promotion gate (Principle #11 amended, AZ-385 + AZ-490 follow-up): never re-introduce a previously-spoofed FC GPS source until ALL THREE hold — (i) FC gps_health == STABLE_NON_SPOOFED for ≥ 10 s, (ii) the next satellite-anchored frame agrees with the FC GPS within a configurable tolerance, AND (iii) the FC's reported position is within ≤ 200 m of the companion's last emitted PoseEstimate. The same gate is applied at takeoff when a Manifest takeoff_origin is present: an FC GPS reading that disagrees with the operator origin by > 200 m is logged as suspect and the operator origin wins. Document every reject in FDR + GCS STATUSTEXT.

6. Extensions and Helpers

Helper	Purpose	Used By
ImuPreintegrator	shared with C1	C1, C5
SE3Utils	shared with C1, C4	C1, C4, C5
WgsConverter	shared with C4, C8	C4, C5, C8
SourceLabelStateMachine	spoofing-promotion gate logic	C5 only — keep inside the component

7. Caveats & Edge Cases

Known limitations:

• AC-4.5 internal smoothing is onboard only; the FC log is forward-time only. The smoothed past-keyframe estimates go to FDR, not back to the FC.
• iSAM2 + IncrementalFixedLagSmoother requires careful key management; missing keys cause silent factor-add failures — the implementation MUST log every add_* call's success/failure status.

Potential race conditions:

• Single writer thread for the iSAM2 graph by design. C1 + C4 + C8-inbound deliver to a timestamp-ordered merge queue ahead of C5's writer thread.

Performance bottlenecks:

• Marginals.marginalCovariance(pose_key) is the per-frame hot spot. D-CROSS-LATENCY-1 hybrid degrades C4's covariance recovery (not C5's) under thermal throttle.

8. Dependency Graph

Must be implemented after: C1 (input), C4 (input + shared graph), C8 inbound (FC IMU prior).

Can be implemented in parallel with: C6, C13 — independent paths.

Blocks: C8 outbound (no per-frame estimate), F3 / F4 / F5 / F7 / F9 / F10.

9. Logging Strategy

Log Level	When	Example
ERROR	EstimatorFatalError; iSAM2 numerical failure; AC-5.2 path imminent	C5 fatal iSAM2 failure; frame=12345; AC-5.2 fallback
WARN	EstimatorDegradedError; spoofing-promotion blocked; cov norm growing >2× steady	C5 degraded: cov_inflation=3.1, spoof_block=true
INFO	Strategy ready; warm-start applied; spoof-promotion gate state changes	C5 ready: estimator=gtsam_isam2, K=15
DEBUG	per-frame factor adds + smoothed history depth	C5 frame=12345 vio_added=true pose_added=true imu_added=true smoothed_n=15

Log format: structured JSON. Log storage: stdout / journald / FDR via C13 (ERROR + WARN always; smoothed past-keyframe entries always go to FDR per AC-4.5; spoofing-promotion-block events go to FDR + GCS STATUSTEXT).