From 7afce9cc723897121583378155cb22867fbb72f6 Mon Sep 17 00:00:00 2001 From: Florian Pfaff <6773539+FlorianPfaff@users.noreply.github.com> Date: Sat, 9 May 2026 02:13:54 +0200 Subject: [PATCH 1/5] Add robust model update wrapper --- src/pyrecest/filters/kalman_filter.py | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/pyrecest/filters/kalman_filter.py b/src/pyrecest/filters/kalman_filter.py index 232603ff0..6beb4b359 100644 --- a/src/pyrecest/filters/kalman_filter.py +++ b/src/pyrecest/filters/kalman_filter.py @@ -307,6 +307,29 @@ def update_linear_robust( return diagnostics return None + def update_model_robust( + self, + measurement_model, + measurement, + **kwargs, + ): + """Robustly update with a structural linear Gaussian model object.""" + measurement_matrix = _get_required_model_attribute( + measurement_model, + "measurement_matrix", + ) + meas_noise = _get_required_model_attribute( + measurement_model, + "meas_noise", + "measurement_noise_cov", + ) + return self.update_linear_robust( + measurement, + measurement_matrix, + meas_noise, + **kwargs, + ) + def update_model( self, measurement_model, From a643bf08743451a57bb07c57f892b76762ecb6a1 Mon Sep 17 00:00:00 2001 From: Florian Pfaff <6773539+FlorianPfaff@users.noreply.github.com> Date: Sat, 9 May 2026 02:16:02 +0200 Subject: [PATCH 2/5] Add robust linear Gaussian update tests --- .../test_robust_linear_gaussian_update.py | 177 ++++++++++++++++++ 1 file changed, 177 insertions(+) create mode 100644 tests/filters/test_robust_linear_gaussian_update.py diff --git a/tests/filters/test_robust_linear_gaussian_update.py b/tests/filters/test_robust_linear_gaussian_update.py new file mode 100644 index 000000000..0559996d2 --- /dev/null +++ b/tests/filters/test_robust_linear_gaussian_update.py @@ -0,0 +1,177 @@ +import unittest + +import numpy as np +import numpy.testing as npt + +import pyrecest.backend +from pyrecest.backend import array, eye, to_numpy +from pyrecest.distributions import GaussianDistribution +from pyrecest.filters import KalmanFilter +from pyrecest.filters._linear_gaussian import ( + huber_covariance_scale, + linear_gaussian_update, + linear_gaussian_update_robust, + normalized_innovation_squared, + student_t_covariance_scale, +) + + +@unittest.skipIf( + pyrecest.backend.__backend_name__ in ("pytorch", "jax"), + reason="tests compare backend scalars with NumPy helpers", +) +class RobustLinearGaussianUpdateTest(unittest.TestCase): + def setUp(self): + self.mean = array([0.0, 0.0]) + self.covariance = eye(2) + self.measurement_matrix = eye(2) + self.meas_noise = eye(2) + + def test_normalized_innovation_squared_matches_mahalanobis_norm(self): + innovation = array([2.0, 0.0]) + innovation_covariance = eye(2) * 2.0 + + nis = normalized_innovation_squared(innovation, innovation_covariance) + + self.assertAlmostEqual(float(nis), 2.0) + + def test_student_t_scale_is_monotonic(self): + small = student_t_covariance_scale(2.0, measurement_dim=2, dof=4.0) + large = student_t_covariance_scale(100.0, measurement_dim=2, dof=4.0) + + self.assertEqual(float(small), 1.0) + self.assertGreater(float(large), float(small)) + + def test_huber_scale_uses_mahalanobis_threshold(self): + inlier = huber_covariance_scale(4.0, huber_threshold=2.0) + outlier = huber_covariance_scale(100.0, huber_threshold=2.0) + + self.assertEqual(float(inlier), 1.0) + self.assertGreater(float(outlier), 1.0) + + def test_student_t_update_downweights_outlier(self): + measurement = array([100.0, 100.0]) + plain_mean, _ = linear_gaussian_update( + self.mean, + self.covariance, + measurement, + self.measurement_matrix, + self.meas_noise, + ) + robust_mean, _, diagnostics = linear_gaussian_update_robust( + self.mean, + self.covariance, + measurement, + self.measurement_matrix, + self.meas_noise, + robust_update="student-t", + student_t_dof=4.0, + return_diagnostics=True, + ) + + self.assertTrue(diagnostics["accepted"]) + self.assertGreater(float(diagnostics["scale"]), 1.0) + self.assertLess( + np.linalg.norm(to_numpy(robust_mean)), + np.linalg.norm(to_numpy(plain_mean)), + ) + + def test_huber_update_downweights_outlier(self): + measurement = array([100.0, 100.0]) + plain_mean, _ = linear_gaussian_update( + self.mean, + self.covariance, + measurement, + self.measurement_matrix, + self.meas_noise, + ) + robust_mean, _, diagnostics = linear_gaussian_update_robust( + self.mean, + self.covariance, + measurement, + self.measurement_matrix, + self.meas_noise, + robust_update="huber", + huber_threshold=2.0, + return_diagnostics=True, + ) + + self.assertTrue(diagnostics["accepted"]) + self.assertGreater(float(diagnostics["scale"]), 1.0) + self.assertLess( + np.linalg.norm(to_numpy(robust_mean)), + np.linalg.norm(to_numpy(plain_mean)), + ) + + def test_none_method_matches_standard_update(self): + measurement = array([1.0, -1.0]) + plain_mean, plain_covariance = linear_gaussian_update( + self.mean, + self.covariance, + measurement, + self.measurement_matrix, + self.meas_noise, + ) + robust_mean, robust_covariance = linear_gaussian_update_robust( + self.mean, + self.covariance, + measurement, + self.measurement_matrix, + self.meas_noise, + robust_update="none", + ) + + npt.assert_allclose(to_numpy(robust_mean), to_numpy(plain_mean)) + npt.assert_allclose(to_numpy(robust_covariance), to_numpy(plain_covariance)) + + def test_gate_rejects_without_robust_update(self): + measurement = array([100.0, 100.0]) + robust_mean, robust_covariance, diagnostics = linear_gaussian_update_robust( + self.mean, + self.covariance, + measurement, + self.measurement_matrix, + self.meas_noise, + robust_update="none", + gate_threshold=1.0, + return_diagnostics=True, + ) + + self.assertFalse(diagnostics["accepted"]) + npt.assert_allclose(to_numpy(robust_mean), to_numpy(self.mean)) + npt.assert_allclose(to_numpy(robust_covariance), to_numpy(self.covariance)) + + def test_kalman_filter_update_linear_robust_returns_diagnostics(self): + kf = KalmanFilter(GaussianDistribution(self.mean, self.covariance)) + diagnostics = kf.update_linear_robust( + array([100.0, 100.0]), + self.measurement_matrix, + self.meas_noise, + robust_update="student-t", + return_diagnostics=True, + ) + + self.assertIn("nis", diagnostics) + self.assertTrue(diagnostics["accepted"]) + self.assertGreater(float(diagnostics["scale"]), 1.0) + + def test_update_model_robust_uses_structural_measurement_model(self): + class MeasurementModel: + measurement_matrix = self.measurement_matrix + meas_noise = self.meas_noise + + kf = KalmanFilter(GaussianDistribution(self.mean, self.covariance)) + diagnostics = kf.update_model_robust( + MeasurementModel(), + array([100.0, 100.0]), + robust_update="huber", + return_diagnostics=True, + ) + + self.assertIn("nis", diagnostics) + self.assertTrue(diagnostics["accepted"]) + self.assertGreater(float(diagnostics["scale"]), 1.0) + + +if __name__ == "__main__": + unittest.main() From b5e17909b47d266da48e459fa6b20e660b9a9d00 Mon Sep 17 00:00:00 2001 From: Florian Pfaff <6773539+FlorianPfaff@users.noreply.github.com> Date: Sat, 9 May 2026 10:38:33 +0200 Subject: [PATCH 3/5] Fix robust update test backend lint --- tests/filters/test_robust_linear_gaussian_update.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/tests/filters/test_robust_linear_gaussian_update.py b/tests/filters/test_robust_linear_gaussian_update.py index 0559996d2..e8dfeb4ce 100644 --- a/tests/filters/test_robust_linear_gaussian_update.py +++ b/tests/filters/test_robust_linear_gaussian_update.py @@ -3,8 +3,7 @@ import numpy as np import numpy.testing as npt -import pyrecest.backend -from pyrecest.backend import array, eye, to_numpy +from pyrecest.backend import __backend_name__, array, eye, to_numpy from pyrecest.distributions import GaussianDistribution from pyrecest.filters import KalmanFilter from pyrecest.filters._linear_gaussian import ( @@ -17,7 +16,7 @@ @unittest.skipIf( - pyrecest.backend.__backend_name__ in ("pytorch", "jax"), + __backend_name__ in ("pytorch", "jax"), reason="tests compare backend scalars with NumPy helpers", ) class RobustLinearGaussianUpdateTest(unittest.TestCase): From b552e18c5e46553f90859b8c76ff794d71a8321d Mon Sep 17 00:00:00 2001 From: Florian Pfaff <6773539+FlorianPfaff@users.noreply.github.com> Date: Sat, 9 May 2026 10:49:55 +0200 Subject: [PATCH 4/5] Fix SO3 transition normalization surface --- src/pyrecest/filters/so3_grid_transition.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/pyrecest/filters/so3_grid_transition.py b/src/pyrecest/filters/so3_grid_transition.py index 4c588c630..577974f0c 100644 --- a/src/pyrecest/filters/so3_grid_transition.py +++ b/src/pyrecest/filters/so3_grid_transition.py @@ -88,10 +88,10 @@ def so3_right_multiplication_grid_transition( column_maxima = reshape(amax(exponents, axis=0), (1, exponents.shape[1])) scores = exp(exponents - column_maxima) - manifold_size = ( - 0.5 - * AbstractHypersphereSubsetDistribution.compute_unit_hypersphere_surface( - quaternion_grid.shape[1] + manifold_dim = quaternion_grid.shape[1] - 1 + manifold_size = 0.5 * ( + AbstractHypersphereSubsetDistribution.compute_unit_hypersphere_surface( + manifold_dim ) ) column_integrals = sum(scores, axis=0, keepdims=True) / scores.shape[0] From 948ba9152949ec669359bd0825b026a574c917af Mon Sep 17 00:00:00 2001 From: Florian Pfaff <6773539+FlorianPfaff@users.noreply.github.com> Date: Sat, 9 May 2026 11:02:04 +0200 Subject: [PATCH 5/5] Fix conditional hemisphere normalization check --- .../conditional/sd_half_cond_sd_half_grid_distribution.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/pyrecest/distributions/conditional/sd_half_cond_sd_half_grid_distribution.py b/src/pyrecest/distributions/conditional/sd_half_cond_sd_half_grid_distribution.py index ae92fb677..d34004348 100644 --- a/src/pyrecest/distributions/conditional/sd_half_cond_sd_half_grid_distribution.py +++ b/src/pyrecest/distributions/conditional/sd_half_cond_sd_half_grid_distribution.py @@ -57,11 +57,11 @@ def __init__(self, grid, grid_values, enforce_pdf_nonnegative=True): def _check_normalization(self, tol=0.01): """Warn if any column is not normalized to 1 over the hemisphere.""" - # Match HyperhemisphericalGridDistribution.get_manifold_size(), which - # uses the grid embedding dimension for the existing quadrature rule. + # Match HyperhemisphericalGridDistribution.get_manifold_size(): a grid + # in R^d represents S^{d-1}, so the quadrature surface uses d - 1. hemisphere_surface = 0.5 * ( AbstractHypersphereSubsetDistribution.compute_unit_hypersphere_surface( - self.grid.shape[1] + self.grid.shape[1] - 1 ) ) ints = mean(self.grid_values, axis=0) * hemisphere_surface