mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-23 04:36:38 +00:00
docs(01-eskf-core): create phase plan — 3 plans in 2 waves
This commit is contained in:
Yuzviak
@@ -0,0 +1,355 @@
|
||||
---
|
||||
phase: 01-eskf-core
|
||||
plan: 01
|
||||
type: execute
|
||||
wave: 1
|
||||
depends_on: []
|
||||
files_modified:
|
||||
- src/gps_denied/core/eskf.py
|
||||
- src/gps_denied/schemas/eskf.py
|
||||
autonomous: true
|
||||
requirements:
|
||||
- ESKF-01
|
||||
- ESKF-02
|
||||
- ESKF-03
|
||||
- ESKF-04
|
||||
- ESKF-05
|
||||
|
||||
must_haves:
|
||||
truths:
|
||||
- "ESKF propagates nominal state (position, velocity, quaternion, biases) from IMU inputs and covariance grows between measurement updates"
|
||||
- "VO measurement update reduces position uncertainty and innovation is within expected bounds"
|
||||
- "Satellite measurement update corrects absolute position and covariance tightens to satellite noise level"
|
||||
- "Confidence tier outputs HIGH when satellite correction is recent and covariance small, MEDIUM on VO-only, LOW on IMU-only"
|
||||
- "ESKF initializes from a GPS position with high-uncertainty covariance"
|
||||
artifacts:
|
||||
- path: "src/gps_denied/core/eskf.py"
|
||||
provides: "15-state ESKF implementation"
|
||||
min_lines: 250
|
||||
exports: ["ESKF"]
|
||||
- path: "src/gps_denied/schemas/eskf.py"
|
||||
provides: "ESKF data contracts"
|
||||
exports: ["ESKFState", "ESKFConfig", "ConfidenceTier", "IMUMeasurement"]
|
||||
key_links:
|
||||
- from: "src/gps_denied/core/eskf.py"
|
||||
to: "src/gps_denied/schemas/eskf.py"
|
||||
via: "import ESKFState, ESKFConfig, ConfidenceTier, IMUMeasurement"
|
||||
pattern: "from gps_denied\\.schemas\\.eskf import"
|
||||
- from: "src/gps_denied/core/eskf.py"
|
||||
to: "numpy"
|
||||
via: "matrix math for F, Q, P, K, H"
|
||||
pattern: "import numpy as np"
|
||||
---
|
||||
|
||||
<objective>
|
||||
Implement the 15-state Error-State Kalman Filter (ESKF) that fuses IMU, VO, and satellite measurements and outputs confidence-tiered position estimates.
|
||||
|
||||
Purpose: The ESKF is the central position estimator — without it, no GPS_INPUT messages can be generated, no IMU prediction can fill inter-frame gaps, and no confidence tier can drive fix_type.
|
||||
|
||||
Output: `src/gps_denied/core/eskf.py` (ESKF class) and `src/gps_denied/schemas/eskf.py` (data contracts).
|
||||
</objective>
|
||||
|
||||
<execution_context>
|
||||
@$HOME/.claude/get-shit-done/workflows/execute-plan.md
|
||||
@$HOME/.claude/get-shit-done/templates/summary.md
|
||||
</execution_context>
|
||||
|
||||
<context>
|
||||
@.planning/PROJECT.md
|
||||
@.planning/ROADMAP.md
|
||||
@.planning/STATE.md
|
||||
@.planning/codebase/CONVENTIONS.md
|
||||
@_docs/01_solution/solution.md (lines 127-253 — ESKF spec, confidence tiers, GPS_INPUT fields)
|
||||
@src/gps_denied/schemas/__init__.py
|
||||
@src/gps_denied/schemas/vo.py
|
||||
@src/gps_denied/core/coordinates.py
|
||||
</context>
|
||||
|
||||
<tasks>
|
||||
|
||||
<task type="auto">
|
||||
<name>Task 1: Create ESKF schema contracts</name>
|
||||
<files>src/gps_denied/schemas/eskf.py</files>
|
||||
<read_first>
|
||||
- src/gps_denied/schemas/__init__.py (GPSPoint, CameraParameters — the pattern for Pydantic schemas in this project)
|
||||
- src/gps_denied/schemas/vo.py (RelativePose — the VO output that ESKF consumes; note model_config for numpy)
|
||||
- _docs/01_solution/solution.md (lines 127-253 for state vector, confidence tiers, GPS_INPUT mapping)
|
||||
</read_first>
|
||||
<action>
|
||||
Create `src/gps_denied/schemas/eskf.py` with these exact types:
|
||||
|
||||
```python
|
||||
"""Error-State Kalman Filter schemas."""
|
||||
|
||||
from enum import Enum
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
from pydantic import BaseModel, Field
|
||||
|
||||
|
||||
class ConfidenceTier(str, Enum):
|
||||
"""ESKF confidence tier for GPS_INPUT fix_type mapping."""
|
||||
HIGH = "HIGH" # Satellite match <30s ago, covariance < 400 m^2
|
||||
MEDIUM = "MEDIUM" # VO tracking OK, no recent satellite match
|
||||
LOW = "LOW" # IMU-only, cuVSLAM lost
|
||||
FAILED = "FAILED" # 3+ consecutive total failures
|
||||
|
||||
|
||||
class IMUMeasurement(BaseModel):
|
||||
"""Single IMU reading from flight controller."""
|
||||
model_config = {"arbitrary_types_allowed": True}
|
||||
|
||||
accel: np.ndarray # (3,) m/s^2 in body frame
|
||||
gyro: np.ndarray # (3,) rad/s in body frame
|
||||
timestamp: float # seconds since epoch
|
||||
|
||||
|
||||
class ESKFConfig(BaseModel):
|
||||
"""ESKF tuning parameters."""
|
||||
|
||||
# Process noise (from IMU datasheet — ICM-42688-P)
|
||||
accel_noise_density: float = 6.86e-4 # 70 ug/sqrt(Hz) -> m/s^2/sqrt(Hz)
|
||||
gyro_noise_density: float = 5.24e-5 # 3.0e-3 deg/s/sqrt(Hz) -> rad/s/sqrt(Hz)
|
||||
accel_random_walk: float = 2.0e-3 # m/s^3/sqrt(Hz)
|
||||
gyro_random_walk: float = 8.73e-7 # 5.0e-5 deg/s^2/sqrt(Hz) -> rad/s^2/sqrt(Hz)
|
||||
|
||||
# VO measurement noise
|
||||
vo_position_noise: float = 0.3 # meters (cuVSLAM at 600m altitude)
|
||||
|
||||
# Satellite measurement noise range
|
||||
sat_noise_min: float = 5.0 # meters (high-confidence RANSAC)
|
||||
sat_noise_max: float = 20.0 # meters (low-confidence RANSAC)
|
||||
|
||||
# Confidence tier thresholds
|
||||
satellite_max_age: float = 30.0 # seconds
|
||||
covariance_high_threshold: float = 400.0 # m^2 (trace of position covariance)
|
||||
|
||||
# Initial covariance diagonals
|
||||
init_pos_var: float = 100.0 # m^2
|
||||
init_vel_var: float = 1.0 # (m/s)^2
|
||||
init_att_var: float = 0.01 # rad^2
|
||||
init_accel_bias_var: float = 0.01 # (m/s^2)^2
|
||||
init_gyro_bias_var: float = 1e-6 # (rad/s)^2
|
||||
|
||||
|
||||
class ESKFState(BaseModel):
|
||||
"""Full ESKF nominal state snapshot."""
|
||||
model_config = {"arbitrary_types_allowed": True}
|
||||
|
||||
position: np.ndarray # (3,) NED meters from origin
|
||||
velocity: np.ndarray # (3,) NED m/s
|
||||
quaternion: np.ndarray # (4,) [w, x, y, z] body-to-NED
|
||||
accel_bias: np.ndarray # (3,) m/s^2
|
||||
gyro_bias: np.ndarray # (3,) rad/s
|
||||
covariance: np.ndarray # (15, 15)
|
||||
timestamp: float # seconds since epoch
|
||||
confidence: ConfidenceTier
|
||||
last_satellite_time: Optional[float] = None
|
||||
last_vo_time: Optional[float] = None
|
||||
```
|
||||
|
||||
Also update `src/gps_denied/schemas/__init__.py` to add re-exports:
|
||||
|
||||
Add these imports at the end of the file:
|
||||
```python
|
||||
from gps_denied.schemas.eskf import ConfidenceTier, ESKFConfig, ESKFState, IMUMeasurement
|
||||
```
|
||||
|
||||
Per ESKF-01 (state vector), ESKF-04 (initialization config), ESKF-05 (confidence tiers).
|
||||
</action>
|
||||
<verify>
|
||||
<automated>python -c "from gps_denied.schemas.eskf import ESKFState, ESKFConfig, ConfidenceTier, IMUMeasurement; print('OK')"</automated>
|
||||
</verify>
|
||||
<acceptance_criteria>
|
||||
- src/gps_denied/schemas/eskf.py contains `class ConfidenceTier(str, Enum)` with members HIGH, MEDIUM, LOW, FAILED
|
||||
- src/gps_denied/schemas/eskf.py contains `class IMUMeasurement(BaseModel)` with fields accel, gyro, timestamp
|
||||
- src/gps_denied/schemas/eskf.py contains `class ESKFConfig(BaseModel)` with fields accel_noise_density, gyro_noise_density, satellite_max_age, covariance_high_threshold
|
||||
- src/gps_denied/schemas/eskf.py contains `class ESKFState(BaseModel)` with fields position, velocity, quaternion, accel_bias, gyro_bias, covariance, timestamp, confidence
|
||||
- ESKFState.quaternion comment documents [w, x, y, z] convention
|
||||
- ESKFState.position comment documents NED meters from origin
|
||||
- `python -c "from gps_denied.schemas.eskf import ESKFState, ESKFConfig, ConfidenceTier, IMUMeasurement"` exits 0
|
||||
</acceptance_criteria>
|
||||
<done>All four ESKF schema classes importable, fields match solution.md spec exactly, ConfidenceTier has all four tiers</done>
|
||||
</task>
|
||||
|
||||
<task type="auto" tdd="true">
|
||||
<name>Task 2: Implement ESKF core algorithm</name>
|
||||
<files>src/gps_denied/core/eskf.py</files>
|
||||
<read_first>
|
||||
- src/gps_denied/schemas/eskf.py (just created — the contracts this implementation uses)
|
||||
- src/gps_denied/core/coordinates.py (CoordinateTransformer — used for NED/WGS84 conversion, gps_to_enu, enu_to_gps methods)
|
||||
- src/gps_denied/schemas/vo.py (RelativePose — the VO measurement input; fields: translation, rotation, confidence, scale_ambiguous)
|
||||
- src/gps_denied/schemas/__init__.py (GPSPoint — used for initialization from GPS position)
|
||||
- _docs/01_solution/solution.md (lines 127-178 for ESKF math, lines 244-253 for confidence tiers)
|
||||
- src/gps_denied/core/vo.py (to understand interface pattern: ABC + concrete class in same module)
|
||||
</read_first>
|
||||
<behavior>
|
||||
- Test: ESKF initialized with GPS position has state.position == gps_to_enu(gps), quaternion == [1,0,0,0], covariance diagonal matches config init vars
|
||||
- Test: After predict() with zero IMU and dt=0.01, position unchanged, covariance[0:3,0:3] trace increases
|
||||
- Test: After predict() with accel=[0,0,-9.81] (gravity in body NED) and dt=0.1, velocity change is near zero (gravity compensated)
|
||||
- Test: VO update with zero innovation leaves state unchanged, covariance does not increase
|
||||
- Test: VO update with 1m innovation in north reduces position covariance and shifts position toward measurement
|
||||
- Test: Satellite update with known position reduces position covariance below pre-update level
|
||||
- Test: Confidence is HIGH when last_satellite_time < 30s ago and position covariance trace < 400
|
||||
- Test: Confidence is MEDIUM when last_vo_time is recent but no satellite
|
||||
- Test: Confidence is LOW when only IMU (no VO, no satellite)
|
||||
- Test: Confidence is FAILED when get_confidence called with consecutive_failures >= 3
|
||||
</behavior>
|
||||
<action>
|
||||
Create `src/gps_denied/core/eskf.py` implementing the full 15-state Error-State Kalman Filter.
|
||||
|
||||
Module docstring: `"""Error-State Kalman Filter (ESKF) — 15-state sensor fusion (Component F17)."""`
|
||||
|
||||
Class `ESKF`:
|
||||
|
||||
**Constructor** `__init__(self, config: ESKFConfig | None = None)`:
|
||||
- Store config (default ESKFConfig())
|
||||
- Initialize: `_initialized: bool = False`, `_nominal_state: dict | None = None`, `_P: np.ndarray | None = None` (15x15 covariance), `_last_satellite_time: float | None = None`, `_last_vo_time: float | None = None`, `_last_timestamp: float | None = None`
|
||||
|
||||
**`initialize(self, position_ned: np.ndarray, timestamp: float, velocity: np.ndarray | None = None, quaternion: np.ndarray | None = None) -> None`**:
|
||||
- Set nominal state: position=position_ned, velocity=velocity or zeros(3), quaternion=quaternion or [1,0,0,0] (identity), accel_bias=zeros(3), gyro_bias=zeros(3)
|
||||
- Initialize P as 15x15 diagonal: [init_pos_var]*3 + [init_vel_var]*3 + [init_att_var]*3 + [init_accel_bias_var]*3 + [init_gyro_bias_var]*3
|
||||
- Set _initialized=True, _last_timestamp=timestamp
|
||||
|
||||
**`initialize_from_gps(self, gps: GPSPoint, altitude: float, timestamp: float, coord_transformer: CoordinateTransformer, flight_id: str) -> None`**:
|
||||
- Convert GPS to NED using coord_transformer.gps_to_enu(flight_id, gps) (note: existing code uses ENU, treat as [East, North, Up] -> convert to NED: [North, East, -Up] — BUT the existing codebase uses ENU throughout, so store as ENU to match CoordinateTransformer convention)
|
||||
- IMPORTANT: The existing CoordinateTransformer uses ENU (East, North, Up). Store position in ENU to maintain compatibility. Document this in a comment.
|
||||
- Call self.initialize(position_enu, timestamp)
|
||||
|
||||
Per ESKF-04: initialization from GLOBAL_POSITION_INT.
|
||||
|
||||
**`predict(self, imu: IMUMeasurement) -> None`** (per ESKF-01):
|
||||
- Compute dt = imu.timestamp - self._last_timestamp. If dt <= 0 or dt > 1.0, skip.
|
||||
- Extract corrected measurements: a_corrected = imu.accel - self._nominal_state["accel_bias"], w_corrected = imu.gyro - self._nominal_state["gyro_bias"]
|
||||
- Get rotation matrix R from quaternion: R = _quat_to_rotation_matrix(self._nominal_state["quaternion"])
|
||||
- Gravity vector g = np.array([0, 0, -9.81]) (in ENU, up is positive, gravity points down)
|
||||
- Propagate nominal state:
|
||||
- position += velocity * dt
|
||||
- velocity += (R @ a_corrected + g) * dt
|
||||
- quaternion = _quat_multiply(quaternion, _exp_quaternion(w_corrected * dt))
|
||||
- quaternion = quaternion / np.linalg.norm(quaternion) (normalize)
|
||||
- Biases unchanged (random walk modeled in Q)
|
||||
- Build 15x15 state transition matrix F:
|
||||
- F = I_15
|
||||
- F[0:3, 3:6] = I_3 * dt (position depends on velocity)
|
||||
- F[3:6, 6:9] = -R @ _skew_symmetric(a_corrected) * dt (velocity depends on attitude error)
|
||||
- F[3:6, 9:12] = -R * dt (velocity depends on accel bias)
|
||||
- F[6:9, 12:15] = -I_3 * dt (attitude depends on gyro bias)
|
||||
- Build 15x15 process noise Q:
|
||||
- Q = zeros(15, 15)
|
||||
- Q[3:6, 3:6] = (config.accel_noise_density**2) * dt * I_3 (velocity noise from accel)
|
||||
- Q[6:9, 6:9] = (config.gyro_noise_density**2) * dt * I_3 (attitude noise from gyro)
|
||||
- Q[9:12, 9:12] = (config.accel_random_walk**2) * dt * I_3 (accel bias random walk)
|
||||
- Q[12:15, 12:15] = (config.gyro_random_walk**2) * dt * I_3 (gyro bias random walk)
|
||||
- Propagate covariance: P = F @ P @ F.T + Q
|
||||
- Update _last_timestamp
|
||||
|
||||
**`update_vo(self, relative_position: np.ndarray, dt_vo: float) -> np.ndarray`** (per ESKF-02):
|
||||
- Predicted relative position = self._nominal_state["velocity"] * dt_vo
|
||||
- Innovation z = relative_position - predicted_relative_position (3,)
|
||||
- H_vo = zeros(3, 15); H_vo[0:3, 3:6] = I_3 * dt_vo (maps velocity error to relative position)
|
||||
- Alternative simpler H: H_vo[0:3, 0:3] = I_3 (direct position observation of relative displacement)
|
||||
- Use the simpler form: H_vo observes the position error directly since relative_position is integrated
|
||||
- R_vo = config.vo_position_noise**2 * I_3
|
||||
- S = H_vo @ P @ H_vo.T + R_vo
|
||||
- K = P @ H_vo.T @ np.linalg.inv(S)
|
||||
- delta_x = K @ z (15,)
|
||||
- Apply error-state correction to nominal state:
|
||||
- position += delta_x[0:3]
|
||||
- velocity += delta_x[3:6]
|
||||
- quaternion = _quat_multiply(quaternion, _exp_quaternion(delta_x[6:9]))
|
||||
- quaternion /= np.linalg.norm(quaternion)
|
||||
- accel_bias += delta_x[9:12]
|
||||
- gyro_bias += delta_x[12:15]
|
||||
- Reset error state: P = (I_15 - K @ H_vo) @ P
|
||||
- Update _last_vo_time = self._last_timestamp
|
||||
- Return innovation z
|
||||
|
||||
**`update_satellite(self, position_enu: np.ndarray, noise_meters: float) -> np.ndarray`** (per ESKF-03):
|
||||
- Innovation z = position_enu - self._nominal_state["position"] (3,)
|
||||
- H_sat = zeros(3, 15); H_sat[0:3, 0:3] = I_3 (directly observes position)
|
||||
- R_sat = noise_meters**2 * I_3
|
||||
- S = H_sat @ P @ H_sat.T + R_sat
|
||||
- K = P @ H_sat.T @ np.linalg.inv(S)
|
||||
- delta_x = K @ z (15,)
|
||||
- Apply same error-state correction as update_vo
|
||||
- P = (I_15 - K @ H_sat) @ P
|
||||
- Update _last_satellite_time = self._last_timestamp
|
||||
- Return innovation z
|
||||
|
||||
**`get_confidence(self, consecutive_failures: int = 0) -> ConfidenceTier`** (per ESKF-05):
|
||||
- If consecutive_failures >= 3: return FAILED
|
||||
- current_time = self._last_timestamp
|
||||
- pos_cov_trace = np.trace(self._P[0:3, 0:3])
|
||||
- If _last_satellite_time is not None and (current_time - _last_satellite_time) < config.satellite_max_age and pos_cov_trace < config.covariance_high_threshold: return HIGH
|
||||
- If _last_vo_time is not None and (current_time - _last_vo_time) < 5.0: return MEDIUM
|
||||
- return LOW
|
||||
|
||||
**`get_state(self) -> ESKFState`**:
|
||||
- Return ESKFState with all current nominal state fields, covariance=self._P.copy(), timestamp=self._last_timestamp, confidence=self.get_confidence()
|
||||
|
||||
**`@property initialized(self) -> bool`**: return self._initialized
|
||||
|
||||
**Helper functions** (module-level, private):
|
||||
|
||||
- `_quat_to_rotation_matrix(q: np.ndarray) -> np.ndarray`: Convert [w,x,y,z] quaternion to 3x3 rotation matrix using standard formula: R = (2w^2-1)I + 2(v*v^T) + 2w*skew(v) where v=[x,y,z]
|
||||
- `_quat_multiply(q1: np.ndarray, q2: np.ndarray) -> np.ndarray`: Hamilton product [w,x,y,z] convention
|
||||
- `_exp_quaternion(theta: np.ndarray) -> np.ndarray`: Convert rotation vector (3,) to quaternion: angle = ||theta||, if angle < 1e-10 return [1,0,0,0], else return [cos(angle/2), sin(angle/2)*theta/angle]
|
||||
- `_skew_symmetric(v: np.ndarray) -> np.ndarray`: 3x3 skew-symmetric matrix from 3-vector
|
||||
|
||||
All functions use numpy only. No scipy dependency needed.
|
||||
</action>
|
||||
<verify>
|
||||
<automated>python -c "
|
||||
import numpy as np
|
||||
from gps_denied.core.eskf import ESKF
|
||||
from gps_denied.schemas.eskf import ESKFConfig, IMUMeasurement
|
||||
eskf = ESKF()
|
||||
eskf.initialize(np.zeros(3), 0.0)
|
||||
assert eskf.initialized
|
||||
imu = IMUMeasurement(accel=np.array([0., 0., 9.81]), gyro=np.zeros(3), timestamp=0.01)
|
||||
eskf.predict(imu)
|
||||
state = eskf.get_state()
|
||||
assert state.position is not None
|
||||
assert state.covariance.shape == (15, 15)
|
||||
print('ESKF basic test OK')
|
||||
"</automated>
|
||||
</verify>
|
||||
<acceptance_criteria>
|
||||
- src/gps_denied/core/eskf.py contains `class ESKF:`
|
||||
- src/gps_denied/core/eskf.py contains `def predict(self, imu: IMUMeasurement)` with F matrix construction and P = F @ P @ F.T + Q
|
||||
- src/gps_denied/core/eskf.py contains `def update_vo(self, relative_position: np.ndarray` with Kalman gain K = P @ H.T @ inv(S)
|
||||
- src/gps_denied/core/eskf.py contains `def update_satellite(self, position_enu: np.ndarray` with H_sat[0:3, 0:3] = I_3
|
||||
- src/gps_denied/core/eskf.py contains `def get_confidence(self` returning ConfidenceTier with checks for satellite_max_age and covariance_high_threshold
|
||||
- src/gps_denied/core/eskf.py contains `def initialize_from_gps(self, gps: GPSPoint`
|
||||
- src/gps_denied/core/eskf.py contains `def _quat_to_rotation_matrix(`
|
||||
- src/gps_denied/core/eskf.py contains `def _skew_symmetric(`
|
||||
- Covariance matrix P is 15x15
|
||||
- Error-state vector is 15-dimensional: delta_x[0:3] position, [3:6] velocity, [6:9] attitude, [9:12] accel_bias, [12:15] gyro_bias
|
||||
- `python -c "from gps_denied.core.eskf import ESKF"` exits 0
|
||||
</acceptance_criteria>
|
||||
<done>ESKF class fully implements: IMU prediction with F/Q matrices and bias propagation (ESKF-01), VO measurement update with Kalman gain (ESKF-02), satellite measurement update with absolute position correction (ESKF-03), GPS initialization with high-uncertainty covariance (ESKF-04), and confidence tier computation (ESKF-05)</done>
|
||||
</task>
|
||||
|
||||
</tasks>
|
||||
|
||||
<verification>
|
||||
1. `python -c "from gps_denied.core.eskf import ESKF; from gps_denied.schemas.eskf import ESKFState, ESKFConfig, ConfidenceTier, IMUMeasurement; print('All imports OK')"`
|
||||
2. `python -c "import numpy as np; from gps_denied.core.eskf import ESKF; e = ESKF(); e.initialize(np.zeros(3), 0.0); s = e.get_state(); assert s.covariance.shape == (15,15); print('State shape OK')"`
|
||||
3. `ruff check src/gps_denied/core/eskf.py src/gps_denied/schemas/eskf.py` passes
|
||||
</verification>
|
||||
|
||||
<success_criteria>
|
||||
- ESKF class importable and instantiable with default config
|
||||
- predict() propagates nominal state and grows covariance
|
||||
- update_vo() applies Kalman correction from relative pose
|
||||
- update_satellite() applies Kalman correction from absolute position
|
||||
- get_confidence() returns correct tier based on timing and covariance
|
||||
- initialize_from_gps() sets state from GPS coordinates
|
||||
- All math uses numpy, no external filter library
|
||||
</success_criteria>
|
||||
|
||||
<output>
|
||||
After completion, create `.planning/phases/01-eskf-core/01-01-SUMMARY.md`
|
||||
</output>
|
||||
@@ -0,0 +1,389 @@
|
||||
---
|
||||
phase: 01-eskf-core
|
||||
plan: 02
|
||||
type: execute
|
||||
wave: 1
|
||||
depends_on: []
|
||||
files_modified:
|
||||
- src/gps_denied/core/coordinates.py
|
||||
autonomous: true
|
||||
requirements:
|
||||
- ESKF-06
|
||||
|
||||
must_haves:
|
||||
truths:
|
||||
- "pixel_to_gps uses camera intrinsic matrix K to unproject pixel to camera ray, not fake 0.1m/pixel scaling"
|
||||
- "Camera-to-body transform uses T_cam_body rotation (nadir-pointing camera mount)"
|
||||
- "Body-to-NED rotation uses a quaternion parameter (from ESKF attitude), not hardcoded identity"
|
||||
- "Ray-ground intersection at known altitude produces NED position, then converts to WGS84"
|
||||
- "gps_to_pixel is the inverse: WGS84 -> NED -> body -> camera -> pixel"
|
||||
- "transform_points applies a real 3x3 homography via cv2.perspectiveTransform"
|
||||
artifacts:
|
||||
- path: "src/gps_denied/core/coordinates.py"
|
||||
provides: "Full coordinate chain: pixel -> camera ray -> body -> NED -> WGS84"
|
||||
contains: "np.linalg.inv"
|
||||
key_links:
|
||||
- from: "src/gps_denied/core/coordinates.py"
|
||||
to: "numpy"
|
||||
via: "K matrix construction, quaternion rotation, ray-ground intersection"
|
||||
pattern: "np\\.array"
|
||||
- from: "src/gps_denied/core/coordinates.py"
|
||||
to: "cv2"
|
||||
via: "perspectiveTransform for homography application"
|
||||
pattern: "cv2\\.perspectiveTransform"
|
||||
---
|
||||
|
||||
<objective>
|
||||
Replace all FAKE Math stubs in CoordinateTransformer with the real pixel-to-GPS coordinate chain: pixel -> camera ray (K matrix) -> body frame (T_cam_body) -> NED (quaternion) -> WGS84.
|
||||
|
||||
Purpose: Without real coordinate transforms, all position estimates, object localization, and satellite matching produce wrong results. The 0.1m/pixel approximation ignores altitude, camera intrinsics, and UAV attitude entirely.
|
||||
|
||||
Output: Updated `src/gps_denied/core/coordinates.py` with real geometric projections.
|
||||
</objective>
|
||||
|
||||
<execution_context>
|
||||
@$HOME/.claude/get-shit-done/workflows/execute-plan.md
|
||||
@$HOME/.claude/get-shit-done/templates/summary.md
|
||||
</execution_context>
|
||||
|
||||
<context>
|
||||
@.planning/PROJECT.md
|
||||
@.planning/ROADMAP.md
|
||||
@.planning/STATE.md
|
||||
@.planning/codebase/CONVENTIONS.md
|
||||
@_docs/01_solution/solution.md (lines 187-222 — coordinate system spec)
|
||||
@src/gps_denied/core/coordinates.py
|
||||
@src/gps_denied/schemas/__init__.py
|
||||
|
||||
<interfaces>
|
||||
<!-- Key types the executor needs -->
|
||||
|
||||
From src/gps_denied/schemas/__init__.py:
|
||||
```python
|
||||
class GPSPoint(BaseModel):
|
||||
lat: float = Field(..., ge=-90, le=90)
|
||||
lon: float = Field(..., ge=-180, le=180)
|
||||
|
||||
class CameraParameters(BaseModel):
|
||||
focal_length: float # mm
|
||||
sensor_width: float # mm
|
||||
sensor_height: float # mm
|
||||
resolution_width: int # pixels
|
||||
resolution_height: int # pixels
|
||||
principal_point: tuple[float, float] | None = None
|
||||
distortion_coefficients: list[float] | None = None
|
||||
```
|
||||
|
||||
From _docs/01_solution/solution.md (camera intrinsics):
|
||||
- ADTI 20L V1 + 16mm lens: fx = fy = 16 * 5456 / 23.2 = 3763 pixels
|
||||
- cx = 2728, cy = 1816
|
||||
- Camera mount: nadir-pointing (Z-down), R_cam_body = Rx(180deg)
|
||||
</interfaces>
|
||||
</context>
|
||||
|
||||
<tasks>
|
||||
|
||||
<task type="auto">
|
||||
<name>Task 1: Replace fake coordinate transforms with real camera projection math</name>
|
||||
<files>src/gps_denied/core/coordinates.py</files>
|
||||
<read_first>
|
||||
- src/gps_denied/core/coordinates.py (current file — understand existing methods, ENU convention, OriginNotSetError)
|
||||
- src/gps_denied/schemas/__init__.py (CameraParameters fields: focal_length in mm, sensor_width/height in mm, resolution_width/height in pixels, principal_point optional)
|
||||
- _docs/01_solution/solution.md (lines 187-222 for reference frames: Camera C, Body B, NED N, WGS84; transformation chain; T_cam_body spec)
|
||||
</read_first>
|
||||
<action>
|
||||
Modify `src/gps_denied/core/coordinates.py`. Keep all existing methods (set_enu_origin, get_enu_origin, gps_to_enu, enu_to_gps) UNCHANGED. Replace the implementations of `pixel_to_gps`, `gps_to_pixel`, `image_object_to_gps`, and `transform_points`.
|
||||
|
||||
Add imports at top:
|
||||
```python
|
||||
import cv2
|
||||
import numpy as np
|
||||
```
|
||||
|
||||
Add module-level helper function:
|
||||
|
||||
```python
|
||||
def _build_intrinsic_matrix(cam: CameraParameters) -> np.ndarray:
|
||||
"""Build 3x3 camera intrinsic matrix K from CameraParameters."""
|
||||
fx = cam.focal_length * cam.resolution_width / cam.sensor_width
|
||||
fy = cam.focal_length * cam.resolution_height / cam.sensor_height
|
||||
if cam.principal_point is not None:
|
||||
cx, cy = cam.principal_point
|
||||
else:
|
||||
cx = cam.resolution_width / 2.0
|
||||
cy = cam.resolution_height / 2.0
|
||||
return np.array([
|
||||
[fx, 0, cx],
|
||||
[ 0, fy, cy],
|
||||
[ 0, 0, 1],
|
||||
], dtype=np.float64)
|
||||
```
|
||||
|
||||
Add module-level helper:
|
||||
|
||||
```python
|
||||
def _cam_to_body_rotation() -> np.ndarray:
|
||||
"""Camera-to-body rotation for nadir-pointing camera.
|
||||
|
||||
Camera frame: Z forward (optical axis), X right, Y down (OpenCV convention).
|
||||
Body frame: X forward (nose), Y right (starboard), Z down.
|
||||
Camera points nadir: camera Z-axis aligns with body -Z (downward).
|
||||
|
||||
The rotation is Rx(180deg): flips Y and Z axes.
|
||||
R_cam_body = [[1, 0, 0], [0, -1, 0], [0, 0, -1]]
|
||||
"""
|
||||
return np.array([
|
||||
[1, 0, 0],
|
||||
[0, -1, 0],
|
||||
[0, 0, -1],
|
||||
], dtype=np.float64)
|
||||
```
|
||||
|
||||
Add module-level helper:
|
||||
|
||||
```python
|
||||
def _quat_to_rotation_matrix(q: np.ndarray) -> np.ndarray:
|
||||
"""Convert [w, x, y, z] quaternion to 3x3 rotation matrix."""
|
||||
w, x, y, z = q
|
||||
return np.array([
|
||||
[1 - 2*(y*y + z*z), 2*(x*y - w*z), 2*(x*z + w*y)],
|
||||
[2*(x*y + w*z), 1 - 2*(x*x + z*z), 2*(y*z - w*x)],
|
||||
[2*(x*z - w*y), 2*(y*z + w*x), 1 - 2*(x*x + y*y)],
|
||||
], dtype=np.float64)
|
||||
```
|
||||
|
||||
Replace `pixel_to_gps` method:
|
||||
|
||||
```python
|
||||
def pixel_to_gps(
|
||||
self,
|
||||
flight_id: str,
|
||||
pixel: tuple[float, float],
|
||||
frame_pose: dict,
|
||||
camera_params: CameraParameters,
|
||||
altitude: float,
|
||||
quaternion: np.ndarray | None = None,
|
||||
) -> GPSPoint:
|
||||
"""Unproject pixel to GPS via ray-ground intersection.
|
||||
|
||||
Chain: pixel -> camera ray (K^-1) -> body (T_cam_body) -> ENU (quaternion) -> WGS84.
|
||||
|
||||
Args:
|
||||
flight_id: Flight identifier for ENU origin lookup.
|
||||
pixel: (u, v) pixel coordinates.
|
||||
frame_pose: Dict with "position" key -> [east, north, up] in ENU meters.
|
||||
camera_params: Camera intrinsic parameters.
|
||||
altitude: UAV altitude above ground in meters (positive up).
|
||||
quaternion: [w, x, y, z] body-to-ENU rotation quaternion.
|
||||
If None, assumes identity (level flight, north-facing).
|
||||
"""
|
||||
# Step 1: Pixel -> camera ray
|
||||
K = _build_intrinsic_matrix(camera_params)
|
||||
K_inv = np.linalg.inv(K)
|
||||
pixel_h = np.array([pixel[0], pixel[1], 1.0])
|
||||
ray_cam = K_inv @ pixel_h # direction in camera frame
|
||||
|
||||
# Step 2: Camera -> body
|
||||
R_cam_body = _cam_to_body_rotation()
|
||||
ray_body = R_cam_body @ ray_cam
|
||||
|
||||
# Step 3: Body -> ENU
|
||||
if quaternion is not None:
|
||||
R_body_enu = _quat_to_rotation_matrix(quaternion)
|
||||
else:
|
||||
R_body_enu = np.eye(3)
|
||||
ray_enu = R_body_enu @ ray_body
|
||||
|
||||
# Step 4: Ray-ground intersection
|
||||
# UAV is at altitude (Up component), ground is at Up=0
|
||||
# Ray: P = P_uav + t * ray_enu
|
||||
# Ground plane: Up = 0 -> t = -altitude / ray_enu[2]
|
||||
if abs(ray_enu[2]) < 1e-10:
|
||||
# Ray parallel to ground — return UAV position projected
|
||||
frame_enu = frame_pose.get("position", [0, 0, 0])
|
||||
return self.enu_to_gps(flight_id, (frame_enu[0], frame_enu[1], 0.0))
|
||||
|
||||
t = -altitude / ray_enu[2]
|
||||
if t < 0:
|
||||
# Ray points upward — use UAV position
|
||||
frame_enu = frame_pose.get("position", [0, 0, 0])
|
||||
return self.enu_to_gps(flight_id, (frame_enu[0], frame_enu[1], 0.0))
|
||||
|
||||
frame_enu = frame_pose.get("position", [0, 0, 0])
|
||||
ground_east = frame_enu[0] + t * ray_enu[0]
|
||||
ground_north = frame_enu[1] + t * ray_enu[1]
|
||||
|
||||
# Step 5: ENU -> WGS84
|
||||
return self.enu_to_gps(flight_id, (ground_east, ground_north, 0.0))
|
||||
```
|
||||
|
||||
Replace `gps_to_pixel` method:
|
||||
|
||||
```python
|
||||
def gps_to_pixel(
|
||||
self,
|
||||
flight_id: str,
|
||||
gps: GPSPoint,
|
||||
frame_pose: dict,
|
||||
camera_params: CameraParameters,
|
||||
altitude: float,
|
||||
quaternion: np.ndarray | None = None,
|
||||
) -> tuple[float, float]:
|
||||
"""Project GPS coordinate to image pixel.
|
||||
|
||||
Inverse of pixel_to_gps: WGS84 -> ENU -> body -> camera -> pixel.
|
||||
"""
|
||||
# Step 1: GPS -> ENU
|
||||
enu = self.gps_to_enu(flight_id, gps)
|
||||
|
||||
# Step 2: ENU point relative to UAV
|
||||
frame_enu = frame_pose.get("position", [0, 0, 0])
|
||||
point_enu = np.array([
|
||||
enu[0] - frame_enu[0],
|
||||
enu[1] - frame_enu[1],
|
||||
-altitude, # ground is at -altitude relative to UAV (UAV is at +altitude)
|
||||
])
|
||||
|
||||
# Step 3: ENU -> body
|
||||
if quaternion is not None:
|
||||
R_body_enu = _quat_to_rotation_matrix(quaternion)
|
||||
else:
|
||||
R_body_enu = np.eye(3)
|
||||
point_body = R_body_enu.T @ point_enu
|
||||
|
||||
# Step 4: Body -> camera
|
||||
R_cam_body = _cam_to_body_rotation()
|
||||
point_cam = R_cam_body.T @ point_body
|
||||
|
||||
# Step 5: Camera -> pixel
|
||||
if abs(point_cam[2]) < 1e-10:
|
||||
cx = camera_params.resolution_width / 2.0
|
||||
cy = camera_params.resolution_height / 2.0
|
||||
return (cx, cy)
|
||||
|
||||
K = _build_intrinsic_matrix(camera_params)
|
||||
pixel_h = K @ (point_cam / point_cam[2])
|
||||
return (float(pixel_h[0]), float(pixel_h[1]))
|
||||
```
|
||||
|
||||
Replace `image_object_to_gps` method:
|
||||
|
||||
```python
|
||||
def image_object_to_gps(
|
||||
self,
|
||||
flight_id: str,
|
||||
frame_id: int,
|
||||
object_pixel: tuple[float, float],
|
||||
frame_pose: dict | None = None,
|
||||
camera_params: CameraParameters | None = None,
|
||||
altitude: float = 100.0,
|
||||
quaternion: np.ndarray | None = None,
|
||||
) -> GPSPoint:
|
||||
"""Convert object pixel coordinates to GPS using full projection chain.
|
||||
|
||||
If frame_pose or camera_params are not provided, uses defaults for
|
||||
backward compatibility (level flight, ADTI 20L V1 camera).
|
||||
"""
|
||||
if frame_pose is None:
|
||||
frame_pose = {"position": [0, 0, 0]}
|
||||
if camera_params is None:
|
||||
camera_params = CameraParameters(
|
||||
focal_length=16.0,
|
||||
sensor_width=23.2,
|
||||
sensor_height=15.4,
|
||||
resolution_width=5456,
|
||||
resolution_height=3632,
|
||||
)
|
||||
return self.pixel_to_gps(
|
||||
flight_id, object_pixel, frame_pose, camera_params, altitude, quaternion
|
||||
)
|
||||
```
|
||||
|
||||
Replace `transform_points` method:
|
||||
|
||||
```python
|
||||
def transform_points(
|
||||
self,
|
||||
points: list[tuple[float, float]],
|
||||
transformation: list[list[float]],
|
||||
) -> list[tuple[float, float]]:
|
||||
"""Apply 3x3 homography to a list of 2D points using cv2.perspectiveTransform."""
|
||||
if not points:
|
||||
return []
|
||||
H = np.array(transformation, dtype=np.float64)
|
||||
pts = np.array(points, dtype=np.float64).reshape(-1, 1, 2)
|
||||
transformed = cv2.perspectiveTransform(pts, H)
|
||||
return [(float(p[0][0]), float(p[0][1])) for p in transformed]
|
||||
```
|
||||
|
||||
Per ESKF-06: complete coordinate chain replacing all FAKE Math stubs.
|
||||
</action>
|
||||
<verify>
|
||||
<automated>python -c "
|
||||
import numpy as np
|
||||
from gps_denied.core.coordinates import CoordinateTransformer, _build_intrinsic_matrix
|
||||
from gps_denied.schemas import CameraParameters, GPSPoint
|
||||
|
||||
ct = CoordinateTransformer()
|
||||
ct.set_enu_origin('f1', GPSPoint(lat=48.0, lon=37.0))
|
||||
|
||||
# Test: image center at altitude 600m with identity quaternion should project near UAV position
|
||||
cam = CameraParameters(focal_length=16.0, sensor_width=23.2, sensor_height=15.4, resolution_width=5456, resolution_height=3632)
|
||||
pose = {'position': [0, 0, 0]}
|
||||
q_identity = np.array([1.0, 0.0, 0.0, 0.0])
|
||||
|
||||
gps = ct.pixel_to_gps('f1', (2728.0, 1816.0), pose, cam, 600.0, q_identity)
|
||||
# Center pixel should project to UAV nadir position
|
||||
assert abs(gps.lat - 48.0) < 0.001, f'lat={gps.lat}'
|
||||
assert abs(gps.lon - 37.0) < 0.001, f'lon={gps.lon}'
|
||||
|
||||
# Test K matrix
|
||||
K = _build_intrinsic_matrix(cam)
|
||||
assert K.shape == (3, 3)
|
||||
fx_expected = 16.0 * 5456 / 23.2
|
||||
assert abs(K[0, 0] - fx_expected) < 1.0, f'fx={K[0,0]}, expected={fx_expected}'
|
||||
|
||||
print('Coordinate chain tests OK')
|
||||
"</automated>
|
||||
</verify>
|
||||
<acceptance_criteria>
|
||||
- src/gps_denied/core/coordinates.py contains `import numpy as np`
|
||||
- src/gps_denied/core/coordinates.py contains `import cv2`
|
||||
- src/gps_denied/core/coordinates.py contains `def _build_intrinsic_matrix(cam: CameraParameters) -> np.ndarray`
|
||||
- src/gps_denied/core/coordinates.py contains `def _cam_to_body_rotation() -> np.ndarray`
|
||||
- src/gps_denied/core/coordinates.py contains `def _quat_to_rotation_matrix(q: np.ndarray) -> np.ndarray`
|
||||
- src/gps_denied/core/coordinates.py contains `K_inv = np.linalg.inv(K)` in pixel_to_gps
|
||||
- src/gps_denied/core/coordinates.py contains `ray_cam = K_inv @ pixel_h` in pixel_to_gps
|
||||
- src/gps_denied/core/coordinates.py contains `R_cam_body @ ray_cam` in pixel_to_gps
|
||||
- src/gps_denied/core/coordinates.py contains `t = -altitude / ray_enu[2]` in pixel_to_gps (ray-ground intersection)
|
||||
- src/gps_denied/core/coordinates.py contains `cv2.perspectiveTransform` in transform_points
|
||||
- The string "FAKE Math" does NOT appear in the file
|
||||
- The string "0.1" scaling factor does NOT appear in pixel_to_gps
|
||||
- gps_to_enu and enu_to_gps methods are UNCHANGED from original
|
||||
- image_object_to_gps uses ADTI 20L V1 defaults: focal_length=16.0, sensor_width=23.2, resolution_width=5456
|
||||
</acceptance_criteria>
|
||||
<done>All FAKE Math stubs replaced. pixel_to_gps uses K^-1 unprojection, T_cam_body rotation, quaternion body-to-ENU, and ray-ground intersection. gps_to_pixel is the exact inverse. transform_points uses cv2.perspectiveTransform. image_object_to_gps uses real ADTI 20L V1 camera defaults.</done>
|
||||
</task>
|
||||
|
||||
</tasks>
|
||||
|
||||
<verification>
|
||||
1. `python -c "from gps_denied.core.coordinates import CoordinateTransformer, _build_intrinsic_matrix, _cam_to_body_rotation, _quat_to_rotation_matrix; print('Imports OK')"` — all new functions importable
|
||||
2. `ruff check src/gps_denied/core/coordinates.py` — no lint errors
|
||||
3. `grep -c "FAKE Math" src/gps_denied/core/coordinates.py` — returns 0 (all stubs removed)
|
||||
</verification>
|
||||
|
||||
<success_criteria>
|
||||
- pixel_to_gps implements real K^-1 unprojection with camera intrinsics
|
||||
- Camera-to-body rotation R_cam_body accounts for nadir-pointing mount
|
||||
- Body-to-ENU rotation uses quaternion parameter
|
||||
- Ray-ground intersection uses altitude for ground plane
|
||||
- Image center pixel projects to UAV nadir position (verified numerically)
|
||||
- gps_to_pixel is the exact inverse of pixel_to_gps
|
||||
- transform_points uses cv2.perspectiveTransform
|
||||
- No FAKE Math stubs remain
|
||||
</success_criteria>
|
||||
|
||||
<output>
|
||||
After completion, create `.planning/phases/01-eskf-core/01-02-SUMMARY.md`
|
||||
</output>
|
||||
@@ -0,0 +1,445 @@
|
||||
---
|
||||
phase: 01-eskf-core
|
||||
plan: 03
|
||||
type: execute
|
||||
wave: 2
|
||||
depends_on:
|
||||
- 01-01
|
||||
- 01-02
|
||||
files_modified:
|
||||
- tests/test_eskf.py
|
||||
- tests/test_coordinates.py
|
||||
autonomous: true
|
||||
requirements:
|
||||
- ESKF-01
|
||||
- ESKF-02
|
||||
- ESKF-03
|
||||
- ESKF-04
|
||||
- ESKF-05
|
||||
- ESKF-06
|
||||
|
||||
must_haves:
|
||||
truths:
|
||||
- "ESKF propagation test confirms covariance grows from IMU-only prediction"
|
||||
- "ESKF VO update test confirms position uncertainty decreases after update"
|
||||
- "ESKF satellite update test confirms position converges to satellite measurement"
|
||||
- "Confidence tier test covers all four tiers: HIGH, MEDIUM, LOW, FAILED"
|
||||
- "Coordinate chain test verifies image center projects to nadir at correct GPS"
|
||||
- "Coordinate chain round-trip test: pixel -> GPS -> pixel returns original pixel"
|
||||
- "All tests pass with pytest"
|
||||
artifacts:
|
||||
- path: "tests/test_eskf.py"
|
||||
provides: "Unit tests for ESKF core algorithm"
|
||||
min_lines: 150
|
||||
- path: "tests/test_coordinates.py"
|
||||
provides: "Updated coordinate tests with real projection math"
|
||||
min_lines: 80
|
||||
key_links:
|
||||
- from: "tests/test_eskf.py"
|
||||
to: "src/gps_denied/core/eskf.py"
|
||||
via: "import ESKF"
|
||||
pattern: "from gps_denied\\.core\\.eskf import ESKF"
|
||||
- from: "tests/test_coordinates.py"
|
||||
to: "src/gps_denied/core/coordinates.py"
|
||||
via: "import CoordinateTransformer"
|
||||
pattern: "from gps_denied\\.core\\.coordinates import"
|
||||
---
|
||||
|
||||
<objective>
|
||||
Create comprehensive unit tests for the ESKF algorithm and update existing coordinate tests to verify the real projection math, covering all six ESKF requirements.
|
||||
|
||||
Purpose: Verify correctness of all ESKF operations (propagation, VO update, satellite update, initialization, confidence tiers) and the full coordinate chain (pixel -> camera ray -> body -> ENU -> WGS84) against known geometry.
|
||||
|
||||
Output: `tests/test_eskf.py` (new) and updated `tests/test_coordinates.py`.
|
||||
</objective>
|
||||
|
||||
<execution_context>
|
||||
@$HOME/.claude/get-shit-done/workflows/execute-plan.md
|
||||
@$HOME/.claude/get-shit-done/templates/summary.md
|
||||
</execution_context>
|
||||
|
||||
<context>
|
||||
@.planning/PROJECT.md
|
||||
@.planning/ROADMAP.md
|
||||
@.planning/STATE.md
|
||||
@.planning/codebase/TESTING.md
|
||||
@.planning/phases/01-eskf-core/01-01-SUMMARY.md
|
||||
@.planning/phases/01-eskf-core/01-02-SUMMARY.md
|
||||
|
||||
<interfaces>
|
||||
<!-- From Plan 01 -->
|
||||
From src/gps_denied/core/eskf.py:
|
||||
```python
|
||||
class ESKF:
|
||||
def __init__(self, config: ESKFConfig | None = None): ...
|
||||
def initialize(self, position_ned: np.ndarray, timestamp: float, velocity=None, quaternion=None): ...
|
||||
def initialize_from_gps(self, gps: GPSPoint, altitude: float, timestamp: float, coord_transformer, flight_id: str): ...
|
||||
def predict(self, imu: IMUMeasurement) -> None: ...
|
||||
def update_vo(self, relative_position: np.ndarray, dt_vo: float) -> np.ndarray: ...
|
||||
def update_satellite(self, position_enu: np.ndarray, noise_meters: float) -> np.ndarray: ...
|
||||
def get_confidence(self, consecutive_failures: int = 0) -> ConfidenceTier: ...
|
||||
def get_state(self) -> ESKFState: ...
|
||||
@property
|
||||
def initialized(self) -> bool: ...
|
||||
```
|
||||
|
||||
From src/gps_denied/schemas/eskf.py:
|
||||
```python
|
||||
class ConfidenceTier(str, Enum): HIGH, MEDIUM, LOW, FAILED
|
||||
class IMUMeasurement(BaseModel): accel (3,), gyro (3,), timestamp float
|
||||
class ESKFConfig(BaseModel): accel_noise_density, gyro_noise_density, ..., satellite_max_age=30.0, covariance_high_threshold=400.0
|
||||
class ESKFState(BaseModel): position (3,), velocity (3,), quaternion (4,), accel_bias (3,), gyro_bias (3,), covariance (15,15), timestamp, confidence
|
||||
```
|
||||
|
||||
<!-- From Plan 02 -->
|
||||
From src/gps_denied/core/coordinates.py:
|
||||
```python
|
||||
def pixel_to_gps(self, flight_id, pixel, frame_pose, camera_params, altitude, quaternion=None) -> GPSPoint
|
||||
def gps_to_pixel(self, flight_id, gps, frame_pose, camera_params, altitude, quaternion=None) -> tuple[float, float]
|
||||
def _build_intrinsic_matrix(cam: CameraParameters) -> np.ndarray
|
||||
def _cam_to_body_rotation() -> np.ndarray
|
||||
def _quat_to_rotation_matrix(q: np.ndarray) -> np.ndarray
|
||||
```
|
||||
</interfaces>
|
||||
</context>
|
||||
|
||||
<tasks>
|
||||
|
||||
<task type="auto">
|
||||
<name>Task 1: Create ESKF unit tests</name>
|
||||
<files>tests/test_eskf.py</files>
|
||||
<read_first>
|
||||
- src/gps_denied/core/eskf.py (the implementation being tested — read ALL methods, understand state layout, error-state correction)
|
||||
- src/gps_denied/schemas/eskf.py (ESKFConfig defaults, ConfidenceTier enum values, IMUMeasurement fields)
|
||||
- tests/test_coordinates.py (test pattern in this project: pytest fixtures, assertion style, approx usage)
|
||||
- tests/test_vo.py (another test file for reference on numpy assertion patterns)
|
||||
</read_first>
|
||||
<action>
|
||||
Create `tests/test_eskf.py` with the following test functions. Follow project conventions: module docstring, pytest fixtures, no class-based tests (only test_coordinates uses functions).
|
||||
|
||||
Module docstring: `"""Tests for ESKF (F17) — Error-State Kalman Filter."""`
|
||||
|
||||
Imports:
|
||||
```python
|
||||
import numpy as np
|
||||
import pytest
|
||||
from gps_denied.core.eskf import ESKF
|
||||
from gps_denied.schemas.eskf import ConfidenceTier, ESKFConfig, IMUMeasurement
|
||||
from gps_denied.schemas import GPSPoint
|
||||
from gps_denied.core.coordinates import CoordinateTransformer
|
||||
```
|
||||
|
||||
Fixtures:
|
||||
```python
|
||||
@pytest.fixture
|
||||
def eskf():
|
||||
"""ESKF initialized at origin with default config."""
|
||||
e = ESKF()
|
||||
e.initialize(np.zeros(3), timestamp=0.0)
|
||||
return e
|
||||
|
||||
@pytest.fixture
|
||||
def config():
|
||||
return ESKFConfig()
|
||||
```
|
||||
|
||||
**Test: test_initialization_default (ESKF-04)**
|
||||
- Create ESKF(), call initialize(np.array([100, 200, 0]), 0.0)
|
||||
- Assert eskf.initialized is True
|
||||
- state = eskf.get_state()
|
||||
- Assert state.position is close to [100, 200, 0]
|
||||
- Assert state.velocity is close to [0, 0, 0]
|
||||
- Assert state.quaternion is close to [1, 0, 0, 0]
|
||||
- Assert state.covariance.shape == (15, 15)
|
||||
- Assert state.covariance[0, 0] == config.init_pos_var (100.0)
|
||||
- Assert state.covariance[3, 3] == config.init_vel_var (1.0)
|
||||
- Assert state.covariance[6, 6] == config.init_att_var (0.01)
|
||||
|
||||
**Test: test_initialization_from_gps (ESKF-04)**
|
||||
- Create CoordinateTransformer, set origin GPSPoint(48.0, 37.0)
|
||||
- Create ESKF(), call initialize_from_gps(GPSPoint(48.001, 37.001), altitude=600, timestamp=0.0, coord_transformer=ct, flight_id="f1")
|
||||
- Assert eskf.initialized is True
|
||||
- state = eskf.get_state()
|
||||
- Assert state.position is NOT all zeros (GPS offset was applied)
|
||||
- Assert np.linalg.norm(state.position[:2]) > 50 (0.001 deg lat ~= 111m)
|
||||
|
||||
**Test: test_predict_covariance_grows (ESKF-01)**
|
||||
- Use eskf fixture
|
||||
- Record P_before = eskf.get_state().covariance.copy()
|
||||
- trace_before = np.trace(P_before[0:3, 0:3])
|
||||
- Predict with IMU: accel=[0, 0, 9.81], gyro=[0, 0, 0], timestamp=0.01
|
||||
- P_after = eskf.get_state().covariance
|
||||
- trace_after = np.trace(P_after[0:3, 0:3])
|
||||
- Assert trace_after > trace_before (covariance grew)
|
||||
|
||||
**Test: test_predict_gravity_compensation (ESKF-01)**
|
||||
- Use eskf fixture (identity quaternion = level flight in ENU)
|
||||
- Predict with 100 steps: accel=[0, 0, 9.81] (gravity in body down, but body aligned with ENU so accel_z = +g), gyro=[0,0,0], dt=0.01
|
||||
- After 1 second of stationary IMU with gravity, velocity should be near zero (gravity compensated by the g vector subtraction)
|
||||
- state = eskf.get_state()
|
||||
- Assert np.linalg.norm(state.velocity) < 1.0 (small drift from numerical integration, but not 9.81*1=9.81)
|
||||
|
||||
**Test: test_predict_with_acceleration (ESKF-01)**
|
||||
- Use eskf fixture
|
||||
- Apply 100 IMU predictions with accel=[1.0, 0, 9.81] (1 m/s^2 forward + gravity), gyro=[0,0,0], dt=0.01
|
||||
- After 1 second, velocity[0] should be approximately 1.0 m/s (from the extra 1 m/s^2 acceleration)
|
||||
- state = eskf.get_state()
|
||||
- Assert abs(state.velocity[0] - 1.0) < 0.5 (approximate due to discrete integration)
|
||||
|
||||
**Test: test_predict_position_propagation (ESKF-01)**
|
||||
- Create ESKF, initialize with position=[0,0,0], velocity=[10, 0, 0]
|
||||
- Predict 100 steps with accel=[0, 0, 9.81], gyro=[0,0,0], dt=0.01
|
||||
- After 1 second at 10 m/s, position[0] should be approximately 10.0 meters
|
||||
- Assert abs(state.position[0] - 10.0) < 2.0
|
||||
|
||||
**Test: test_vo_update_reduces_uncertainty (ESKF-02)**
|
||||
- Use eskf fixture
|
||||
- Predict a few steps to grow covariance
|
||||
- Record trace_before = np.trace(eskf.get_state().covariance[0:3, 0:3])
|
||||
- Call update_vo(relative_position=np.array([0.0, 0.0, 0.0]), dt_vo=0.1)
|
||||
- trace_after = np.trace(eskf.get_state().covariance[0:3, 0:3])
|
||||
- Assert trace_after < trace_before
|
||||
|
||||
**Test: test_vo_update_corrects_position (ESKF-02)**
|
||||
- Use eskf fixture
|
||||
- Predict 10 steps with accel=[0, 0, 9.81], gyro=[0,0,0], dt=0.1 (1 second)
|
||||
- Record position_before = eskf.get_state().position.copy()
|
||||
- Call update_vo with relative_position that includes a 1m north offset beyond predicted
|
||||
- Record position_after = eskf.get_state().position
|
||||
- Assert position_after != position_before (position was corrected)
|
||||
|
||||
**Test: test_vo_update_returns_innovation (ESKF-02)**
|
||||
- Use eskf fixture
|
||||
- innovation = eskf.update_vo(np.array([1.0, 0.0, 0.0]), dt_vo=0.1)
|
||||
- Assert innovation.shape == (3,)
|
||||
- Assert np.linalg.norm(innovation) > 0.5 (non-trivial innovation since predicted relative position is near zero)
|
||||
|
||||
**Test: test_satellite_update_corrects_position (ESKF-03)**
|
||||
- Use eskf fixture
|
||||
- Predict several steps to drift from origin
|
||||
- Record pos_before = eskf.get_state().position.copy()
|
||||
- Call update_satellite(position_enu=np.array([50.0, 50.0, 0.0]), noise_meters=10.0)
|
||||
- pos_after = eskf.get_state().position
|
||||
- Assert position moved toward [50, 50, 0]: np.linalg.norm(pos_after - [50,50,0]) < np.linalg.norm(pos_before - [50,50,0])
|
||||
|
||||
**Test: test_satellite_update_tightens_covariance (ESKF-03)**
|
||||
- Use eskf fixture (high initial covariance)
|
||||
- trace_before = np.trace(eskf.get_state().covariance[0:3, 0:3])
|
||||
- Call update_satellite(np.zeros(3), noise_meters=5.0)
|
||||
- trace_after = np.trace(eskf.get_state().covariance[0:3, 0:3])
|
||||
- Assert trace_after < trace_before
|
||||
|
||||
**Test: test_satellite_update_covariance_bounded_by_noise (ESKF-03)**
|
||||
- Use eskf fixture
|
||||
- Call update_satellite(np.zeros(3), noise_meters=5.0)
|
||||
- pos_cov_trace = np.trace(eskf.get_state().covariance[0:3, 0:3])
|
||||
- Assert pos_cov_trace < 3 * 5.0**2 (covariance bounded by satellite noise)
|
||||
|
||||
**Test: test_confidence_high (ESKF-05)**
|
||||
- Use eskf fixture
|
||||
- Call update_satellite to set last_satellite_time
|
||||
- Advance timestamp by small dt (predict one step)
|
||||
- Assert eskf.get_confidence() == ConfidenceTier.HIGH
|
||||
|
||||
**Test: test_confidence_medium (ESKF-05)**
|
||||
- Create ESKF, initialize at time 0.0
|
||||
- Call update_vo to set last_vo_time (but never satellite)
|
||||
- Assert eskf.get_confidence() == ConfidenceTier.MEDIUM
|
||||
|
||||
**Test: test_confidence_low (ESKF-05)**
|
||||
- Create ESKF, initialize at time 0.0
|
||||
- Predict many steps without any measurement update (no VO, no satellite)
|
||||
- Eventually confidence should be LOW (no recent VO or satellite)
|
||||
- Assert eskf.get_confidence() == ConfidenceTier.LOW
|
||||
|
||||
**Test: test_confidence_failed (ESKF-05)**
|
||||
- Use eskf fixture
|
||||
- Assert eskf.get_confidence(consecutive_failures=3) == ConfidenceTier.FAILED
|
||||
- Assert eskf.get_confidence(consecutive_failures=0) != ConfidenceTier.FAILED
|
||||
|
||||
**Test: test_get_state_returns_eskf_state (ESKF-01)**
|
||||
- Use eskf fixture
|
||||
- state = eskf.get_state()
|
||||
- Assert isinstance(state, ESKFState)
|
||||
- Assert state.position.shape == (3,)
|
||||
- Assert state.velocity.shape == (3,)
|
||||
- Assert state.quaternion.shape == (4,)
|
||||
- Assert state.accel_bias.shape == (3,)
|
||||
- Assert state.gyro_bias.shape == (3,)
|
||||
- Assert state.covariance.shape == (15, 15)
|
||||
|
||||
**Test: test_full_fusion_sequence (integration)**
|
||||
- Create ESKF, initialize at origin
|
||||
- Run 10 IMU predictions (simulate 0.5s at 20Hz)
|
||||
- Assert covariance grew
|
||||
- Run VO update with small correction
|
||||
- Assert covariance decreased
|
||||
- Run 10 more IMU predictions
|
||||
- Run satellite update with known position
|
||||
- Assert position is close to satellite position (within satellite noise)
|
||||
- Assert confidence is HIGH
|
||||
</action>
|
||||
<verify>
|
||||
<automated>cd /home/yuzviak/Azaion/gps-denied-onboard && python -m pytest tests/test_eskf.py -v --tb=short 2>&1 | tail -40</automated>
|
||||
</verify>
|
||||
<acceptance_criteria>
|
||||
- tests/test_eskf.py contains at least 15 test functions (test_initialization_default, test_initialization_from_gps, test_predict_covariance_grows, test_predict_gravity_compensation, test_predict_with_acceleration, test_predict_position_propagation, test_vo_update_reduces_uncertainty, test_vo_update_corrects_position, test_vo_update_returns_innovation, test_satellite_update_corrects_position, test_satellite_update_tightens_covariance, test_satellite_update_covariance_bounded_by_noise, test_confidence_high, test_confidence_medium, test_confidence_low, test_confidence_failed, test_get_state_returns_eskf_state, test_full_fusion_sequence)
|
||||
- tests/test_eskf.py contains `from gps_denied.core.eskf import ESKF`
|
||||
- tests/test_eskf.py contains `from gps_denied.schemas.eskf import ConfidenceTier, ESKFConfig, IMUMeasurement`
|
||||
- Every test function contains at least one `assert` statement
|
||||
- `pytest tests/test_eskf.py` exits 0 (all tests pass)
|
||||
</acceptance_criteria>
|
||||
<done>All 18 ESKF tests pass, covering: initialization (ESKF-04), IMU prediction with covariance growth and gravity compensation (ESKF-01), VO update with uncertainty reduction (ESKF-02), satellite update with absolute correction (ESKF-03), all four confidence tiers (ESKF-05), and full fusion integration</done>
|
||||
</task>
|
||||
|
||||
<task type="auto">
|
||||
<name>Task 2: Update coordinate chain tests for real projection math</name>
|
||||
<files>tests/test_coordinates.py</files>
|
||||
<read_first>
|
||||
- tests/test_coordinates.py (current tests — understand what exists, what must be preserved vs updated)
|
||||
- src/gps_denied/core/coordinates.py (the updated implementation with real K matrix, ray-ground intersection, perspectiveTransform)
|
||||
- src/gps_denied/schemas/__init__.py (CameraParameters fields)
|
||||
</read_first>
|
||||
<action>
|
||||
Rewrite `tests/test_coordinates.py` to test the real coordinate chain. Keep the existing tests for gps_to_enu and enu_to_gps UNCHANGED (they still pass since those methods were not modified). Update pixel_to_gps tests and add new ones for the real math.
|
||||
|
||||
Module docstring: `"""Tests for CoordinateTransformer (F13) — real coordinate chain."""`
|
||||
|
||||
Keep existing imports, add:
|
||||
```python
|
||||
import numpy as np
|
||||
```
|
||||
|
||||
Keep existing fixtures unchanged.
|
||||
|
||||
**Keep test_enu_origin_management** — unchanged.
|
||||
|
||||
**Keep test_gps_to_enu** — unchanged.
|
||||
|
||||
**Keep test_enu_roundtrip** — unchanged.
|
||||
|
||||
**Replace test_pixel_to_gps_flow** with these new tests:
|
||||
|
||||
**Test: test_pixel_to_gps_center_projects_to_nadir**
|
||||
- Set origin GPSPoint(48.0, 37.0)
|
||||
- ADTI 20L V1 camera: focal_length=16.0, sensor_width=23.2, sensor_height=15.4, resolution_width=5456, resolution_height=3632
|
||||
- pixel = (2728.0, 1816.0) (image center = principal point)
|
||||
- pose = {"position": [0, 0, 0]}
|
||||
- quaternion = np.array([1.0, 0.0, 0.0, 0.0]) (identity = level, north-facing)
|
||||
- altitude = 600.0
|
||||
- gps = transformer.pixel_to_gps("flight_123", pixel, pose, cam, altitude, quaternion)
|
||||
- Assert abs(gps.lat - 48.0) < 1e-6 (center pixel projects exactly below UAV)
|
||||
- Assert abs(gps.lon - 37.0) < 1e-6
|
||||
|
||||
**Test: test_pixel_to_gps_off_center**
|
||||
- Same setup as above, but pixel = (2728 + 100, 1816.0) (100 pixels to the right)
|
||||
- gps = transformer.pixel_to_gps(...)
|
||||
- The point should be offset east from origin (right in camera = east when north-facing with nadir camera)
|
||||
- offset_enu = transformer.gps_to_enu("flight_123", gps)
|
||||
- Assert offset_enu[0] > 0 (east offset from camera right)
|
||||
- Verify ground offset is approximately: 100 pixels * (altitude / fx) where fx = 16*5456/23.2 = 3763.45
|
||||
- Expected east offset ~= 100 * 600 / 3763.45 ~= 15.9 meters
|
||||
- Assert abs(offset_enu[0] - 15.9) < 1.0
|
||||
|
||||
**Test: test_pixel_to_gps_roundtrip**
|
||||
- Set origin, create camera, pose, identity quaternion, altitude=600
|
||||
- For several test pixels: (1000, 1000), (3000, 2000), (4000, 500)
|
||||
- gps = pixel_to_gps(... pixel, ... quaternion)
|
||||
- pixel_back = gps_to_pixel(... gps, ... quaternion)
|
||||
- Assert abs(pixel_back[0] - pixel[0]) < 0.1
|
||||
- Assert abs(pixel_back[1] - pixel[1]) < 0.1
|
||||
|
||||
**Test: test_pixel_to_gps_altitude_scaling**
|
||||
- Same pixel (off-center), two altitudes: 300m and 600m
|
||||
- The ground offset at 600m should be approximately 2x the offset at 300m
|
||||
- gps_300 = pixel_to_gps(... altitude=300)
|
||||
- gps_600 = pixel_to_gps(... altitude=600)
|
||||
- enu_300 = gps_to_enu(gps_300)
|
||||
- enu_600 = gps_to_enu(gps_600)
|
||||
- Assert abs(enu_600[0] / enu_300[0] - 2.0) < 0.1
|
||||
|
||||
**Test: test_pixel_to_gps_with_quaternion_rotation**
|
||||
- Use a quaternion that represents a 90-degree yaw (heading east instead of north)
|
||||
- What was "right" in camera (east when north-facing) becomes "south" when east-facing
|
||||
- Verify the GPS offset direction changes accordingly
|
||||
- Use quaternion for 90deg yaw around Z (down in NED, up in ENU):
|
||||
- q = [cos(pi/4), 0, 0, sin(pi/4)] for yaw rotation
|
||||
- pixel to the right of center should now project south, not east
|
||||
|
||||
**Test: test_transform_points_homography**
|
||||
- points = [(0, 0), (100, 0), (100, 100), (0, 100)]
|
||||
- H = identity [[1,0,0],[0,1,0],[0,0,1]]
|
||||
- result = transformer.transform_points(points, H)
|
||||
- Assert result == points (identity preserves points)
|
||||
- H_translate = [[1,0,10],[0,1,20],[0,0,1]]
|
||||
- result = transformer.transform_points(points, H_translate)
|
||||
- Assert result[0] is approximately (10, 20)
|
||||
|
||||
**Test: test_transform_points_empty**
|
||||
- result = transformer.transform_points([], [[1,0,0],[0,1,0],[0,0,1]])
|
||||
- Assert result == []
|
||||
|
||||
**Test: test_image_object_to_gps_uses_real_camera**
|
||||
- Set origin
|
||||
- Use image center pixel
|
||||
- result = transformer.image_object_to_gps("flight_123", 1, (2728.0, 1816.0))
|
||||
- Assert result is close to origin (nadir projection with default altitude=100)
|
||||
- Assert abs(result.lat - 48.0) < 0.01
|
||||
|
||||
**Test: test_build_intrinsic_matrix**
|
||||
- from gps_denied.core.coordinates import _build_intrinsic_matrix
|
||||
- cam = CameraParameters(focal_length=16.0, sensor_width=23.2, sensor_height=15.4, resolution_width=5456, resolution_height=3632)
|
||||
- K = _build_intrinsic_matrix(cam)
|
||||
- Assert K.shape == (3, 3)
|
||||
- fx = 16.0 * 5456 / 23.2 # = 3763.45
|
||||
- Assert abs(K[0, 0] - fx) < 1.0
|
||||
- fy = 16.0 * 3632 / 15.4 # = 3773.51
|
||||
- Assert abs(K[1, 1] - fy) < 1.0
|
||||
- Assert abs(K[0, 2] - 2728.0) < 1.0 # cx = width/2
|
||||
- Assert abs(K[1, 2] - 1816.0) < 1.0 # cy = height/2
|
||||
|
||||
Per ESKF-06: verifying the full coordinate chain pixel -> camera ray -> body -> ENU -> WGS84.
|
||||
</action>
|
||||
<verify>
|
||||
<automated>cd /home/yuzviak/Azaion/gps-denied-onboard && python -m pytest tests/test_coordinates.py -v --tb=short 2>&1 | tail -30</automated>
|
||||
</verify>
|
||||
<acceptance_criteria>
|
||||
- tests/test_coordinates.py contains `import numpy as np`
|
||||
- tests/test_coordinates.py contains test_enu_origin_management (preserved from original)
|
||||
- tests/test_coordinates.py contains test_gps_to_enu (preserved from original)
|
||||
- tests/test_coordinates.py contains test_enu_roundtrip (preserved from original)
|
||||
- tests/test_coordinates.py contains test_pixel_to_gps_center_projects_to_nadir
|
||||
- tests/test_coordinates.py contains test_pixel_to_gps_off_center with assertion `abs(offset_enu[0] - 15.9) < 1.0` or similar ground truth check
|
||||
- tests/test_coordinates.py contains test_pixel_to_gps_roundtrip testing at least 3 pixel positions
|
||||
- tests/test_coordinates.py contains test_pixel_to_gps_altitude_scaling verifying 2x scaling
|
||||
- tests/test_coordinates.py contains test_transform_points_homography
|
||||
- tests/test_coordinates.py contains test_build_intrinsic_matrix verifying fx ~= 3763
|
||||
- `pytest tests/test_coordinates.py` exits 0 (all tests pass)
|
||||
</acceptance_criteria>
|
||||
<done>Coordinate tests verify: center pixel projects to nadir (ESKF-06 chain), off-center pixel offset matches expected ground distance, pixel-to-GPS-to-pixel roundtrip within 0.1px, altitude scaling is linear, quaternion rotation changes projection direction, homography transform works via cv2, K matrix values match ADTI 20L V1 spec</done>
|
||||
</task>
|
||||
|
||||
</tasks>
|
||||
|
||||
<verification>
|
||||
1. `cd /home/yuzviak/Azaion/gps-denied-onboard && python -m pytest tests/test_eskf.py tests/test_coordinates.py -v --tb=short` — all tests pass
|
||||
2. `cd /home/yuzviak/Azaion/gps-denied-onboard && python -m pytest tests/ -x --tb=short` — existing tests also still pass (no regressions)
|
||||
3. Test count: `pytest tests/test_eskf.py --collect-only | grep "test session starts" -A 1` — at least 15 tests
|
||||
4. Test count: `pytest tests/test_coordinates.py --collect-only | grep "test session starts" -A 1` — at least 10 tests
|
||||
</verification>
|
||||
|
||||
<success_criteria>
|
||||
- All ESKF unit tests pass: initialization, prediction, VO update, satellite update, confidence tiers, state retrieval, full fusion sequence
|
||||
- All coordinate chain tests pass: center pixel nadir, off-center with ground truth, roundtrip, altitude scaling, quaternion rotation, homography, K matrix
|
||||
- Existing tests in tests/ directory are not broken (no regressions)
|
||||
- Phase 1 success criteria are met:
|
||||
1. ESKF propagates and covariance grows (test_predict_covariance_grows)
|
||||
2. VO update reduces uncertainty (test_vo_update_reduces_uncertainty)
|
||||
3. Satellite update corrects position (test_satellite_update_corrects_position)
|
||||
4. Confidence tiers work (test_confidence_high/medium/low/failed)
|
||||
5. Full coordinate chain works (test_pixel_to_gps_center_projects_to_nadir, test_pixel_to_gps_roundtrip)
|
||||
</success_criteria>
|
||||
|
||||
<output>
|
||||
After completion, create `.planning/phases/01-eskf-core/01-03-SUMMARY.md`
|
||||
</output>
|
||||
Reference in New Issue
Block a user