Regulariser amendment and implementation in ParticleUpdater

stonesoup/predictor/particle.py

@@ -341,6 +341,7 @@ def predict(self, prior, timestamp=None, **kwargs):
             existence_probability=predicted_existence,
             parent=untransitioned_state,
             timestamp=timestamp,
+            transition_model=self.transition_model
         )
         return new_particle_state
 

stonesoup/regulariser/particle.py

@@ -5,6 +5,8 @@
 from .base import Regulariser
 from ..functions import cholesky_eps
 from ..types.state import ParticleState
+from ..models.transition import TransitionModel
+from ..base import Property
 
 
 class MCMCRegulariser(Regulariser):
@@ -14,7 +16,7 @@ class MCMCRegulariser(Regulariser):
     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
+    This is a particular 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
@@ -24,17 +26,19 @@ class MCMCRegulariser(Regulariser):
     ----------
     .. [1] Robert, Christian P. & Casella, George, Monte Carlo Statistical Methods, Springer, 1999.
 
-    .. [2] Ristic, Branco & Arulampalam, Sanjeev & Gordon, Neil, Beyond the Kalman Filter:
+    .. [2] Ristic, Branko & Arulampalam, Sanjeev & Gordon, Neil, Beyond the Kalman Filter:
         Particle Filters for Target Tracking Applications, Artech House, 2004. """
 
+    transition_model: TransitionModel = Property(doc="Transition model used for prediction")
+
     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`
+        prior : :class:`~.ParticleState` type
+            prior particle distribution.
+        posterior : :class:`~.ParticleState` type
             posterior particle distribution
         detections : set of :class:`~.Detection`
             set of detections containing clutter,
@@ -47,13 +51,21 @@ def regularise(self, prior, posterior, detections):
         """
 
         if not isinstance(posterior, ParticleState):
-            posterior = ParticleState(None, particle_list=posterior)
+            raise TypeError('Only ParticleState type is supported!')
 
         if not isinstance(prior, ParticleState):
-            prior = ParticleState(None, particle_list=prior)
+            raise TypeError('Only ParticleState type is supported!')
 
         regularised_particles = copy.copy(posterior)
         moved_particles = copy.copy(posterior)
+        transitioned_prior = copy.copy(prior)
+
+        if self.transition_model is not None:
+            time_interval = posterior.timestamp - prior.timestamp
+            new_state_vector = self.transition_model.function(prior,
+                                                              noise=False,
+                                                              time_interval=time_interval)
+            transitioned_prior.state_vector = new_state_vector
 
         if detections is not None:
             ndim = prior.state_vector.shape[0]
@@ -70,13 +82,11 @@ def regularise(self, prior, posterior, detections):
                 hopt * cholesky_eps(covar_est) @ np.random.randn(ndim, nparticles)
 
             # Evaluate likelihoods
