add solution drafts, add component decomposition , add spec for other docs

This commit is contained in:
Oleksandr Bezdieniezhnykh
2025-11-19 23:07:29 +02:00
parent e87c33b0ee
commit 30339402f7
24 changed files with 2506 additions and 3 deletions
@@ -0,0 +1,45 @@
# 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).