Add SMCPHDUpdater

Author: sglvladi

stonesoup/updater/particle.py

@@ -14,6 +14,7 @@
 from ..predictor.particle import MultiModelPredictor, RaoBlackwellisedMultiModelPredictor
 from ..resampler import Resampler
 from ..regulariser import Regulariser
+from ..types.numeric import Probability
 from ..types.prediction import (
     Prediction, ParticleMeasurementPrediction, GaussianStatePrediction, MeasurementPrediction)
 from ..types.update import ParticleStateUpdate, Update
@@ -519,3 +520,100 @@ def _log_space_product(A, B):
         Astack = np.stack([A] * B.shape[1]).transpose(1, 0, 2)
         Bstack = np.stack([B] * A.shape[0]).transpose(0, 2, 1)
         return np.squeeze(logsumexp(Astack + Bstack, axis=2))
+
+
+class SMCPHDUpdater(ParticleUpdater):
+    """ SMC-PHD updater class
+
+    Sequential Monte-Carlo (SMC) PHD updater implementation, based on [1]_ .
+
+    Notes
+    -----
+    - It is assumed that the proposal distribution is the same as the dynamics
+    - Target "spawing" is not implemented
+
+    .. [1] Ba-Ngu Vo, S. Singh and A. Doucet, "Sequential Monte Carlo Implementation of the
+            PHD Filter for Multi-target Tracking," Sixth International Conference of Information
+            Fusion, 2003. Proceedings of the, 2003, pp. 792-799, doi: 10.1109/ICIF.2003.177320.
+    .. [2]  P. Horridge and S. Maskell,  “Using a probabilistic hypothesis density filter to
+            confirm tracks in a multi-target environment,” in 2011 Jahrestagung der Gesellschaft
+            fr Informatik, October 2011.
+    """
+    prob_detect: Probability = Property(
+        default=Probability(0.85),
+        doc="Target Detection Probability")
+    clutter_intensity: float = Property(
+        doc="Average number of clutter measurements per time step, per unit volume")
+    num_samples: int = Property(
+        default=1024,
+        doc="The number of samples. Default is 1024")
+
+    def update(self, multihypothesis, **kwargs):
+        """ SMC-PHD update step
+
+        Parameters
+        ----------
+        multihypothesis : :class:`~.MultipleHypothesis`
+            A container of :class:`~SingleHypothesis` objects. All hypotheses are assumed to have
+            the same prediction (and hence same timestamp).
+
+        Returns
+        -------
+        : :class:`~.ParticleStateUpdate`
+            The state posterior
+        """
+
+        prediction = copy.copy(multihypothesis[0].prediction)
+        detections = [hypothesis.measurement for hypothesis in multihypothesis if hypothesis]
+
+        # Calculate w^{n,i} Eq. (20) of [2]
+        log_weights_per_hyp = self.get_log_weights_per_hypothesis(prediction, detections)
+
+        # Update weights Eq. (8) of [1]
+        # w_k^i = \sum_{z \in Z_k}{w^{n,i}}, where i is the index of z in Z_k
+        log_post_weights = logsumexp(log_weights_per_hyp, axis=1)
+        prediction.log_weight = log_post_weights
+
+        # Resample
+        log_num_targets = logsumexp(log_post_weights)  # N_{k|k}
+        # Normalize weights
+        prediction.log_weight = log_post_weights - log_num_targets
+        if self.resampler is not None:
+            prediction = self.resampler.resample(prediction, self.num_samples)  # Resample
+        # De-normalize
+        prediction.log_weight = prediction.log_weight + log_num_targets
+
+        return Update.from_state(
+            state=multihypothesis[0].prediction,
+            state_vector=prediction.state_vector,
+            log_weight=prediction.log_weight,
+            hypothesis=multihypothesis,
+            timestamp=multihypothesis[0].measurement.timestamp,
+        )
+
+    def get_log_weights_per_hypothesis(self, prediction, detections):
+        num_samples = prediction.state_vector.shape[1]
+
+        # Compute g(z|x) matrix as in [1]
+        g = self._get_measurement_loglikelihoods(prediction, detections)
+
+        # Calculate w^{n,i} Eq. (20) of [2]
+        Ck = self.prob_detect.log() + g + prediction.log_weight[:, np.newaxis]
+        C = logsumexp(Ck, axis=0)
+        k = np.log(self.clutter_intensity)
+        C_plus = np.logaddexp(C, k)
+        log_weights_per_hyp = np.full((num_samples, len(detections) + 1), -np.inf)
+        log_weights_per_hyp[:, 0] = np.log(1 - self.prob_detect) + prediction.log_weight
+        if len(detections):
+            log_weights_per_hyp[:, 1:] = Ck - C_plus
+
+        return log_weights_per_hyp
+
+    def _get_measurement_loglikelihoods(self, prediction, detections):
+        num_samples = prediction.state_vector.shape[1]
+        # Compute g(z|x) matrix as in [1]
+        g = np.zeros((num_samples, len(detections)))
+        for i, detection in enumerate(detections):
+            measurement_model = self._check_measurement_model(detection.measurement_model)
+            g[:, i] = measurement_model.logpdf(detection, prediction, noise=True)
+        return g

