mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-23 02:16:36 +00:00
feat(tests): add comprehensive ESKF + coordinate chain tests (ESKF-01..06)
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
@@ -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
|
||||
Reference in New Issue
Block a user