diff --git a/src/pyrecest/sampling/sigma_points.py b/src/pyrecest/sampling/sigma_points.py index 7234f1b8f..f96321247 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,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]) 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 000000000..1678a8458 --- /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)