213 lines
7.6 KiB
Python
213 lines
7.6 KiB
Python
"""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()
|