Temporary rebase staging for #4289

src/pyrecest/sampling/sigma_points.py

@@ -90,6 +90,18 @@ def _validate_finite_scalar(value, name: str) -> float:
     return result
 
 
+def _merwe_scale(n: int, alpha: float, kappa: float) -> float:
+    """Return ``alpha**2 * (n + kappa)`` without subtractive cancellation."""
+
+    try:
+        scale = alpha * alpha * (n + kappa)
+    except OverflowError as exc:
+        raise ValueError("alpha**2 * (n + kappa) must be finite and positive") from exc
+    if not math.isfinite(scale) or scale <= 0.0:
+        raise ValueError("alpha**2 * (n + kappa) must be finite and positive")
+    return scale
+
+
 def _validate_sigma_inputs(x, P, n: int):
     if _has_complex_dtype(x):
         raise ValueError("x must contain real values")
@@ -138,8 +150,8 @@ 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(n, self.alpha, self.kappa)
+        lam = scale - n
 
         self.Wm = concatenate(
             [
@@ -168,11 +180,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(n, self.alpha, 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)]

tests/test_sigma_points_small_alpha.py

@@ -0,0 +1,30 @@
+import unittest
+
+import numpy as np
+import numpy.testing as npt
+from pyrecest.backend import __backend_name__, asarray, to_numpy
+from pyrecest.sampling import MerweScaledSigmaPoints
+
+
+@unittest.skipIf(
+    __backend_name__ == "pytorch",
+    reason="Sigma-point tests use NumPy assertions and the PyTorch backend is unsupported",
+)
+class TestMerweSmallAlpha(unittest.TestCase):
+    def test_small_positive_alpha_does_not_cancel_scale_to_zero(self):
+        points = MerweScaledSigmaPoints(n=1, alpha=1.0e-9, beta=2.0, kappa=0.0)
+
+        sigmas = points.sigma_points(asarray([0.0]), asarray([[1.0]]))
+
+        self.assertTrue(np.all(np.isfinite(to_numpy(points.Wm))))
+        self.assertTrue(np.all(np.isfinite(to_numpy(points.Wc))))
+        npt.assert_allclose(
+            to_numpy(sigmas),
+            np.array([[0.0], [1.0e-9], [-1.0e-9]]),
+            rtol=1.0e-12,
+            atol=0.0,
+        )
+
+
+if __name__ == "__main__":
+    unittest.main()