feat(gps): split Doppler paths for ESKF and PPS

2026-07-26 01:59:08 +03:00
parent 49b8609bb4
commit 23650be635
35 changed files with 3884 additions and 82 deletions
@@ -0,0 +1,148 @@
+"""Regression tests for the project-specific Doppler/WLS/ESKF model."""
+
+from __future__ import annotations
+
+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))))
+
+
+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_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()