fix(gps): adapt Doppler bandwidth from board validation
This commit is contained in:
@@ -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"),
|
||||
|
||||
Reference in New Issue
Block a user