diff --git a/src/pyrecest/smoothers/rauch_tung_striebel_smoother.py b/src/pyrecest/smoothers/rauch_tung_striebel_smoother.py index 8fd03cb7d..612159d4e 100644 --- a/src/pyrecest/smoothers/rauch_tung_striebel_smoother.py +++ b/src/pyrecest/smoothers/rauch_tung_striebel_smoother.py @@ -123,6 +123,15 @@ def filter( # pylint: disable=too-many-arguments,too-many-positional-arguments, state_dim, default=identity_matrix, ) + expected_measurement_matrix_shape = (measurement_dim, state_dim) + if any( + tuple(matrix.shape) != expected_measurement_matrix_shape + for matrix in measurement_matrices_list + ): + raise ValueError( + "measurement_matrices must contain matrices with shape " + f"{expected_measurement_matrix_shape}." + ) meas_noise_covariances_list = self._normalize_matrix_sequence( meas_noise_covariances, len(measurement_list), diff --git a/tests/smoothers/test_rts_measurement_matrix_shape.py b/tests/smoothers/test_rts_measurement_matrix_shape.py new file mode 100644 index 000000000..6eabacd01 --- /dev/null +++ b/tests/smoothers/test_rts_measurement_matrix_shape.py @@ -0,0 +1,27 @@ +import unittest + +from pyrecest.backend import array, eye, zeros +from pyrecest.distributions import GaussianDistribution +from pyrecest.smoothers import RauchTungStriebelSmoother + + +class RauchTungStriebelMeasurementMatrixShapeTest(unittest.TestCase): + def test_rejects_measurement_matrix_with_wrong_row_count(self): + smoother = RauchTungStriebelSmoother() + + with self.assertRaisesRegex( + ValueError, + r"measurement_matrices must contain matrices with shape \(1, 2\)", + ): + smoother.filter( + initial_state=GaussianDistribution(zeros(2), eye(2)), + measurements=array([1.0, 2.0]), + measurement_matrices=eye(2), + meas_noise_covariances=array([1.0, 1.0]), + system_matrices=eye(2), + sys_noise_covariances=zeros((2, 2)), + ) + + +if __name__ == "__main__": + unittest.main()