gps-denied-desktop/docs/02_components/08_State_Estimator/spec.md

State Estimator Component

Detailed Description

The State Estimator is the "Brain" of the system. It utilizes a Factor Graph (GTSAM) to fuse information from all sources:

• Relative factors from L1 (Frame t-1 \to t).
• Absolute pose factors from L3 (Frame t \to World).
• Prior factors (Altitude, Smoothness).
• User Inputs (Manual geometric constraints).

It handles the optimization window (Smoothing and Mapping) and outputs the final trajectory. It also manages the logic for "350m outlier" rejection using robust error kernels (Huber/Cauchy).

API Methods

add_frame_update

• Input: timestamp: float, l1_data: L1Result, l3_data: L3Result | None, altitude_prior: float
• Output: EstimatedState
• Description:
  1. Adds a new node to the graph for time t.
  2. Adds factor for L1 (if tracking valid).
  3. Adds factor for L3 (if global match found).
  4. Adds altitude prior.
  5. Performs incremental optimization (iSAM2).
  6. Returns optimized state { lat, lon, alt, roll, pitch, yaw, uncertainty_covariance }.
• Test Cases:
  • Sequence with drift -> L3 update snaps trajectory back to truth.
  • Outlier L3 input (350m off) -> Robust kernel ignores it, trajectory stays smooth.

add_user_correction

• Input: timestamp: float, uav_pixel: tuple, sat_coord: LatLon
• Output: void
• Description: Adds a strong "Pin" constraint to the graph at the specified past timestamp and re-optimizes.
• Test Cases:
  • Retroactive fix -> Updates current position estimate based on past correction.

get_current_state

• Input: void
• Output: EstimatedState
• Description: Returns the latest optimized pose.

Integration Tests

• Full Graph Test: Feed synthetic noisy data. Verify that the output error is lower than the input noise (fusion gain).

Non-functional Tests

• Stability: Ensure graph doesn't explode (numerical instability) over long sequences (2000+ frames).