diff --git a/src/pyrecest/filters/adaptive_process_noise.py b/src/pyrecest/filters/adaptive_process_noise.py index fd74b3e98a..80cb89d652 100644 --- a/src/pyrecest/filters/adaptive_process_noise.py +++ b/src/pyrecest/filters/adaptive_process_noise.py @@ -194,15 +194,26 @@ def ratio(self, source_weights: Mapping[str, float] | None = None) -> float: if not self.ratios_by_source: return 1.0 if source_weights: - numerator = 0.0 - denominator = 0.0 + weighted_ratios: list[tuple[float, float]] = [] for source, ratio in self.ratios_by_source.items(): - weight = float(source_weights.get(source, 0.0)) + weight = _normalize_nonnegative_finite_scalar( + source_weights.get(source, 0.0), + f"source_weights[{source!r}]", + ) if weight > 0.0: - numerator += weight * ratio - denominator += weight - if denominator > 0.0: - return float(numerator / denominator) + weighted_ratios.append((ratio, weight)) + if weighted_ratios: + weight_scale = max(weight for _, weight in weighted_ratios) + aggregate = 0.0 + total_weight = 0.0 + for ratio, weight in weighted_ratios: + scaled_weight = weight / weight_scale + updated_total_weight = total_weight + scaled_weight + aggregate += (scaled_weight / updated_total_weight) * ( + ratio - aggregate + ) + total_weight = updated_total_weight + return float(aggregate) return float(np.mean(list(self.ratios_by_source.values()))) def scale(self, source_weights: Mapping[str, float] | None = None) -> float: diff --git a/tests/filters/test_adaptive_process_noise_source_weights.py b/tests/filters/test_adaptive_process_noise_source_weights.py new file mode 100644 index 0000000000..21c93effcc --- /dev/null +++ b/tests/filters/test_adaptive_process_noise_source_weights.py @@ -0,0 +1,51 @@ +"""Regression tests for weighted rolling NIS aggregation.""" + +import numpy as np +import pytest + +from pyrecest.filters.adaptive_process_noise import ( + AdaptiveProcessNoiseConfig, + RollingNISProcessNoiseAdapter, +) + + +def _adapter_with_two_sources(): + adapter = RollingNISProcessNoiseAdapter( + AdaptiveProcessNoiseConfig(ewma_alpha=1.0) + ) + adapter.observe(source="radar", measurement_dim=1, nis=2.0) + adapter.observe(source="camera", measurement_dim=1, nis=4.0) + return adapter + + +def test_weighted_ratio_remains_finite_for_maximum_finite_weights(): + adapter = _adapter_with_two_sources() + weight = np.finfo(float).max + + ratio = adapter.ratio({"radar": weight, "camera": weight}) + + assert np.isfinite(ratio) + assert ratio == pytest.approx(3.0) + + +@pytest.mark.parametrize( + "weight", + [ + np.nan, + np.inf, + -np.inf, + -1.0, + True, + "1.0", + np.array([1.0]), + np.timedelta64(1, "ns"), + ], +) +def test_weighted_ratio_rejects_invalid_source_weights(weight): + adapter = _adapter_with_two_sources() + + with pytest.raises( + ValueError, + match=r"source_weights\['radar'\] must be a nonnegative finite scalar", + ): + adapter.ratio({"radar": weight, "camera": 1.0})