Create regulariser object and MCMC particle regulariser

docs/source/stonesoup.regulariser.rst

@@ -0,0 +1,14 @@
+Regulariser
+===========
+
+.. automodule:: stonesoup.regulariser
+    :no-members:
+
+.. automodule:: stonesoup.regulariser.base
+    :show-inheritance:
+
+Particle
+--------
+
+.. automodule:: stonesoup.regulariser.particle
+    :show-inheritance:

docs/source/stonesoup.rst

@@ -46,6 +46,7 @@ Algorithm Components
     stonesoup.mixturereducer
     stonesoup.models
     stonesoup.predictor
+    stonesoup.regulariser
     stonesoup.resampler
     stonesoup.smoother
     stonesoup.updater

stonesoup/regulariser/__init__.py

@@ -0,0 +1,3 @@
+from .base import Regulariser
+
+__all__ = ["Regulariser"]

stonesoup/regulariser/base.py

@@ -0,0 +1,5 @@
+from ..base import Base
+
+
+class Regulariser(Base):
+    """Regulariser base class"""

stonesoup/regulariser/particle.py

@@ -0,0 +1,108 @@
+import copy
+import numpy as np
+from scipy.stats import multivariate_normal, uniform
+
+from .base import Regulariser
+from ..functions import cholesky_eps
+from ..types.state import ParticleState
+
+
+class MCMCRegulariser(Regulariser):
+    """Markov chain Monte-Carlo (MCMC) move steps, or regularisation steps, can be implemented in
+    particle filters to prevent sample impoverishment that results from resampling.
+    One way of avoiding this is to only perform resampling when deemed necessary by some measure
+    of effectiveness. Sometimes this is not desirable, or possible, when a particular algorithm
+    requires the introduction of new samples as part of the filtering process for example.
+
+    This is a particlar implementation of a MCMC move step that uses the Metropolis-Hastings
+    algorithm [1]_. After resampling, particles are moved a small amount, according do a Gaussian
+    kernel, to a new state only if the Metropolis-Hastings acceptance probability is met by a
+    random number assigned to each particle from a uniform random distribution, otherwise they
+    remain the same. Further details on the implementation are given in [2]_.
+
+    References
+    ----------
+    .. [1] Robert, Christian P. & Casella, George, Monte Carlo Statistical Methods, Springer, 1999.
+
+    .. [2] Ristic, Branco & Arulampalam, Sanjeev & Gordon, Neil, Beyond the Kalman Filter:
+        Particle Filters for Target Tracking Applications, Artech House, 2004. """
+
+    def regularise(self, prior, posterior, detections):
+        """Regularise the particles
+
+        Parameters
+        ----------
+        prior : :class:`~.ParticleState` type or list of :class:`~.Particle`
+            prior particle distribution
+        posterior : :class:`~.ParticleState` type or list of :class:`~.Particle`
+            posterior particle distribution
+        detections : set of :class:`~.Detection`
+            set of detections containing clutter,
+            true detections or both
+
+        Returns
+        -------
+        particle state: :class:`~.ParticleState`
+           The particle state after regularisation
+        """
+
+        if not isinstance(posterior, ParticleState):
+            posterior = copy.copy(posterior)
+            posterior = ParticleState(None, particle_list=posterior)
+
+        if not isinstance(prior, ParticleState):
+            prior = copy.copy(prior)
+            prior = ParticleState(None, particle_list=prior)
+
+        regularised_particles = copy.copy(posterior)
+        moved_particles = copy.copy(posterior)
+
+        if detections is not None:
+            ndim = prior.state_vector.shape[0]
+            nparticles = len(posterior)
+
+            measurement_model = next(iter(detections)).measurement_model
+
+            # calculate the optimal bandwidth for the Gaussian kernel
+            hopt = (4/(ndim+2))**(1/(ndim+4))*nparticles**(-1/(ndim+4))
+            covar_est = posterior.covar
+
+            # move particles
+            moved_particles.state_vector = moved_particles.state_vector + \
+                hopt * cholesky_eps(covar_est) @ np.random.randn(ndim, nparticles)
+
+            # Evaluate likelihoods
+            part_diff = np.array(moved_particles.state_vector - prior.state_vector).T
+            part_diff_mean = np.mean(part_diff, axis=0)
+            move_likelihood = multivariate_normal.pdf(part_diff,
+                                                      mean=part_diff_mean,
+                                                      cov=np.array(covar_est))
+            post_part_diff = np.array(posterior.state_vector - prior.state_vector).T
+            post_part_diff_mean = np.mean(post_part_diff, axis=0)
+            post_likelihood = multivariate_normal.pdf(post_part_diff,
+                                                      mean=post_part_diff_mean,
+                                                      cov=covar_est)
+
+            # Evaluate measurement likelihoods
+            move_meas_likelihood = []
+            post_meas_likelihood = []
+            for detection in detections:
+                move_meas_likelihood.append(measurement_model.logpdf(detection, moved_particles))
+                post_meas_likelihood.append(measurement_model.logpdf(detection, moved_particles))
+
+            # In the case that there are multiple measurements,
+            # we select the highest overall likelihood.
+            max_likelihood_idx = np.argmax(np.sum(move_meas_likelihood, axis=1))
+
+            # Calculate acceptance probability (alpha)
+            alpha = (move_meas_likelihood[max_likelihood_idx]*move_likelihood) / \
+                    (post_meas_likelihood[max_likelihood_idx]*post_likelihood)
+
+            # All 'jittered' particles that are above the alpha threshold are kept, the rest are
+            # rejected and the original posterior used
+            selector = uniform.rvs(size=nparticles)
+            index = alpha > selector
+
+            regularised_particles.state_vector[:, index] = moved_particles.state_vector[:, index]
+
+        return regularised_particles

