diff --git a/src/pyrecest/_backend/_shared_numpy/random.py b/src/pyrecest/_backend/_shared_numpy/random.py index dc5569f25..02db1e8cc 100644 --- a/src/pyrecest/_backend/_shared_numpy/random.py +++ b/src/pyrecest/_backend/_shared_numpy/random.py @@ -90,7 +90,10 @@ def _validate_uniform_bounds(low, high): def _uniform(low=0.0, high=1.0, size=None): _validate_uniform_bounds(low, high) - return _np.random.uniform(low, high, _normalize_size(size)) + try: + return _np.random.uniform(low, high, _normalize_size(size)) + except OverflowError as exc: + raise OverflowError("high - low range exceeds valid bounds") from exc uniform = _modify_func_default_dtype( diff --git a/src/pyrecest/filters/mem_rbpf_tracker.py b/src/pyrecest/filters/mem_rbpf_tracker.py index fec88b01d..056ec5f9c 100644 --- a/src/pyrecest/filters/mem_rbpf_tracker.py +++ b/src/pyrecest/filters/mem_rbpf_tracker.py @@ -317,6 +317,13 @@ def predict_linear( shape_system_matrix=None, shape_sys_noise=None, ): + if inputs is not None: + inputs = array(inputs) + expected_shape = self.kinematic_state.shape + if inputs.shape != expected_shape: + raise ValueError( + f"inputs must have shape {expected_shape}, got {inputs.shape}" + ) if system_matrix is not None: self.system_matrix = array(system_matrix) self._validate_system_matrix(self.system_matrix) @@ -326,7 +333,7 @@ def predict_linear( ) self.kinematic_state = self.system_matrix @ self.kinematic_state if inputs is not None: - self.kinematic_state = self.kinematic_state + array(inputs) + self.kinematic_state = self.kinematic_state + inputs self.covariance = self._symmetrize( self.system_matrix @ self.covariance @ self.system_matrix.T + self.sys_noise ) diff --git a/tests/filters/test_mem_rbpf_control_input_validation.py b/tests/filters/test_mem_rbpf_control_input_validation.py new file mode 100644 index 000000000..15148abb3 --- /dev/null +++ b/tests/filters/test_mem_rbpf_control_input_validation.py @@ -0,0 +1,56 @@ +import unittest + +import numpy.testing as npt +import pyrecest.backend +from pyrecest.backend import array, diag, eye +from pyrecest.filters.mem_rbpf_tracker import MEMRBPFTracker + + +@unittest.skipIf( + pyrecest.backend.__backend_name__ != "numpy", + reason="MEM-RBPF tracker tests currently use numpy.testing assertions", +) +class MEMRBPFControlInputValidationTest(unittest.TestCase): + @staticmethod + def _tracker(): + return MEMRBPFTracker( + kinematic_state=array([0.0, 0.0, 1.0, -0.5]), + covariance=eye(4), + shape_state=array([0.2, 2.0, 1.0]), + shape_covariance=diag(array([0.05, 0.1, 0.1])), + meas_noise_cov=0.05 * eye(2), + sys_noise=0.01 * eye(4), + shape_sys_noise=diag(array([0.01, 0.01, 0.01])), + n_particles=8, + resampling_threshold=0, + rng=0, + ) + + def test_rejects_broadcastable_wrong_sized_input_without_mutating_state(self): + tracker = self._tracker() + prior_state = tracker.kinematic_state.copy() + prior_covariance = tracker.covariance.copy() + prior_system_matrix = tracker.system_matrix.copy() + prior_sys_noise = tracker.sys_noise.copy() + prior_theta = tracker.theta.copy() + prior_axis = tracker.axis.copy() + prior_axis_covariances = tracker.axis_covariances.copy() + + with self.assertRaisesRegex(ValueError, r"inputs must have shape \(4,\)"): + tracker.predict_linear( + system_matrix=2.0 * eye(4), + sys_noise=3.0 * eye(4), + inputs=array([2.0]), + ) + + npt.assert_array_equal(tracker.kinematic_state, prior_state) + npt.assert_array_equal(tracker.covariance, prior_covariance) + npt.assert_array_equal(tracker.system_matrix, prior_system_matrix) + npt.assert_array_equal(tracker.sys_noise, prior_sys_noise) + npt.assert_array_equal(tracker.theta, prior_theta) + npt.assert_array_equal(tracker.axis, prior_axis) + npt.assert_array_equal(tracker.axis_covariances, prior_axis_covariances) + + +if __name__ == "__main__": + unittest.main()