-            part_diff = moved_particles.state_vector - prior.state_vector
-            part_diff_mean = np.average(part_diff, axis=1)
-            move_likelihood = multivariate_normal.logpdf((part_diff - part_diff_mean).T,
+            part_diff = moved_particles.state_vector - transitioned_prior.state_vector
+            move_likelihood = multivariate_normal.logpdf(part_diff.T,
                                                          cov=covar_est)
-            post_part_diff = posterior.state_vector - prior.state_vector
-            post_part_diff_mean = np.average(post_part_diff, axis=1)
-            post_likelihood = multivariate_normal.logpdf((post_part_diff - post_part_diff_mean).T,
+            post_part_diff = posterior.state_vector - transitioned_prior.state_vector
+            post_likelihood = multivariate_normal.logpdf(post_part_diff.T,
                                                          cov=covar_est)
 
             # Evaluate measurement likelihoods

stonesoup/regulariser/tests/test_particle.py

@@ -1,17 +1,21 @@
 import numpy as np
 import datetime
+import pytest
 
 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 ...models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity
 from ...types.detection import Detection
 from ...types.update import ParticleStateUpdate
 from ..particle import MCMCRegulariser
 
 
 def test_regulariser():
+    transition_model = CombinedLinearGaussianTransitionModel([ConstantVelocity([0.05])])
+
     particles = ParticleState(state_vector=None, particle_list=[Particle(np.array([[10], [10]]),
                                                                          1 / 9),
                                                                 Particle(np.array([[10], [20]]),
@@ -32,34 +36,43 @@ def test_regulariser():
                                                                          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)
+    new_state_vector = transition_model.function(particles,
+                                                 noise=True,
+                                                 time_interval=datetime.timedelta(seconds=1))
+    prediction = ParticleStatePrediction(new_state_vector,
+                                         timestamp=timestamp,
+                                         transition_model=transition_model)
+    meas_pred = ParticleMeasurementPrediction(prediction, 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()
+                                       particle_list=particles.particle_list,
+                                       timestamp=timestamp+datetime.timedelta(seconds=1))
+    regulariser = MCMCRegulariser(transition_model=transition_model)
 
     # state check
-    new_particles = regulariser.regularise(particles, state_update, measurement)
+    new_particles = regulariser.regularise(prediction, 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)
+    # list check3
+    with pytest.raises(TypeError) as e:
+        new_particles = regulariser.regularise(particles.particle_list,
+                                               state_update,
+                                               measurement)
+    assert "Only ParticleState type is supported!" in str(e.value)
+    with pytest.raises(Exception) as e:
+        new_particles = regulariser.regularise(particles,
+                                               state_update.particle_list,
+                                               measurement)
+    assert "Only ParticleState type is supported!" in str(e.value)
 
 
 def test_no_measurement():
@@ -90,7 +103,7 @@ def test_no_measurement():
                                                               measurement=None,
                                                               measurement_prediction=meas_pred),
                                        particle_list=particles.particle_list, timestamp=timestamp)
-    regulariser = MCMCRegulariser()
+    regulariser = MCMCRegulariser(transition_model=None)
 
     new_particles = regulariser.regularise(particles, state_update, detections=None)
 

stonesoup/updater/particle.py

@@ -65,6 +65,11 @@ def update(self, hypothesis, **kwargs):
         if self.resampler is not None:
             predicted_state = self.resampler.resample(predicted_state)
 
+        if self.regulariser is not None:
+            predicted_state = self.regulariser.regularise(predicted_state.parent,
+                                                          predicted_state,
+                                                          [hypothesis.measurement])
+
         return Update.from_state(
             state=hypothesis.prediction,
             state_vector=predicted_state.state_vector,

stonesoup/updater/tests/test_particle.py

@@ -12,13 +12,14 @@
 from ...types.hypothesis import SingleHypothesis
 from ...types.multihypothesis import MultipleHypothesis
 from ...types.particle import Particle
+from ...types.state import ParticleState
 from ...types.prediction import (
     ParticleStatePrediction, ParticleMeasurementPrediction)
 from ...updater.particle import (
     ParticleUpdater, GromovFlowParticleUpdater,
     GromovFlowKalmanParticleUpdater, BernoulliParticleUpdater)
 from ...predictor.particle import BernoulliParticlePredictor
-from ...models.transition.linear import ConstantVelocity
+from ...models.transition.linear import ConstantVelocity, CombinedLinearGaussianTransitionModel
 from ...types.update import BernoulliParticleStateUpdate
 from ...regulariser.particle import MCMCRegulariser
 from ...sampler.particle import ParticleSampler
@@ -139,7 +140,7 @@ def test_bernoulli_particle():
 
     prediction = predictor.predict(prior, timestamp=new_timestamp)
     resampler = SystematicResampler()
-    regulariser = MCMCRegulariser()
+    regulariser = MCMCRegulariser(transition_model=cv)
 
     updater = BernoulliParticleUpdater(measurement_model=None,
                                        resampler=resampler,
@@ -170,3 +171,56 @@ def test_bernoulli_particle():
     assert update.hypothesis == hypotheses
     # Check that the existence probability is returned
     assert update.existence_probability is not None
+
+
+def test_regularised_particle():
+
+    transition_model = CombinedLinearGaussianTransitionModel([ConstantVelocity([0.05])])
+    measurement_model = LinearGaussian(
+        ndim_state=2, mapping=[0], noise_covar=np.array([[10]]))
+
+    updater = ParticleUpdater(regulariser=MCMCRegulariser(transition_model=transition_model),
+                              measurement_model=measurement_model)
+    # Measurement model
+    timestamp = datetime.datetime.now()
+    particles = [Particle([[10], [10]], 1 / 9),
+                 Particle([[10], [20]], 1 / 9),
+                 Particle([[10], [30]], 1 / 9),
+                 Particle([[20], [10]], 1 / 9),
+                 Particle([[20], [20]], 1 / 9),
+                 Particle([[20], [30]], 1 / 9),
+                 Particle([[30], [10]], 1 / 9),
+                 Particle([[30], [20]], 1 / 9),
+                 Particle([[30], [30]], 1 / 9),
+                 ]
+
+    particles = ParticleState(None, particle_list=particles, timestamp=timestamp)
+    predicted_state = transition_model.function(particles,
+                                                noise=True,
+                                                time_interval=datetime.timedelta(seconds=1))
+    prediction = ParticleStatePrediction(predicted_state,
+                                         weight=np.array([1/9]*9),
+                                         timestamp=timestamp,
+                                         transition_model=transition_model,
+                                         parent=particles)
+
+    measurement = Detection([[40.0]], timestamp=timestamp, measurement_model=measurement_model)
+    eval_measurement_prediction = ParticleMeasurementPrediction(
+        StateVectors([prediction.state_vector[0, :]]), timestamp=timestamp)
+
+    measurement_prediction = updater.predict_measurement(prediction)
+
+    assert np.all(eval_measurement_prediction.state_vector == measurement_prediction.state_vector)
+    assert measurement_prediction.timestamp == timestamp
+
+    updated_state = updater.update(SingleHypothesis(
+        prediction, measurement, measurement_prediction))
+
+    # Don't know what the particles will exactly be due to randomness so check
+    # some obvious properties
+
+    assert np.all(weight == 1 / 9 for weight in updated_state.weight)
+    assert updated_state.timestamp == timestamp
+    assert updated_state.hypothesis.measurement_prediction == measurement_prediction
+    assert updated_state.hypothesis.prediction == prediction
+    assert updated_state.hypothesis.measurement == measurement