stonesoup/regulariser/tests/test_particle.py

@@ -0,0 +1,102 @@
+import numpy as np
+import datetime
+
+from ...types.state import ParticleState
+from ...types.particle import Particle
+from ...types.hypothesis import SingleHypothesis
+from ...types.prediction import ParticleStatePrediction, ParticleMeasurementPrediction
+from ...models.measurement.linear import LinearGaussian
+from ...types.detection import Detection
+from ...types.update import ParticleStateUpdate
+from ..particle import MCMCRegulariser
+
+
+def test_regulariser():
+    particles = ParticleState(state_vector=None, particle_list=[Particle(np.array([[10], [10]]),
+                                                                         1 / 9),
+                                                                Particle(np.array([[10], [20]]),
+                                                                         1 / 9),
+                                                                Particle(np.array([[10], [30]]),
+                                                                         1 / 9),
+                                                                Particle(np.array([[20], [10]]),
+                                                                         1 / 9),
+                                                                Particle(np.array([[20], [20]]),
+                                                                         1 / 9),
+                                                                Particle(np.array([[20], [30]]),
+                                                                         1 / 9),
+                                                                Particle(np.array([[30], [10]]),
+                                                                         1 / 9),
+                                                                Particle(np.array([[30], [20]]),
+                                                                         1 / 9),
+                                                                Particle(np.array([[30], [30]]),
+                                                                         1 / 9),
+                                                                ])
+    timestamp = datetime.datetime.now()
+    prediction = ParticleStatePrediction(None, particle_list=particles.particle_list,
+                                         timestamp=timestamp)
+    meas_pred = ParticleMeasurementPrediction(None, particle_list=particles, timestamp=timestamp)
+    measurement_model = LinearGaussian(ndim_state=2, mapping=(0, 1), noise_covar=np.eye(2))
+    measurement = [Detection(state_vector=np.array([[5], [7]]),
+                             timestamp=timestamp, measurement_model=measurement_model)]
+    state_update = ParticleStateUpdate(None, SingleHypothesis(prediction=prediction,
+                                                              measurement=measurement,
+                                                              measurement_prediction=meas_pred),
+                                       particle_list=particles.particle_list, timestamp=timestamp)
+    regulariser = MCMCRegulariser()
+
+    # state check
+    new_particles = regulariser.regularise(particles, state_update, measurement)
+    # Check the shape of the new state vector
+    assert new_particles.state_vector.shape == state_update.state_vector.shape
+    # Check weights are unchanged
+    assert any(new_particles.weight == state_update.weight)
+    # Check that the timestamp is the same
+    assert new_particles.timestamp == state_update.timestamp
+
+    # list check
+    new_particles = regulariser.regularise(particles.particle_list, state_update.particle_list,
+                                           measurement)
+    # Check the shape of the new state vector
+    assert new_particles.state_vector.shape == state_update.state_vector.shape
+    # Check weights are unchanged
+    assert any(new_particles.weight == state_update.weight)
+
+
+def test_no_measurement():
+    particles = ParticleState(state_vector=None, particle_list=[Particle(np.array([[10], [10]]),
+                                                                         1 / 9),
+                                                                Particle(np.array([[10], [20]]),
+                                                                         1 / 9),
+                                                                Particle(np.array([[10], [30]]),
+                                                                         1 / 9),
+                                                                Particle(np.array([[20], [10]]),
+                                                                         1 / 9),
+                                                                Particle(np.array([[20], [20]]),
+                                                                         1 / 9),
+                                                                Particle(np.array([[20], [30]]),
+                                                                         1 / 9),
+                                                                Particle(np.array([[30], [10]]),
+                                                                         1 / 9),
+                                                                Particle(np.array([[30], [20]]),
+                                                                         1 / 9),
+                                                                Particle(np.array([[30], [30]]),
+                                                                         1 / 9),
+                                                                ])
+    timestamp = datetime.datetime.now()
+    prediction = ParticleStatePrediction(None, particle_list=particles.particle_list,
+                                         timestamp=timestamp)
+    meas_pred = ParticleMeasurementPrediction(None, particle_list=particles, timestamp=timestamp)
+    state_update = ParticleStateUpdate(None, SingleHypothesis(prediction=prediction,
+                                                              measurement=None,
+                                                              measurement_prediction=meas_pred),
+                                       particle_list=particles.particle_list, timestamp=timestamp)
+    regulariser = MCMCRegulariser()
+
+    new_particles = regulariser.regularise(particles, state_update, detections=None)
+
+    # Check the shape of the new state vector
+    assert new_particles.state_vector.shape == state_update.state_vector.shape
+    # Check weights are unchanged
+    assert any(new_particles.weight == state_update.weight)
+    # Check that the timestamp is the same
+    assert new_particles.timestamp == state_update.timestamp