azaion/gps-denied-onboard
1
0
Fork 0
mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-04-23 03:46:37 +00:00
Code Issues Packages Projects Releases Wiki Activity

feat(tests): add comprehensive ESKF + coordinate chain tests (ESKF-01..06)

Browse Source 
test_eskf.py: 18 tests covering initialization, IMU prediction, VO/satellite
updates, confidence tiers, and full fusion integration.
test_coordinates.py: 17 new tests for K matrix, ray-ground intersection,
pixel-GPS roundtrips, and cv2.perspectiveTransform homography.

All 35 tests pass.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
Yuzviak
2026-04-01 23:52:58 +03:00
parent dccadd4bf4
commit 2e5436a6c7
2 changed files with 638 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tests/test_coordinates.py
+317 -12
View File
@@ -1,8 +1,15 @@
"""Tests for CoordinateTransformer (F13)."""
"""Tests for CoordinateTransformer (F13) — Real coordinate chain (ESKF-06)."""
import numpy as np
import pytest
from gps_denied.core.coordinates import CoordinateTransformer, OriginNotSetError
from gps_denied.core.coordinates import (
CoordinateTransformer,
OriginNotSetError,
_build_intrinsic_matrix,
_cam_to_body_rotation,
_quat_to_rotation_matrix,
)
from gps_denied.schemas import CameraParameters, GPSPoint
@@ -67,18 +74,316 @@ def test_pixel_to_gps_flow(transformer):
resolution_height=3000,
)
# Image center should yield the frame center (mock implementation logic)
pixel = (2000.0, 1500.0)
# Image center should yield approximately the frame center
cx = cam.resolution_width / 2.0
cy = cam.resolution_height / 2.0
pixel = (cx, cy)
pose = {"position": [0, 0, 0]}
q_identity = np.array([1.0, 0.0, 0.0, 0.0])
gps_res = transformer.pixel_to_gps(fid, pixel, pose, cam, 100.0)
assert gps_res.lat == origin.lat
assert gps_res.lon == origin.lon
gps_res = transformer.pixel_to_gps(fid, pixel, pose, cam, 100.0, quaternion=q_identity)
assert pytest.approx(gps_res.lat, abs=1e-4) == origin.lat
assert pytest.approx(gps_res.lon, abs=1e-4) == origin.lon
# Inverse must match pixel (mock implementations match)
pix_res = transformer.gps_to_pixel(fid, gps_res, pose, cam, 100.0)
assert pix_res == pixel
# Inverse must match pixel
pix_res = transformer.gps_to_pixel(fid, gps_res, pose, cam, 100.0, quaternion=q_identity)
assert abs(pix_res[0] - pixel[0]) < 1.0
assert abs(pix_res[1] - pixel[1]) < 1.0
# And image_object_to_gps should work
obj_gps = transformer.image_object_to_gps(fid, 1, pixel)
assert obj_gps.lat == origin.lat
obj_gps = transformer.image_object_to_gps(
fid, 1, pixel, frame_pose=pose, camera_params=cam, altitude=100.0, quaternion=q_identity
)
assert pytest.approx(obj_gps.lat, abs=1e-4) == origin.lat
# ============================================================================
# ESKF-06: Real Coordinate Chain Tests
# ============================================================================
class TestIntrinsicMatrix:
"""Tests for camera intrinsic matrix construction."""
def test_build_intrinsic_matrix_adti_20l_v1(self):
"""Test K matrix for ADTI 20L V1 camera with 16mm lens."""
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)
# Expected focal lengths
fx_expected = 16.0 * 5456 / 23.2
fy_expected = 16.0 * 3632 / 15.4
assert K.shape == (3, 3)
assert abs(K[0, 0] - fx_expected) < 1.0 # fx
assert abs(K[1, 1] - fy_expected) < 1.0 # fy
assert abs(K[0, 2] - 2728.0) < 1.0 # cx (center)
assert abs(K[1, 2] - 1816.0) < 1.0 # cy (center)
assert K[2, 2] == 1.0
def test_build_intrinsic_matrix_custom_principal_point(self):
"""Test K matrix with custom principal point."""
cam = CameraParameters(
focal_length=16.0,
sensor_width=23.2,
sensor_height=15.4,
resolution_width=5456,
resolution_height=3632,
principal_point=(2700.0, 1800.0),
)
K = _build_intrinsic_matrix(cam)
assert abs(K[0, 2] - 2700.0) < 0.1 # cx
assert abs(K[1, 2] - 1800.0) < 0.1 # cy
class TestCameraToBodyRotation:
"""Tests for camera-to-body rotation matrix."""
def test_cam_to_body_is_180_x_rotation(self):
"""Test that camera-to-body rotation is Rx(180deg)."""
R_cam_body = _cam_to_body_rotation()
# Should flip Y and Z axes, keep X
assert R_cam_body.shape == (3, 3)
assert R_cam_body[0, 0] == 1.0
assert R_cam_body[1, 1] == -1.0
assert R_cam_body[2, 2] == -1.0
assert np.allclose(np.linalg.det(R_cam_body), 1.0) # det = 1 (proper rotation)
class TestQuaternionRotation:
"""Tests for quaternion to rotation matrix conversion."""
def test_identity_quaternion(self):
"""Test identity quaternion [1, 0, 0, 0] produces identity matrix."""
q = np.array([1.0, 0.0, 0.0, 0.0])
R = _quat_to_rotation_matrix(q)
assert np.allclose(R, np.eye(3))
def test_90_degree_z_rotation(self):
"""Test 90-degree rotation around Z axis."""
# q = [cos(45deg), 0, 0, sin(45deg)] for 90-degree rotation around Z
angle = np.pi / 4 # 45 degrees
q = np.array([np.cos(angle), 0.0, 0.0, np.sin(angle)])
R = _quat_to_rotation_matrix(q)
# Should rotate [1, 0, 0] to approximately [0, 1, 0]
x_axis = np.array([1.0, 0.0, 0.0])
rotated = R @ x_axis
expected = np.array([0.0, 1.0, 0.0])
assert np.allclose(rotated, expected, atol=0.01)
class TestPixelToGPSChain:
"""Tests for full pixel-to-GPS projection chain (ESKF-06)."""
def test_image_center_nadir_projection(self):
"""Test that image center pixel projects to UAV nadir position."""
transformer = CoordinateTransformer()
origin = GPSPoint(lat=48.0, lon=37.0)
transformer.set_enu_origin("f1", origin)
cam = CameraParameters(
focal_length=16.0,
sensor_width=23.2,
sensor_height=15.4,
resolution_width=5456,
resolution_height=3632,
)
# Center pixel with identity quaternion (level flight)
pixel = (2728.0, 1816.0)
pose = {"position": [0.0, 0.0, 0.0]}
q_identity = np.array([1.0, 0.0, 0.0, 0.0])
gps = transformer.pixel_to_gps(
"f1", pixel, pose, cam, altitude=600.0, quaternion=q_identity
)
# Center pixel should project to UAV nadir (origin)
assert abs(gps.lat - 48.0) < 0.001
assert abs(gps.lon - 37.0) < 0.001
def test_pixel_offset_produces_ground_offset(self):
"""Test that off-center pixel offsets project to corresponding ground distance."""
transformer = CoordinateTransformer()
origin = GPSPoint(lat=48.0, lon=37.0)
transformer.set_enu_origin("f1", origin)
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.0, 0.0]}
q_identity = np.array([1.0, 0.0, 0.0, 0.0])
altitude = 600.0
# Center pixel
center_pixel = (2728.0, 1816.0)
center_gps = transformer.pixel_to_gps(
"f1", center_pixel, pose, cam, altitude=altitude, quaternion=q_identity
)
# Offset pixel (100 pixels to the right)
offset_pixel = (2828.0, 1816.0)
offset_gps = transformer.pixel_to_gps(
"f1", offset_pixel, pose, cam, altitude=altitude, quaternion=q_identity
)
# Longitude difference should be non-zero (right = east = longer)
assert offset_gps.lon != center_gps.lon
def test_altitude_scaling_is_linear(self):
"""Test that doubling altitude doubles ground distance from nadir."""
transformer = CoordinateTransformer()
origin = GPSPoint(lat=48.0, lon=37.0)
transformer.set_enu_origin("f1", origin)
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.0, 0.0]}
q_identity = np.array([1.0, 0.0, 0.0, 0.0])
pixel = (2728.0 + 100, 1816.0) # Offset pixel
# Project at altitude 300m
gps_300 = transformer.pixel_to_gps(
"f1", pixel, pose, cam, altitude=300.0, quaternion=q_identity
)
# Project at altitude 600m
gps_600 = transformer.pixel_to_gps(
"f1", pixel, pose, cam, altitude=600.0, quaternion=q_identity
)
# Distance from origin should scale with altitude (roughly)
# At 600m, ground distance should be ~2x that at 300m
dist_300 = abs(gps_300.lon - 37.0)
dist_600 = abs(gps_600.lon - 37.0)
ratio = dist_600 / dist_300 if dist_300 > 0 else 1.0
assert 1.8 < ratio < 2.2 # Allow some numerical tolerance
class TestGPSToPixelInverse:
"""Tests for GPS-to-pixel projection (inverse of pixel-to-GPS)."""
def test_gps_to_pixel_is_inverse(self):
"""Test that gps_to_pixel is the exact inverse of pixel_to_gps."""
transformer = CoordinateTransformer()
origin = GPSPoint(lat=48.0, lon=37.0)
transformer.set_enu_origin("f1", origin)
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.0, 0.0]}
q_identity = np.array([1.0, 0.0, 0.0, 0.0])
original_pixel = (2728.0 + 50, 1816.0 - 30)
# pixel -> GPS
gps = transformer.pixel_to_gps(
"f1", original_pixel, pose, cam, altitude=600.0, quaternion=q_identity
)
# GPS -> pixel
recovered_pixel = transformer.gps_to_pixel(
"f1", gps, pose, cam, altitude=600.0, quaternion=q_identity
)
# Should recover original pixel
assert abs(recovered_pixel[0] - original_pixel[0]) < 1.0
assert abs(recovered_pixel[1] - original_pixel[1]) < 1.0
def test_roundtrip_with_quaternion(self):
"""Test pixel-GPS-pixel roundtrip with non-identity quaternion."""
transformer = CoordinateTransformer()
origin = GPSPoint(lat=48.0, lon=37.0)
transformer.set_enu_origin("f1", origin)
cam = CameraParameters(
focal_length=16.0,
sensor_width=23.2,
sensor_height=15.4,
resolution_width=5456,
resolution_height=3632,
)
pose = {"position": [100.0, 50.0, 0.0]}
# Small 30-degree rotation around Z
angle = np.radians(30)
q = np.array([np.cos(angle / 2), 0, 0, np.sin(angle / 2)])
original_pixel = (2728.0, 1816.0) # Center
gps = transformer.pixel_to_gps(
"f1", original_pixel, pose, cam, altitude=600.0, quaternion=q
)
recovered_pixel = transformer.gps_to_pixel(
"f1", gps, pose, cam, altitude=600.0, quaternion=q
)
# Center pixel should still recover to center
assert abs(recovered_pixel[0] - 2728.0) < 10.0
assert abs(recovered_pixel[1] - 1816.0) < 10.0
class TestTransformPoints:
"""Tests for homography transform via cv2.perspectiveTransform."""
def test_transform_points_identity(self):
"""Test homography transform with identity matrix."""
transformer = CoordinateTransformer()
H = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] # Identity
points = [(10.0, 20.0), (30.0, 40.0)]
result = transformer.transform_points(points, H)
assert len(result) == 2
assert abs(result[0][0] - 10.0) < 0.1
assert abs(result[0][1] - 20.0) < 0.1
assert abs(result[1][0] - 30.0) < 0.1
assert abs(result[1][1] - 40.0) < 0.1
def test_transform_points_translation(self):
"""Test homography with translation."""
transformer = CoordinateTransformer()
# Translate by (100, 50)
H = [[1, 0, 100], [0, 1, 50], [0, 0, 1]]
points = [(0.0, 0.0)]
result = transformer.transform_points(points, H)
assert len(result) == 1
assert abs(result[0][0] - 100.0) < 0.1
assert abs(result[0][1] - 50.0) < 0.1
def test_transform_points_empty(self):
"""Test transform_points with empty point list."""
transformer = CoordinateTransformer()
H = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
result = transformer.transform_points([], H)
assert result == []
tests/test_eskf.py
+317
View File
@@ -0,0 +1,317 @@
"""Tests for ESKF (F17) — Error-State Kalman Filter."""
import numpy as np
import pytest
from gps_denied.core.coordinates import CoordinateTransformer
from gps_denied.core.eskf import ESKF
from gps_denied.schemas import GPSPoint
from gps_denied.schemas.eskf import ConfidenceTier, ESKFConfig, IMUMeasurement
@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()
class TestESKFInitialization:
"""Tests for ESKF-04: Initialization."""
def test_initialization_default(self, config):
"""Test basic initialization with position and timestamp."""
e = ESKF()
e.initialize(np.array([100.0, 200.0, 0.0]), timestamp=0.0)
assert e.initialized
state = e.get_state()
assert np.allclose(state.position, [100.0, 200.0, 0.0])
assert np.allclose(state.velocity, [0.0, 0.0, 0.0])
assert np.allclose(state.quaternion, [1.0, 0.0, 0.0, 0.0])
assert state.covariance.shape == (15, 15)
assert state.covariance[0, 0] == config.init_pos_var
assert state.covariance[3, 3] == config.init_vel_var
assert state.covariance[6, 6] == config.init_att_var
def test_initialization_from_gps(self):
"""Test initialization from GPS position."""
ct = CoordinateTransformer()
ct.set_enu_origin("f1", GPSPoint(lat=48.0, lon=37.0))
e = ESKF()
e.initialize_from_gps(
GPSPoint(lat=48.001, lon=37.001),
altitude=600.0,
timestamp=0.0,
coord_transformer=ct,
flight_id="f1",
)
assert e.initialized
state = e.get_state()
assert not np.allclose(state.position, [0.0, 0.0, 0.0])
assert np.linalg.norm(state.position[:2]) > 50 # ~111m per 0.001 deg
class TestESKFPrediction:
"""Tests for ESKF-01: IMU prediction."""
def test_predict_covariance_grows(self, eskf):
"""Test that covariance grows during IMU-only prediction."""
P_before = eskf.get_state().covariance.copy()
trace_before = np.trace(P_before[0:3, 0:3])
imu = IMUMeasurement(
accel=np.array([0.0, 0.0, 9.81]),
gyro=np.zeros(3),
timestamp=0.01,
)
eskf.predict(imu)
P_after = eskf.get_state().covariance
trace_after = np.trace(P_after[0:3, 0:3])
assert trace_after > trace_before
def test_predict_gravity_compensation(self, eskf):
"""Test that gravity is properly compensated (stationary accel has no velocity growth)."""
# 100 steps at 0.01s = 1 second with gravity-aligned acceleration
for i in range(100):
imu = IMUMeasurement(
accel=np.array([0.0, 0.0, 9.81]),
gyro=np.zeros(3),
timestamp=0.01 * (i + 1),
)
eskf.predict(imu)
state = eskf.get_state()
# Velocity should remain near zero (gravity compensated)
assert np.linalg.norm(state.velocity) < 1.0
def test_predict_with_acceleration(self, eskf):
"""Test velocity integration from applied acceleration."""
# 100 steps at 0.01s = 1 second with 1 m/s^2 forward + gravity
for i in range(100):
imu = IMUMeasurement(
accel=np.array([1.0, 0.0, 9.81]),
gyro=np.zeros(3),
timestamp=0.01 * (i + 1),
)
eskf.predict(imu)
state = eskf.get_state()
# Velocity[0] should be approximately 1.0 m/s
assert abs(state.velocity[0] - 1.0) < 0.5
def test_predict_position_propagation(self):
"""Test position propagation from velocity."""
e = ESKF()
e.initialize(np.zeros(3), timestamp=0.0, velocity=np.array([10.0, 0.0, 0.0]))
for i in range(100):
imu = IMUMeasurement(
accel=np.array([0.0, 0.0, 9.81]),
gyro=np.zeros(3),
timestamp=0.01 * (i + 1),
)
e.predict(imu)
state = e.get_state()
# Position[0] should be approximately 10.0 meters after 1 second at 10 m/s
assert abs(state.position[0] - 10.0) < 2.0
class TestESKFVOUpdate:
"""Tests for ESKF-02: VO measurement update."""
def test_vo_update_reduces_uncertainty(self, eskf):
"""Test that VO update reduces position covariance."""
# Grow covariance first
for _ in range(5):
imu = IMUMeasurement(
accel=np.zeros(3),
gyro=np.zeros(3),
timestamp=eskf._last_timestamp + 0.01,
)
eskf.predict(imu)
trace_before = np.trace(eskf.get_state().covariance[0:3, 0:3])
eskf.update_vo(np.zeros(3), dt_vo=0.1)
trace_after = np.trace(eskf.get_state().covariance[0:3, 0:3])
assert trace_after < trace_before
def test_vo_update_corrects_position(self, eskf):
"""Test that VO update shifts position toward measurement."""
# Predict to drift
for i in range(10):
imu = IMUMeasurement(
accel=np.array([0.0, 0.0, 9.81]),
gyro=np.zeros(3),
timestamp=0.01 * (i + 1),
)
eskf.predict(imu)
pos_before = eskf.get_state().position.copy()
# VO measurement with 1m north offset
eskf.update_vo(np.array([0.0, 1.0, 0.0]), dt_vo=0.1)
pos_after = eskf.get_state().position
assert not np.allclose(pos_after, pos_before)
def test_vo_update_returns_innovation(self, eskf):
"""Test that VO update returns innovation vector."""
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
class TestESKFSatelliteUpdate:
"""Tests for ESKF-03: Satellite measurement update."""
def test_satellite_update_corrects_position(self, eskf):
"""Test that satellite update moves position toward measurement."""
# Predict to drift
for i in range(10):
imu = IMUMeasurement(
accel=np.array([0.0, 0.0, 9.81]),
gyro=np.zeros(3),
timestamp=0.01 * (i + 1),
)
eskf.predict(imu)
pos_before = eskf.get_state().position.copy()
target_pos = np.array([50.0, 50.0, 0.0])
eskf.update_satellite(target_pos, noise_meters=10.0)
pos_after = eskf.get_state().position
# Position should move toward target
dist_before = np.linalg.norm(pos_before - target_pos)
dist_after = np.linalg.norm(pos_after - target_pos)
assert dist_after < dist_before
def test_satellite_update_tightens_covariance(self, eskf):
"""Test that satellite update reduces position covariance."""
trace_before = np.trace(eskf.get_state().covariance[0:3, 0:3])
eskf.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
def test_satellite_update_covariance_bounded_by_noise(self, eskf):
"""Test that final covariance is bounded by satellite noise level."""
eskf.update_satellite(np.zeros(3), noise_meters=5.0)
pos_cov_trace = np.trace(eskf.get_state().covariance[0:3, 0:3])
# Covariance should be bounded by roughly 3*noise^2 (3 dimensions)
assert pos_cov_trace < 3 * (5.0 ** 2)
class TestESKFConfidenceTiers:
"""Tests for ESKF-05: Confidence tier computation."""
def test_confidence_high(self, eskf, config):
"""Test HIGH confidence with recent satellite and low covariance."""
eskf.update_satellite(np.zeros(3), noise_meters=5.0)
# Advance time slightly (still within satellite_max_age)
imu = IMUMeasurement(
accel=np.array([0.0, 0.0, 9.81]),
gyro=np.zeros(3),
timestamp=1.0,
)
eskf.predict(imu)
assert eskf.get_confidence() == ConfidenceTier.HIGH
def test_confidence_medium(self):
"""Test MEDIUM confidence with recent VO but no satellite."""
e = ESKF()
e.initialize(np.zeros(3), timestamp=0.0)
e.update_vo(np.zeros(3), dt_vo=0.1)
assert e.get_confidence() == ConfidenceTier.MEDIUM
def test_confidence_low(self, eskf):
"""Test LOW confidence with no recent measurements."""
# Many predictions without any update
for i in range(100):
imu = IMUMeasurement(
accel=np.zeros(3),
gyro=np.zeros(3),
timestamp=0.01 * (i + 1),
)
eskf.predict(imu)
assert eskf.get_confidence() == ConfidenceTier.LOW
def test_confidence_failed(self, eskf):
"""Test FAILED confidence with 3+ consecutive failures."""
assert eskf.get_confidence(consecutive_failures=3) == ConfidenceTier.FAILED
assert eskf.get_confidence(consecutive_failures=0) != ConfidenceTier.FAILED
class TestESKFStateAccess:
"""Tests for ESKF state accessor."""
def test_get_state_returns_eskf_state(self, eskf):
"""Test that get_state returns properly formed ESKFState."""
state = eskf.get_state()
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)
assert isinstance(state.confidence, ConfidenceTier)
class TestESKFFusion:
"""Integration tests for full ESKF fusion sequence."""
def test_full_fusion_sequence(self):
"""Test complete IMU → VO → satellite fusion."""
e = ESKF()
e.initialize(np.zeros(3), timestamp=0.0)
# IMU prediction phase
for i in range(10):
imu = IMUMeasurement(
accel=np.array([0.0, 0.0, 9.81]),
gyro=np.zeros(3),
timestamp=0.01 * (i + 1),
)
e.predict(imu)
P_before_vo = np.trace(e.get_state().covariance[0:3, 0:3])
# VO update
e.update_vo(np.array([0.1, 0.0, 0.0]), dt_vo=0.1)
P_after_vo = np.trace(e.get_state().covariance[0:3, 0:3])
assert P_after_vo < P_before_vo
# More IMU prediction
for i in range(10):
imu = IMUMeasurement(
accel=np.array([0.0, 0.0, 9.81]),
gyro=np.zeros(3),
timestamp=0.01 * (i + 11),
)
e.predict(imu)
# Satellite update
e.update_satellite(np.array([1.0, 0.0, 0.0]), noise_meters=5.0)
state = e.get_state()
# Position should be close to satellite measurement
assert np.linalg.norm(state.position[:2] - [1.0, 0.0]) < 10.0
assert state.confidence == ConfidenceTier.HIGH