Fix cancellation in Merwe sigma-point scaling

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,14 +227,14 @@ 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)]
+        negative = [2.0 * x - sigma for sigma in positive]
         return stack([x, *positive, *negative])
 
 

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)