From 849c93718c5cf4140309b9e6c12f4f3d6cb0b6eb Mon Sep 17 00:00:00 2001 From: Florian Pfaff <6773539+FlorianPfaff@users.noreply.github.com> Date: Thu, 30 Jul 2026 11:41:00 +0200 Subject: [PATCH 1/3] Fix Merwe sigma-point scale cancellation --- src/pyrecest/sampling/sigma_points.py | 56 +++++++++++++++++++++------ 1 file changed, 45 insertions(+), 11 deletions(-) diff --git a/src/pyrecest/sampling/sigma_points.py b/src/pyrecest/sampling/sigma_points.py index 7234f1b8fe..42a2597f0c 100644 --- a/src/pyrecest/sampling/sigma_points.py +++ b/src/pyrecest/sampling/sigma_points.py @@ -115,6 +115,17 @@ def _validate_finite_scalar(value, name: str) -> float: return result +def _merwe_scale(alpha: float, n: int, kappa: float) -> float: + """Return the scaled sigma-point covariance factor without cancellation.""" + + scale = alpha * alpha * (n + kappa) + if not math.isfinite(scale) or scale <= 0.0: + raise ValueError( + "alpha and kappa must yield a finite, positive sigma-point scale" + ) + return scale + + def _validate_sigma_inputs(x, P, n: int): if _has_complex_dtype(x): raise ValueError("x must contain real values") @@ -163,22 +174,45 @@ def __init__(self, n: int, alpha: float, beta: float, kappa: float): def _compute_weights(self): n = self.n - lam = self.alpha**2 * (n + self.kappa) - n - scale = n + lam + scale = _merwe_scale(self.alpha, n, self.kappa) + central_mean_weight = (scale - n) / scale + off_center_weight = 0.5 / scale + off_center_sum = 2.0 * n * off_center_weight + mean_weight_sum = math.fsum((central_mean_weight, off_center_sum)) + + if ( + not math.isfinite(central_mean_weight) + or not math.isfinite(off_center_weight) + or not math.isfinite(off_center_sum) + or not math.isclose( + mean_weight_sum, + 1.0, + rel_tol=0.0, + abs_tol=8.0 * np.finfo(np.float64).eps, + ) + ): + raise ValueError( + "alpha and kappa must yield finite, normalized mean weights" + ) + + central_covariance_weight = central_mean_weight + ( + 1.0 - self.alpha * self.alpha + self.beta + ) + if not math.isfinite(central_covariance_weight): + raise ValueError( + "alpha, beta, and kappa must yield finite covariance weights" + ) self.Wm = concatenate( [ - asarray([lam / scale], dtype=float64), - full(2 * n, 0.5 / scale, dtype=float64), + asarray([central_mean_weight], dtype=float64), + full(2 * n, off_center_weight, dtype=float64), ] ) self.Wc = concatenate( [ - asarray( - [lam / scale + (1.0 - self.alpha**2 + self.beta)], - dtype=float64, - ), - full(2 * n, 0.5 / scale, dtype=float64), + asarray([central_covariance_weight], dtype=float64), + full(2 * n, off_center_weight, dtype=float64), ] ) @@ -193,11 +227,11 @@ def sigma_points(self, x, P): State covariance, shape ``(n, n)``. """ n = self.n - lam = self.alpha**2 * (n + self.kappa) - n + scale = _merwe_scale(self.alpha, n, self.kappa) x, P = _validate_sigma_inputs(x, P, n) - U = linalg.cholesky((n + lam) * P) # lower-triangular + U = linalg.cholesky(scale * P) # lower-triangular positive = [x + U[:, i] for i in range(n)] negative = [x - U[:, i] for i in range(n)] From 91640401f19e845c55d72ba1b6484111fcee4697 Mon Sep 17 00:00:00 2001 From: Florian Pfaff <6773539+FlorianPfaff@users.noreply.github.com> Date: Thu, 30 Jul 2026 11:41:13 +0200 Subject: [PATCH 2/3] Add Merwe scale stability regressions --- ...test_sigma_points_merwe_scale_stability.py | 47 +++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 tests/test_sigma_points_merwe_scale_stability.py diff --git a/tests/test_sigma_points_merwe_scale_stability.py b/tests/test_sigma_points_merwe_scale_stability.py new file mode 100644 index 0000000000..1678a8458e --- /dev/null +++ b/tests/test_sigma_points_merwe_scale_stability.py @@ -0,0 +1,47 @@ +import numpy as np +import pytest + +from pyrecest.backend import asarray, to_numpy +from pyrecest.sampling import MerweScaledSigmaPoints + + +def test_merwe_scale_avoids_lambda_cancellation(): + alpha = 2.0e-8 + points = MerweScaledSigmaPoints(n=1, alpha=alpha, beta=2.0, kappa=0.0) + + sigma_points = to_numpy( + points.sigma_points(asarray(np.zeros(1)), asarray(np.eye(1))) + ) + mean_weights = to_numpy(points.Wm) + scale = alpha * alpha + expected_weights = np.array( + [ + (scale - 1.0) / scale, + 0.5 / scale, + 0.5 / scale, + ] + ) + + np.testing.assert_allclose( + sigma_points[:, 0], + np.array([0.0, alpha, -alpha]), + rtol=1.0e-12, + atol=0.0, + ) + np.testing.assert_allclose( + mean_weights, + expected_weights, + rtol=1.0e-15, + atol=0.0, + ) + + +@pytest.mark.parametrize("alpha", [1.0e-200, 1.0e200], ids=["underflow", "overflow"]) +def test_merwe_rejects_nonrepresentable_sigma_point_scales(alpha): + with pytest.raises(ValueError, match="finite, positive sigma-point scale"): + MerweScaledSigmaPoints(n=1, alpha=alpha, beta=2.0, kappa=0.0) + + +def test_merwe_rejects_mean_weights_that_cannot_represent_unit_sum(): + with pytest.raises(ValueError, match="finite, normalized mean weights"): + MerweScaledSigmaPoints(n=1, alpha=1.0e-8, beta=2.0, kappa=0.0) From f6a7095adfa420b7885dc3e8b7d6fb2a918394f9 Mon Sep 17 00:00:00 2001 From: Florian Pfaff <6773539+FlorianPfaff@users.noreply.github.com> Date: Thu, 30 Jul 2026 16:50:42 +0200 Subject: [PATCH 3/3] Preserve exact Merwe sigma-point pair symmetry --- src/pyrecest/sampling/sigma_points.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pyrecest/sampling/sigma_points.py b/src/pyrecest/sampling/sigma_points.py index 42a2597f0c..f96321247d 100644 --- a/src/pyrecest/sampling/sigma_points.py +++ b/src/pyrecest/sampling/sigma_points.py @@ -234,7 +234,7 @@ def sigma_points(self, x, P): U = linalg.cholesky(scale * P) # lower-triangular positive = [x + U[:, i] for i in range(n)] - negative = [x - U[:, i] for i in range(n)] + negative = [2.0 * x - sigma for sigma in positive] return stack([x, *positive, *negative])