diff --git a/src/pyrecest/filters/mem_rbpf_tracker.py b/src/pyrecest/filters/mem_rbpf_tracker.py index 056ec5f9c..47e94d2c5 100644 --- a/src/pyrecest/filters/mem_rbpf_tracker.py +++ b/src/pyrecest/filters/mem_rbpf_tracker.py @@ -463,13 +463,16 @@ def _update_particle_weights(self, centered, meas_noise_cov, mult_var): marginal_cov = self._symmetrize(marginal_cov) if self.covariance_regularization > 0.0: marginal_cov = marginal_cov + self.covariance_regularization * eye(2) - determinant = linalg.det(marginal_cov) - if float(determinant) <= 0.0: + covariance_eigenvalues = linalg.eigvalsh(marginal_cov) + if bool(backend_any(~isfinite(covariance_eigenvalues))) or bool( + backend_any(covariance_eigenvalues <= 0.0) + ): log_likelihoods.append(array(-float("inf"))) continue + log_determinant = backend_sum(log(covariance_eigenvalues)) inverse_cov = linalg.pinv(marginal_cov) quad = einsum("ma,ab,mb->m", centered, inverse_cov, centered) - log_likelihoods.append(-0.5 * backend_sum(log(determinant) + quad)) + log_likelihoods.append(-0.5 * backend_sum(log_determinant + quad)) log_likelihoods = array(log_likelihoods) log_weights = log(maximum(self.weights, 1e-300)) self.weights = self._normalize_log_weights(log_weights + log_likelihoods) diff --git a/tests/filters/test_mem_rbpf_log_weight_stability.py b/tests/filters/test_mem_rbpf_log_weight_stability.py new file mode 100644 index 000000000..1e2e591d1 --- /dev/null +++ b/tests/filters/test_mem_rbpf_log_weight_stability.py @@ -0,0 +1,38 @@ +import numpy as np +import pytest +from pyrecest import backend +from pyrecest.backend import array, diag, eye, zeros +from pyrecest.filters.mem_rbpf_tracker import MEMRBPFTracker + + +pytestmark = pytest.mark.skipif( + backend.__backend_name__ != "numpy", + reason="MEM-RBPF particle-weight stability is currently exercised on NumPy only", +) + + +def test_mem_rbpf_particle_weights_preserve_tiny_covariance_information(): + tracker = MEMRBPFTracker( + kinematic_state=array([0.0, 0.0, 0.0, 0.0]), + covariance=eye(4), + shape_state=array([0.0, 1.0, 1.0]), + shape_covariance=diag(array([1e-3, 1e-3, 1e-3])), + meas_noise_cov=zeros((2, 2)), + sys_noise=zeros((4, 4)), + shape_sys_noise=zeros((3, 3)), + multiplicative_noise_cov=1e-200 * eye(2), + n_particles=2, + resampling_threshold=0, + ) + tracker.theta = array([0.0, 0.0]) + tracker.axis = array([[1.0, 1.0], [np.sqrt(2.0), np.sqrt(2.0)]]) + tracker.axis_covariances = zeros((2, 2, 2)) + tracker.weights = array([0.5, 0.5]) + + tracker._update_particle_weights( + centered=zeros((1, 2)), + meas_noise_cov=zeros((2, 2)), + mult_var=1e-200, + ) + + np.testing.assert_allclose(np.asarray(tracker.weights), [2.0 / 3.0, 1.0 / 3.0])