gps-denied-onboard/.planning/phases/01-eskf-core/01-03-PLAN.md

phase, plan, type, wave, depends_on, files_modified, autonomous, requirements, must_haves

phase	plan	type	wave	depends_on	files_modified	autonomous	requirements	must_haves
01-eskf-core	03	execute	2	01-01 01-02	tests/test_eskf.py tests/test_coordinates.py	true	ESKF-01 ESKF-02 ESKF-03 ESKF-04 ESKF-05 ESKF-06	truths artifacts key_links 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 path provides min_lines tests/test_eskf.py Unit tests for ESKF core algorithm 150 path provides min_lines tests/test_coordinates.py Updated coordinate tests with real projection math 80 from to via pattern tests/test_eskf.py src/gps_denied/core/eskf.py import ESKF from gps_denied.core.eskf import ESKF from to via pattern tests/test_coordinates.py src/gps_denied/core/coordinates.py import CoordinateTransformer from gps_denied.core.coordinates import

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.

<execution_context> @$HOME/.claude/get-shit-done/workflows/execute-plan.md @$HOME/.claude/get-shit-done/templates/summary.md </execution_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 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:

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 src/gps_denied/core/coordinates.py:

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

Task 1: Create ESKF unit tests tests/test_eskf.py - 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) 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:

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:

@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 cd /home/yuzviak/Azaion/gps-denied-onboard && python -m pytest tests/test_eskf.py -v --tb=short 2>&1 | tail -40 <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> 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

Task 2: Update coordinate chain tests for real projection math tests/test_coordinates.py - 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) 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:

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. cd /home/yuzviak/Azaion/gps-denied-onboard && python -m pytest tests/test_coordinates.py -v --tb=short 2>&1 | tail -30 <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> 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

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

<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>

After completion, create `.planning/phases/01-eskf-core/01-03-SUMMARY.md`