"""Regression tests for the project-specific Doppler/WLS/ESKF model.""" from __future__ import annotations from dataclasses import replace import unittest import numpy as np from .doppler_eskf_model import ( AdaptiveAlpha, DopplerPath, FIXED_DOPPLER_ALPHA, flight_scenarios, local_los_matrix, run_doppler_eskf_scenario, velocity_wls, ) def _scenario(name: str): return next(item for item in flight_scenarios() if item.name == name) def _fused_rms(trace) -> float: selected = trace.time_s >= 5.0 error = trace.fused_velocity_mps[selected] - trace.truth_velocity_mps[selected] return float(np.sqrt(np.mean(np.sum(np.square(error), axis=1)))) def _wls_rms(trace) -> float: truth = np.column_stack( [ np.interp( trace.wls_time_s, trace.time_s, trace.truth_velocity_mps[:, axis], ) for axis in range(3) ] ) error = trace.wls_velocity_mps - truth return float(np.sqrt(np.mean(np.sum(np.square(error), axis=1)))) def _board_raw_noise_scenario(name: str): """Match the 9.17 m/s raw-WLS RMS measured on Genesys2.""" scenario = _scenario(name) nominal_noise = scenario.noise_multiplier return replace( scenario, name=f"{scenario.name}_genesys2_noise", noise_multiplier=lambda time_s, satellite: ( 22.0 * nominal_noise(time_s, satellite) ), ) class DopplerWlsTest(unittest.TestCase): def test_four_state_wls_separates_velocity_and_clock_drift(self) -> None: velocity = np.asarray([145.0, -32.0, 7.5]) clock_drift = 18.0 observation = -local_los_matrix() @ velocity + clock_drift result = velocity_wls(observation, np.full(8, 45.0)) np.testing.assert_allclose(result.velocity_mps, velocity, atol=1.0e-10) self.assertAlmostEqual(result.clock_drift_mps, clock_drift, places=10) self.assertAlmostEqual(result.covariance_scale, 1.0) def test_robust_wls_excludes_a_single_bad_channel(self) -> None: velocity = np.asarray([120.0, 20.0, -2.0]) observation = -local_los_matrix() @ velocity + 4.0 observation[2] += 12.0 plain = velocity_wls(observation, np.full(8, 43.0)) robust = velocity_wls(observation, np.full(8, 43.0), robust=True) self.assertEqual(robust.excluded_satellite, 2) self.assertGreater(np.linalg.norm(plain.velocity_mps - velocity), 1.0) np.testing.assert_allclose(robust.velocity_mps, velocity, atol=1.0e-9) class AdaptiveAlphaTest(unittest.TestCase): def test_motion_switches_alpha_and_quiet_state_recovers(self) -> None: selector = AdaptiveAlpha() for _ in range(80): quiet = selector.update(np.zeros(3), 35.0) self.assertAlmostEqual(quiet, 0.03) dynamic_values = [ selector.update(np.asarray([4.0, 0.0, 0.0]), 35.0) for _ in range(30) ] self.assertGreaterEqual(max(dynamic_values), 0.40) for _ in range(120): recovered = selector.update(np.zeros(3), 40.0) self.assertAlmostEqual(recovered, FIXED_DOPPLER_ALPHA) class DopplerEskfIntegrationTest(unittest.TestCase): def test_board_raw_noise_requires_adaptive_prefilter(self) -> None: steady = run_doppler_eskf_scenario( _board_raw_noise_scenario("steady_cruise"), seed=7, paths=( DopplerPath.FIXED_IIR, DopplerPath.ADAPTIVE_IIR, DopplerPath.RAW, ), ) airliner = run_doppler_eskf_scenario( _board_raw_noise_scenario("airliner_maneuver"), seed=3, paths=( DopplerPath.FIXED_IIR, DopplerPath.ADAPTIVE_IIR, DopplerPath.RAW, ), ) aggressive = run_doppler_eskf_scenario( _board_raw_noise_scenario("aggressive_maneuver"), seed=7, paths=( DopplerPath.FIXED_IIR, DopplerPath.ADAPTIVE_IIR, DopplerPath.RAW, ), ) steady_fixed = _fused_rms(steady[DopplerPath.FIXED_IIR]) steady_adaptive = _fused_rms(steady[DopplerPath.ADAPTIVE_IIR]) steady_raw = _fused_rms(steady[DopplerPath.RAW]) airliner_fixed = _fused_rms(airliner[DopplerPath.FIXED_IIR]) airliner_adaptive = _fused_rms(airliner[DopplerPath.ADAPTIVE_IIR]) airliner_raw = _fused_rms(airliner[DopplerPath.RAW]) aggressive_fixed = _fused_rms(aggressive[DopplerPath.FIXED_IIR]) aggressive_adaptive = _fused_rms( aggressive[DopplerPath.ADAPTIVE_IIR] ) aggressive_raw = _fused_rms(aggressive[DopplerPath.RAW]) # Direct 20 ms snapshots are too noisy at the level actually observed # on the board. Adaptive bandwidth retains the quiet-state result and # beats both direct raw and the 0.39 s fixed-IIR lag in manoeuvres. self.assertLess(steady_adaptive, 1.05 * steady_fixed) self.assertLess(steady_adaptive, 0.50 * steady_raw) self.assertLess(airliner_adaptive, airliner_fixed) self.assertLess(airliner_adaptive, 0.50 * airliner_raw) self.assertLess(aggressive_adaptive, 0.65 * aggressive_fixed) self.assertLess(aggressive_adaptive, 0.50 * aggressive_raw) def test_eskf_filters_raw_doppler_without_pre_iir(self) -> None: traces = run_doppler_eskf_scenario( _scenario("steady_cruise"), seed=7, paths=(DopplerPath.RAW,), ) raw = traces[DopplerPath.RAW] self.assertLess(_fused_rms(raw), 0.45 * _wls_rms(raw)) self.assertGreater(float(np.mean(raw.wls_accepted)), 0.99) def test_raw_path_avoids_fixed_iir_manoeuvre_lag(self) -> None: traces = run_doppler_eskf_scenario( _scenario("airliner_maneuver"), seed=3, paths=(DopplerPath.FIXED_IIR, DopplerPath.RAW), ) fixed = traces[DopplerPath.FIXED_IIR] raw = traces[DopplerPath.RAW] self.assertLess(_fused_rms(raw), 0.35 * _fused_rms(fixed)) # The production 2/2/3 m/s adapter floors keep both paths inside the # NIS gate; the fixed-IIR penalty now appears as lag error, not reject. self.assertGreater(float(np.mean(raw.wls_accepted)), 0.99) self.assertGreater(float(np.mean(fixed.wls_accepted)), 0.99) def test_common_clock_step_does_not_leak_into_raw_velocity(self) -> None: airliner = run_doppler_eskf_scenario( _scenario("airliner_maneuver"), seed=11, paths=(DopplerPath.RAW,), )[DopplerPath.RAW] clock_step = run_doppler_eskf_scenario( _scenario("clock_drift_steps"), seed=11, paths=(DopplerPath.RAW,), )[DopplerPath.RAW] np.testing.assert_allclose( airliner.wls_velocity_mps, clock_step.wls_velocity_mps, atol=1.0e-10, ) np.testing.assert_allclose( airliner.fused_velocity_mps, clock_step.fused_velocity_mps, atol=1.0e-10, ) def test_robust_wls_contains_channel_bursts_before_eskf(self) -> None: traces = run_doppler_eskf_scenario( _scenario("single_satellite_outliers"), seed=5, paths=(DopplerPath.RAW, DopplerPath.RAW_ROBUST), ) raw = traces[DopplerPath.RAW] robust = traces[DopplerPath.RAW_ROBUST] self.assertLess(_wls_rms(robust), 0.65 * _wls_rms(raw)) self.assertGreater(float(np.mean(robust.excluded_satellites >= 0)), 0.02) if __name__ == "__main__": unittest.main()