stonesoup/updater/tests/test_particle.py

@@ -1,4 +1,6 @@
 """Test for updater.particle module"""
+import itertools
+
 import datetime
 from functools import partial
 
@@ -8,16 +10,18 @@
 from ...models.measurement.linear import LinearGaussian
 from ...resampler.particle import SystematicResampler
 from ...types.array import StateVectors
-from ...types.detection import Detection
+from ...types.detection import Detection, MissedDetection
 from ...types.hypothesis import SingleHypothesis
 from ...types.multihypothesis import MultipleHypothesis
+from ...types.numeric import Probability
 from ...types.particle import Particle
 from ...types.state import ParticleState
 from ...types.prediction import (
     ParticleStatePrediction, ParticleMeasurementPrediction)
 from ...updater.particle import (
     ParticleUpdater, GromovFlowParticleUpdater,
-    GromovFlowKalmanParticleUpdater, BernoulliParticleUpdater)
+    GromovFlowKalmanParticleUpdater, BernoulliParticleUpdater,
+    SMCPHDUpdater)
 from ...predictor.particle import BernoulliParticlePredictor
 from ...models.transition.linear import ConstantVelocity, CombinedLinearGaussianTransitionModel
 from ...types.update import BernoulliParticleStateUpdate
@@ -263,3 +267,32 @@ def test_regularised_particle(transition_model, model_flag):
     assert updated_state.hypothesis.measurement_prediction == measurement_prediction
     assert updated_state.hypothesis.prediction == prediction
     assert updated_state.hypothesis.measurement == measurement
+
+
+def test_smcphd():
+    prob_detect = Probability(.9)  # 90% chance of detection.
+    clutter_intensity = 1e-5
+    num_particles = 9
+    timestamp = datetime.datetime.now()
+
+    particles = [Particle(np.array([[i], [j]]), 1 / num_particles)
+                 for i, j in itertools.product([10., 20., 30.], [10., 20., 30.])]
+    prediction = ParticleStatePrediction(None, particle_list=particles,
+                                         timestamp=timestamp)
+    measurements = [Detection([[i]], timestamp=timestamp) for i in [10., 20., 30.]]
+
+    hypotheses = [SingleHypothesis(prediction, MissedDetection(timestamp=timestamp), None)]
+    hypotheses.extend([SingleHypothesis(prediction, measurement, None)
+                       for measurement in measurements])
+    multihypothesis = MultipleHypothesis(hypotheses)
+
+    measurement_model = LinearGaussian(ndim_state=2, mapping=[0], noise_covar=np.array([[0.04]]))
+    updater = SMCPHDUpdater(measurement_model=measurement_model, resampler=SystematicResampler(),
+                            prob_detect=prob_detect, clutter_intensity=clutter_intensity,
+                            num_samples=num_particles)
+
+    updated_state = updater.update(multihypothesis)
+
+    assert updated_state.timestamp == timestamp
+    assert updated_state.hypothesis == multihypothesis
+    assert np.isclose(float(updated_state.weight.sum()), 3, atol=1e-1)