# 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).