fix(gps): adapt Doppler bandwidth from board validation

This commit is contained in:
2026-07-26 03:15:52 +03:00
parent 23650be635
commit 156cfc8b77
24 changed files with 592 additions and 49 deletions
@@ -2,6 +2,7 @@
from __future__ import annotations
from dataclasses import replace
import unittest
import numpy as np
@@ -42,6 +43,19 @@ def _wls_rms(trace) -> float:
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])
@@ -85,6 +99,56 @@ class AdaptiveAlphaTest(unittest.TestCase):
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"),