diff --git a/src/pyrecest/filters/daum_huang_particle_filter.py b/src/pyrecest/filters/daum_huang_particle_filter.py index d8a5dfb36..c0d745c67 100644 --- a/src/pyrecest/filters/daum_huang_particle_filter.py +++ b/src/pyrecest/filters/daum_huang_particle_filter.py @@ -8,7 +8,7 @@ import numpy as np # pylint: disable=no-name-in-module,no-member,too-many-arguments,too-many-positional-arguments -from pyrecest.backend import asarray, eye, ones, to_numpy +from pyrecest.backend import asarray, eye, to_numpy from pyrecest.distributions.nonperiodic.linear_dirac_distribution import ( LinearDiracDistribution, ) @@ -312,13 +312,14 @@ def update_model( return_info: bool = False, ): """Update from a structural Gaussian measurement model with Jacobians.""" + prior_weights = self.filter_state.w result, info = gaussian_particle_flow_update( self.filter_state.d, measurement_model, measurement, flow_type=self.flow_type, measurement_noise_covariance=measurement_noise_covariance, - weights=self.filter_state.w, + weights=prior_weights, n_steps=self.n_steps if n_steps is None else n_steps, step_schedule=( self.step_schedule if step_schedule is None else step_schedule @@ -326,10 +327,7 @@ def update_model( jitter=self.jitter if jitter is None else jitter, return_info=True, ) - n_particles = self.filter_state.w.shape[0] - self._filter_state = LinearDiracDistribution( - result, ones(n_particles) / n_particles - ) + self._filter_state = LinearDiracDistribution(result, prior_weights) return info if return_info else None diff --git a/tests/filters/test_daum_huang_particle_filter.py b/tests/filters/test_daum_huang_particle_filter.py index a2b8467df..53dc01f96 100644 --- a/tests/filters/test_daum_huang_particle_filter.py +++ b/tests/filters/test_daum_huang_particle_filter.py @@ -122,6 +122,56 @@ def test_edh_filter_linear_update_matches_bridge_moments(self): to_numpy(actual_covariance), to_numpy(expected_covariance), atol=1e-10 ) + def test_particle_flow_filters_preserve_nonuniform_weights(self): + particles = array( + [ + [-2.0, -1.0], + [-1.0, 1.0], + [0.0, 0.0], + [1.0, -1.0], + [2.0, 1.0], + ] + ) + weights = array([0.05, 0.10, 0.15, 0.25, 0.45]) + prior = LinearDiracDistribution(particles, weights) + expected_weights = to_numpy(prior.w).copy() + measurement_matrix = array([[1.0, -0.5]]) + measurement = array([0.25]) + measurement_noise = array([[0.75]]) + expected_mean, expected_covariance = gaussian_bridge_moments( + prior.mean(), + prior.covariance(), + measurement_matrix, + measurement, + measurement_noise, + 1.0, + jitter=0.0, + ) + + for filter_type in (EDHParticleFlowFilter, LEDHParticleFlowFilter): + with self.subTest(filter_type=filter_type.__name__): + filt = filter_type( + n_particles=particles.shape[0], + dim=particles.shape[1], + n_steps=4, + jitter=0.0, + ) + filt.filter_state = LinearDiracDistribution(particles, weights) + + filt.update_linear(measurement, measurement_matrix, measurement_noise) + + npt.assert_allclose(to_numpy(filt.filter_state.w), expected_weights) + npt.assert_allclose( + to_numpy(filt.filter_state.mean()), + to_numpy(expected_mean), + atol=1e-10, + ) + npt.assert_allclose( + to_numpy(filt.filter_state.covariance()), + to_numpy(expected_covariance), + atol=1e-10, + ) + def test_ledh_records_particlewise_linearization_points(self): particles = array([[-2.0], [-1.0], [1.0], [2.0]])