mirror of
https://github.com/azaion/gps-denied-desktop.git
synced 2026-04-23 05:06:37 +00:00
add solution drafts, add component decomposition , add spec for other docs
This commit is contained in:
@@ -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).
|
||||
|
||||
Reference in New Issue
Block a user