diff --git a/src/pyrecest/filters/fourier_rhm_tracker.py b/src/pyrecest/filters/fourier_rhm_tracker.py index 6cd943f77..43d3c3517 100644 --- a/src/pyrecest/filters/fourier_rhm_tracker.py +++ b/src/pyrecest/filters/fourier_rhm_tracker.py @@ -1,7 +1,9 @@ from __future__ import annotations +from math import isfinite as python_isfinite from numbers import Integral +import numpy as np import pyrecest.backend # pylint: disable=no-name-in-module,no-member,redefined-builtin,duplicate-code @@ -21,6 +23,7 @@ zeros, zeros_like, ) +from pyrecest.numerics import assert_covariance_matrix from pyrecest.sampling.sigma_points import MerweScaledSigmaPoints from .abstract_extended_object_tracker import AbstractExtendedObjectTracker @@ -30,6 +33,19 @@ def _pol2cart(phi, radius=1.0): return radius * stack((cos(phi), sin(phi))) +def _ensure_finite_real_array(value, name): + """Reject non-finite or complex arrays before they enter the RHM recursion.""" + try: + host_value = np.asarray(pyrecest.backend.to_numpy(value)) + if np.iscomplexobj(host_value): + raise ValueError + finite = bool(np.all(np.isfinite(host_value))) + except (TypeError, ValueError, OverflowError, RuntimeError) as exc: + raise ValueError(f"{name} must contain only finite real values") from exc + if not finite: + raise ValueError(f"{name} must contain only finite real values") + + class FourierRHMTracker( AbstractExtendedObjectTracker ): # pylint: disable=too-many-instance-attributes @@ -78,10 +94,16 @@ def __init__( self.n_harmonics = self._as_integer(n_harmonics, "n_harmonics", 0) self.n_fourier_coefficients = 2 * self.n_harmonics + 1 self.state_dim = self.n_fourier_coefficients + 2 + self.covariance_regularization = self._as_finite_float( + covariance_regularization, "covariance_regularization" + ) + if self.covariance_regularization < 0.0: + raise ValueError("covariance_regularization must be non-negative") if fourier_coefficients is None: + initial_radius = self._as_finite_float(initial_radius, "initial_radius") fourier_coefficients = zeros(self.n_fourier_coefficients) - fourier_coefficients[0] = 2.0 * float(initial_radius) + fourier_coefficients[0] = 2.0 * initial_radius self.fourier_coefficients = self._as_vector( fourier_coefficients, self.n_fourier_coefficients, @@ -109,21 +131,19 @@ def __init__( covariance, self.state_dim, "covariance" ) self._validate_positive_definite( - self.covariance + covariance_regularization * eye(self.state_dim), + self.covariance + + self.covariance_regularization * eye(self.state_dim), "covariance", ) - self.scale_mean = float(scale_mean) - self.scale_variance = float(scale_variance) + self.scale_mean = self._as_finite_float(scale_mean, "scale_mean") + self.scale_variance = self._as_finite_float(scale_variance, "scale_variance") if self.scale_variance < 0.0: raise ValueError("scale_variance must be non-negative") - self.ukf_alpha = float(ukf_alpha) - self.ukf_beta = float(ukf_beta) - self.ukf_kappa = float(ukf_kappa) - self.covariance_regularization = float(covariance_regularization) - if self.covariance_regularization < 0.0: - raise ValueError("covariance_regularization must be non-negative") + self.ukf_alpha = self._as_finite_float(ukf_alpha, "ukf_alpha") + self.ukf_beta = self._as_finite_float(ukf_beta, "ukf_beta") + self.ukf_kappa = self._as_finite_float(ukf_kappa, "ukf_kappa") self.latest_pseudo_measurement = None self.latest_innovation_covariance = None @@ -159,11 +179,22 @@ def _as_integer(value, name, minimum): raise ValueError(f"{name} must be at least {minimum}") return integer + @staticmethod + def _as_finite_float(value, name): + try: + scalar = float(value) + except (TypeError, ValueError, OverflowError) as exc: + raise ValueError(f"{name} must be a finite scalar") from exc + if not python_isfinite(scalar): + raise ValueError(f"{name} must be a finite scalar") + return scalar + @staticmethod def _as_vector(value, dim, name): vector = array(value).reshape(-1) if vector.shape != (dim,): raise ValueError(f"{name} must have shape ({dim},)") + _ensure_finite_real_array(vector, name) return vector @classmethod @@ -177,7 +208,7 @@ def _as_square_matrix(cls, value, dim, name): matrix = diag(matrix) if matrix.shape != (dim, dim): raise ValueError(f"{name} must have shape ({dim}, {dim})") - return cls._symmetrize(matrix) + return assert_covariance_matrix(matrix, name=name, dim=dim) @staticmethod def _normalize_measurements(measurements): @@ -185,14 +216,17 @@ def _normalize_measurements(measurements): if measurements.ndim == 1: if measurements.shape[0] != 2: raise ValueError("A single measurement vector must have shape (2,)") - return reshape(measurements, (2, 1)) - if measurements.ndim != 2: + normalized = reshape(measurements, (2, 1)) + elif measurements.ndim != 2: raise ValueError("measurements must be a vector or a two-dimensional array") - if measurements.shape[0] == 2: - return measurements - if measurements.shape[1] == 2: - return measurements.T - raise ValueError("measurements must have shape (2, n) or (n, 2)") + elif measurements.shape[0] == 2: + normalized = measurements + elif measurements.shape[1] == 2: + normalized = measurements.T + else: + raise ValueError("measurements must have shape (2, n) or (n, 2)") + _ensure_finite_real_array(normalized, "measurements") + return normalized def _state_vector(self): return concatenate([self.fourier_coefficients, self.kinematic_state]) @@ -243,9 +277,12 @@ def get_contour_points(self, n=100): def predict_identity(self, sys_noise=None): if sys_noise is None: sys_noise = zeros((self.state_dim, self.state_dim)) - self.covariance = self._symmetrize( - self.covariance - + self._as_square_matrix(sys_noise, self.state_dim, "sys_noise") + sys_noise = self._as_square_matrix(sys_noise, self.state_dim, "sys_noise") + predicted_covariance = self._symmetrize(self.covariance + sys_noise) + self.covariance = assert_covariance_matrix( + predicted_covariance, + name="predicted_covariance", + dim=self.state_dim, ) if self.log_prior_estimates: self.store_prior_estimates() @@ -258,16 +295,30 @@ def predict_linear(self, system_matrix, sys_noise=None, inputs=None): raise ValueError( f"system_matrix must have shape ({self.state_dim}, {self.state_dim})" ) + _ensure_finite_real_array(system_matrix, "system_matrix") + state = system_matrix @ self._state_vector() if inputs is not None: state = state + self._as_vector(inputs, self.state_dim, "inputs") - self._set_state_vector(state) + state = self._as_vector(state, self.state_dim, "predicted_state") + if sys_noise is None: sys_noise = zeros((self.state_dim, self.state_dim)) - self.covariance = self._symmetrize( - system_matrix @ self.covariance @ system_matrix.T - + self._as_square_matrix(sys_noise, self.state_dim, "sys_noise") + sys_noise = self._as_square_matrix(sys_noise, self.state_dim, "sys_noise") + predicted_covariance = self._symmetrize( + system_matrix @ self.covariance @ system_matrix.T + sys_noise + ) + predicted_covariance = assert_covariance_matrix( + predicted_covariance, + name="predicted_covariance", + dim=self.state_dim, ) + + # Commit only after every failure-prone validation succeeds. In particular, + # invalid process noise must not leave the state advanced while covariance + # remains at the previous time step. + self._set_state_vector(state) + self.covariance = predicted_covariance if self.log_prior_estimates: self.store_prior_estimates() if self.log_prior_extents: @@ -311,7 +362,8 @@ def _update_single(self, measurement, meas_noise_cov, scale_mean, scale_variance augmented_covariance = linalg.block_diag(self.covariance, noise_covariance) augmented_dim = augmented_mean.shape[0] augmented_covariance = self._symmetrize( - augmented_covariance + self.covariance_regularization * eye(augmented_dim) + augmented_covariance + + self.covariance_regularization * eye(augmented_dim) ) sigma_points = MerweScaledSigmaPoints( @@ -378,6 +430,8 @@ def update( scale_mean = self.scale_mean if scale_variance is None: scale_variance = self.scale_variance + scale_mean = self._as_finite_float(scale_mean, "scale_mean") + scale_variance = self._as_finite_float(scale_variance, "scale_variance") if scale_variance < 0.0: raise ValueError("scale_variance must be non-negative") @@ -386,8 +440,8 @@ def update( self._update_single( measurements[:, measurement_index], meas_noise_cov, - float(scale_mean), - float(scale_variance), + scale_mean, + scale_variance, ) if self.log_posterior_estimates: diff --git a/tests/filters/test_fourier_rhm_tracker_validation.py b/tests/filters/test_fourier_rhm_tracker_validation.py new file mode 100644 index 000000000..203b42beb --- /dev/null +++ b/tests/filters/test_fourier_rhm_tracker_validation.py @@ -0,0 +1,81 @@ +import unittest + +import numpy as np +import numpy.testing as npt + +# pylint: disable=no-name-in-module,no-member +import pyrecest.backend +from pyrecest.backend import array, eye, zeros +from pyrecest.filters import FourierRHMTracker + + +@unittest.skipIf( + pyrecest.backend.__backend_name__ != "numpy", + reason="Fourier RHM tracker validation tests use numpy.testing assertions", +) +class TestFourierRHMTrackerValidation(unittest.TestCase): + def test_predict_linear_validation_failure_is_atomic(self): + tracker = FourierRHMTracker(1) + state_before = tracker.get_point_estimate().copy() + covariance_before = tracker.covariance.copy() + system_matrix = 2.0 * eye(tracker.state_dim) + + with self.assertRaises(ValueError): + tracker.predict_linear(system_matrix, sys_noise=zeros((2, 2))) + + npt.assert_allclose(tracker.get_point_estimate(), state_before) + npt.assert_allclose(tracker.covariance, covariance_before) + + def test_process_noise_covariance_is_not_silently_symmetrized(self): + tracker = FourierRHMTracker(1) + covariance_before = tracker.covariance.copy() + asymmetric_noise = eye(tracker.state_dim) + asymmetric_noise[0, 1] = 0.5 + + with self.assertRaises(ValueError): + tracker.predict_identity(asymmetric_noise) + + npt.assert_allclose(tracker.covariance, covariance_before) + + def test_update_rejects_nonfinite_measurement_noise_atomically(self): + tracker = FourierRHMTracker(0) + state_before = tracker.get_point_estimate().copy() + covariance_before = tracker.covariance.copy() + invalid_noise = array([[0.01, 0.0], [0.0, np.nan]]) + + with self.assertRaises(ValueError): + tracker.update(array([2.0, 0.0]), meas_noise_cov=invalid_noise) + + npt.assert_allclose(tracker.get_point_estimate(), state_before) + npt.assert_allclose(tracker.covariance, covariance_before) + + def test_constructor_rejects_nonfinite_scalar_controls(self): + for keyword in ( + "scale_mean", + "scale_variance", + "ukf_alpha", + "ukf_beta", + "ukf_kappa", + "covariance_regularization", + ): + with self.subTest(keyword=keyword), self.assertRaises(ValueError): + FourierRHMTracker(0, **{keyword: np.nan}) + + def test_update_rejects_nonfinite_scale_overrides(self): + tracker = FourierRHMTracker(0) + state_before = tracker.get_point_estimate().copy() + covariance_before = tracker.covariance.copy() + + for keyword in ("scale_mean", "scale_variance"): + with self.subTest(keyword=keyword), self.assertRaises(ValueError): + tracker.update( + array([2.0, 0.0]), + meas_noise_cov=0.01 * eye(2), + **{keyword: np.nan}, + ) + npt.assert_allclose(tracker.get_point_estimate(), state_before) + npt.assert_allclose(tracker.covariance, covariance_before) + + +if __name__ == "__main__": + unittest.main()