diff --git a/src/pyrecest/filters/interacting_multiple_model_filter.py b/src/pyrecest/filters/interacting_multiple_model_filter.py index 4ed276933..eaf6d4b0e 100644 --- a/src/pyrecest/filters/interacting_multiple_model_filter.py +++ b/src/pyrecest/filters/interacting_multiple_model_filter.py @@ -13,6 +13,7 @@ array, asarray, diag, + diagonal, empty, exp, eye, @@ -612,12 +613,13 @@ def _log_linear_measurement_likelihood( @ measurement_matrix.T + meas_noise ) - det_value = float(linalg.det(innovation_covariance)) - if det_value <= 0.0: + try: + cholesky_factor = linalg.cholesky(innovation_covariance) + except (np.linalg.LinAlgError, RuntimeError, ValueError) as exc: raise ValueError( "Innovation covariance must be positive definite to evaluate the IMM likelihood." - ) - logdet = float(log(array(det_value))) + ) from exc + logdet = 2.0 * float(log(diagonal(cholesky_factor)).sum()) mahalanobis_distance = float( innovation.T @ linalg.solve(innovation_covariance, innovation) ) diff --git a/tests/filters/test_interacting_multiple_model_filter.py b/tests/filters/test_interacting_multiple_model_filter.py index 585fce8da..386f55309 100644 --- a/tests/filters/test_interacting_multiple_model_filter.py +++ b/tests/filters/test_interacting_multiple_model_filter.py @@ -1,4 +1,5 @@ import copy +import math import unittest import numpy.testing as npt @@ -140,6 +141,27 @@ def test_external_mode_probability_update(self): array([0.8807970779778823, 0.11920292202211755]), ) + def test_linear_likelihood_handles_positive_definite_determinant_underflow(self): + predicted_state = GaussianDistribution( + array([0.0, 0.0]), + array([[0.0, 0.0], [0.0, 0.0]]), + check_validity=False, + ) + measurement_noise = array([[1e-200, 0.0], [0.0, 1e-200]]) + + log_likelihood = ( + InteractingMultipleModelFilter._log_linear_measurement_likelihood( + array([0.0, 0.0]), + predicted_state, + eye(2), + measurement_noise, + ) + ) + + expected = -math.log(2.0 * math.pi) + 200.0 * math.log(10.0) + self.assertTrue(math.isfinite(log_likelihood)) + self.assertAlmostEqual(log_likelihood, expected, places=12) + def test_rejects_nonfinite_transition_matrix_and_mode_probabilities(self): filter_bank = [ MockGaussianFilter(GaussianDistribution(array([0.0]), array([[1.0]]))),