Merge pull request #719 from mmrbpf

Add Multi-model and Rao-Blackwellised versions of particle filter

LICENSE

@@ -5,6 +5,7 @@ MIT License
 © Copyright 2018-2023 University of Liverpool UK
 © Copyright 2020-2023 Fraunhofer FKIE
 © Copyright 2020-2023 John Hiles
+© Copyright 2020-2023 Riskaware Ltd
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal

stonesoup/predictor/particle.py

@@ -1,7 +1,13 @@
+import copy
+from typing import Sequence
+
+import numpy as np
+
 from .base import Predictor
 from ._utils import predict_lru_cache
 from .kalman import KalmanPredictor, ExtendedKalmanPredictor
 from ..base import Property
+from ..models.transition import TransitionModel
 from ..types.prediction import Prediction
 from ..types.state import GaussianState
 
@@ -91,3 +97,142 @@ def predict(self, prior, *args, **kwargs):
                                      timestamp=particle_prediction.timestamp,
                                      fixed_covar=kalman_prediction.covar, particle_list=None,
                                      transition_model=self.transition_model)
+
+
+class MultiModelPredictor(Predictor):
+    """MultiModelPredictor class
+
+    An implementation of a Particle Filter predictor utilising multiple models.
+    """
+    transition_model = None
+    transition_models: Sequence[TransitionModel] = Property(
+        doc="Transition models used to for particle transition, selected by model index on "
+            "particle. Models dimensions can be subset of the overall state space, by "
+            "using :attr:`model_mappings`."
+    )
+    transition_matrix: np.ndarray = Property(
+        doc="n-model by n-model transition matrix."
+    )
+    model_mappings: Sequence[Sequence[int]] = Property(
+        doc="Sequence of mappings associated with each transition model. This enables mapping "
+            "between model and state space, enabling use of models that may have different "
+            "dimensions (e.g. velocity or acceleration). Parts of the state that aren't mapped "
+            "are set to zero.")
+
+    @property
+    def probabilities(self):
+        return np.cumsum(self.transition_matrix, axis=1)
+
+    @predict_lru_cache()
+    def predict(self, prior, timestamp=None, **kwargs):
+        """Particle Filter prediction step
+
+        Parameters
+        ----------
+        prior : :class:`~.MultiModelParticleState`
+            A prior state object
+        timestamp: :class:`datetime.datetime`, optional
+            A timestamp signifying when the prediction is performed
+            (the default is `None`)
+        Returns
+        -------
+        : :class:`~.ParticleStatePrediction`
+            The predicted state
+        """
+
+        new_particle_state = Prediction.from_state(
+            prior,
+            state_vector=copy.copy(prior.state_vector),
+            parent=prior,
+            particle_list=None,
+            dynamic_model=copy.copy(prior.dynamic_model),
+            timestamp=timestamp)
+
+        for model_index, transition_model in enumerate(self.transition_models):
+            # Change the value of the dynamic value randomly according to the defined
+            # transition matrix
+            current_model_indices = prior.dynamic_model == model_index
+            current_model_count = np.count_nonzero(current_model_indices)
+            if current_model_count == 0:
+                continue
+            new_dynamic_models = np.searchsorted(
+                self.probabilities[model_index],
+                np.random.random(size=current_model_count))
+
+            new_state_vector = self.apply_model(
+                prior[current_model_indices], transition_model, timestamp,
+                self.model_mappings[model_index], noise=True, **kwargs)
+
+            new_particle_state.state_vector[:, current_model_indices] = new_state_vector
+            new_particle_state.dynamic_model[current_model_indices] = new_dynamic_models
+
+        return new_particle_state
+
+    @staticmethod
+    def apply_model(prior, transition_model, timestamp, model_mapping, **kwargs):
+        # Based on given position mapping create a new state vector that contains only the
+        # required states
+        orig_ndim = prior.ndim
+        prior.state_vector = prior.state_vector[model_mapping, :]
+        new_state_vector = transition_model.function(
+            prior, time_interval=timestamp - prior.timestamp, **kwargs)
+
+        # Calculate the indices removed from the state vector to become compatible with the
+        # dynamic model
+        for j in range(orig_ndim):
+            if j not in model_mapping:
+                new_state_vector = np.insert(new_state_vector, j, 0, axis=0)
+
+        return new_state_vector
+
+
+class RaoBlackwellisedMultiModelPredictor(MultiModelPredictor):
+    """Rao-Blackwellised Multi Model Predictor class
+
+    An implementation of a Particle Filter predictor utilising multiple models, with per
+    particle model probabilities.
+    """
+
+    @predict_lru_cache()
+    def predict(self, prior, timestamp=None, **kwargs):
+        """Particle Filter prediction step
+
+        Parameters
+        ----------
+        prior : :class:`~.RaoBlackwellisedParticleState`
+            A prior state object
+        timestamp: :class:`datetime.datetime`, optional
+            A timestamp signifying when the prediction is performed
+            (the default is `None`)
+        Returns
+        -------
+        : :class:`~.ParticleStatePrediction`
+            The predicted state
+        """
+
+        new_particle_state = Prediction.from_state(
+            prior,
+            state_vector=copy.copy(prior.state_vector),
+            parent=prior,
+            particle_list=None,
+            timestamp=timestamp)
+
+        # Change the value of the dynamic value randomly according to the
+        # matrix
+        new_dynamic_models = np.argmax(
+            prior.model_probabilities.astype(float).cumsum(0) > np.random.rand(len(prior)),
+            axis=0)
+
+        for model_index, transition_model in enumerate(self.transition_models):
+
+            new_model_indices = new_dynamic_models == model_index
+            if np.count_nonzero(new_model_indices) == 0:
+                continue
+
+            new_state_vector = self.apply_model(
+                prior[new_model_indices], transition_model, timestamp,
+                self.model_mappings[model_index], noise=True, **kwargs)
+
+            new_particle_state.state_vector[:, new_model_indices] = new_state_vector
+
+        return new_particle_state

stonesoup/predictor/tests/test_multimodel.py

@@ -0,0 +1,92 @@
+import datetime
+
+import numpy as np
+
+from ...models.transition.linear import (
+    ConstantVelocity, CombinedLinearGaussianTransitionModel, ConstantAcceleration)
+from ...predictor.particle import MultiModelPredictor
+from ...types.array import StateVectors
+from ...types.state import MultiModelParticleState
+
+
+def test_multi_model():
+
+    # Define time related variables.
+    timestamp = datetime.datetime.now()
+    time_diff = 2  # 2sec.
+    new_timestamp = timestamp + datetime.timedelta(seconds=time_diff)
+
+    # Define prior state.
+    prior_vectors = np.full((9, 10), 10.).view(StateVectors)
+    # Change particles starting x position, and velocity
+    prior_vectors[0, :] = [10, 10, 10, 20, 20, 20, 30, 30, 30, 10]
+    prior_vectors[1, :] = [10, 20, 30, 10, 20, 30, 10, 20, 30, 30]
+    weight = np.full((prior_vectors.shape[1],), 1/prior_vectors.shape[1])
+    model = np.full((prior_vectors.shape[1], ), 0)
+    prior = MultiModelParticleState(
+        prior_vectors, weight=weight, dynamic_model=model, timestamp=timestamp)
+
+    # Declare the model list.
+    model_list = [
+                  CombinedLinearGaussianTransitionModel((ConstantVelocity(0.1),
+                                                         ConstantVelocity(0.1),
+                                                         ConstantVelocity(0.1))),
+                  CombinedLinearGaussianTransitionModel((ConstantAcceleration(0.01),
+                                                         ConstantAcceleration(0.01),
+                                                         ConstantAcceleration(0.01))),
+                  CombinedLinearGaussianTransitionModel((ConstantVelocity(0.01),
+                                                         ConstantVelocity(0.01),
+                                                         ConstantVelocity(0.01))),
+                  CombinedLinearGaussianTransitionModel((ConstantAcceleration(0.01),
+                                                         ConstantAcceleration(0.01),
+                                                         ConstantAcceleration(0.01))),
+                  CombinedLinearGaussianTransitionModel((ConstantVelocity(0.01),
+                                                         ConstantVelocity(0.01),
+                                                         ConstantAcceleration(0.01))),
+                  CombinedLinearGaussianTransitionModel((ConstantAcceleration(0.01),
+                                                         ConstantVelocity(0.01),
+                                                         ConstantVelocity(0.01))),
+                  CombinedLinearGaussianTransitionModel((ConstantVelocity(0.01),
+                                                         ConstantAcceleration(0.01),
+                                                         ConstantVelocity(0.01))),
+                  CombinedLinearGaussianTransitionModel((ConstantAcceleration(0.01),
+                                                         ConstantAcceleration(0.01),
+                                                         ConstantVelocity(0.01))),
+                  ]
+    # Give the respective position mapping.
+    model_mappings = [
+                      [0, 1, 3, 4, 6, 7],
+                      [0, 1, 2, 3, 4, 5, 6, 7, 8],
+                      [0, 1, 3, 4, 6, 7],
+                      [0, 1, 2, 3, 4, 5, 6, 7, 8],
+                      [0, 1, 3, 4, 6, 7, 8],
+                      [0, 1, 2, 3, 4, 6, 7],
+                      [0, 1, 3, 4, 5, 6, 7],
+                      [0, 1, 2, 3, 4, 5, 6, 7]
+                     ]
+
+    # Provide the required transition matrix.
+    transition = [
+                  [0.65, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05],
+                  [0.05, 0.65, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05],
+                  [0.05, 0.05, 0.65, 0.05, 0.05, 0.05, 0.05, 0.05],
+                  [0.05, 0.05, 0.05, 0.65, 0.05, 0.05, 0.05, 0.05],
+                  [0.05, 0.05, 0.05, 0.05, 0.65, 0.05, 0.05, 0.05],
+                  [0.05, 0.05, 0.05, 0.05, 0.05, 0.65, 0.05, 0.05],
+                  [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.65, 0.05],
+                  [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.65],
+                 ]
+
+    predictor = MultiModelPredictor(model_mappings=model_mappings,
+                                    transition_matrix=transition,
+                                    transition_models=model_list)
+
+    prediction = predictor.predict(prior, timestamp=new_timestamp)
+
+    dynamic_model_list = [p.dynamic_model for p in prediction.particles]
+    dynamic_model_proportions = [dynamic_model_list.count(i) for i in range(len(transition))]
+    dynamic_model_proportions = np.array(dynamic_model_proportions)
+
+    assert prediction.timestamp == new_timestamp
+    assert np.all([prediction.particles[i].weight == 1/10 for i in range(9)])
+    assert len(dynamic_model_proportions) == len(transition)

stonesoup/resampler/particle.py

@@ -41,13 +41,9 @@ def resample(self, particles):
         # that pushed the score over the current value
         u_j = u_i + (1 / n_particles) * np.arange(n_particles)
         index = weight_order[np.searchsorted(cdf, np.log(u_j))]
-        new_particles = ParticleState(state_vector=particles.state_vector[:, index],
-                                      weight=[weight]*n_particles,
-                                      parent=ParticleState(
-                                          state_vector=particles.state_vector[:, index],
-                                          weight=particles.weight[index],
-                                          timestamp=particles.timestamp),
-                                      timestamp=particles.timestamp)
+
+        new_particles = particles[index]
+        new_particles.weight = np.full((n_particles, ), weight)
         return new_particles
 
 

stonesoup/types/particle.py

@@ -1,3 +1,5 @@
+from typing import Sequence
+
 from ..base import Property
 from .array import StateVector
 from .base import Type
@@ -13,13 +15,35 @@ class Particle(Type):
     weight: float = Property(doc='Weight of particle')
     parent: 'Particle' = Property(default=None, doc='Parent particle')
 
-    def __init__(self, state_vector, weight, parent=None, *args, **kwargs):
-        if parent:
-            parent.parent = None
-        if state_vector is not None and not isinstance(state_vector, StateVector):
-            state_vector = StateVector(state_vector)
-        super().__init__(state_vector, weight, parent, *args, **kwargs)
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+        if self.parent:
+            self.parent.parent = None
+        if self.state_vector is not None and not isinstance(self.state_vector, StateVector):
+            self.state_vector = StateVector(self.state_vector)
 
     @property
     def ndim(self):
         return self.state_vector.shape[0]
+
+
+class MultiModelParticle(Particle):
+    """
+    Particle type
+
+    A MultiModelParticle type which contains a state, weight and the dynamic_model
+    """
+    dynamic_model: int = Property(doc='Assigned dynamic model')
+    parent: 'MultiModelParticle' = Property(default=None, doc='Parent particle')
+
+
+class RaoBlackwellisedParticle(Particle):
+    """
+    Particle type
+
+    A RaoBlackwellisedParticle type which contains a state, weight, dynamic_model and
+    associated model probabilities
+    """
+    model_probabilities: Sequence[float] = Property(
+        doc="The dynamic probabilities of changing models")
+    parent: 'RaoBlackwellisedParticle' = Property(default=None, doc='Parent particle')

stonesoup/types/prediction.py

@@ -3,7 +3,8 @@
 
 from .array import CovarianceMatrix
 from .base import Type
-from .state import (State, GaussianState, ParticleState, EnsembleState,
+from .state import (State, GaussianState, EnsembleState,
+                    ParticleState, MultiModelParticleState, RaoBlackwellisedParticleState,
                     SqrtGaussianState, InformationState, TaggedWeightedGaussianState,
                     WeightedGaussianState, CategoricalState, ASDGaussianState)
 from ..base import Property
@@ -135,6 +136,20 @@ class ParticleMeasurementPrediction(MeasurementPrediction, ParticleState):
     """
 
 
+class MultiModelParticleStatePrediction(Prediction, MultiModelParticleState):
+    """MultiModelParticleStatePrediction type
+
+    This is a simple multi-model Particle state prediction object.
+    """
+
+
+class RaoBlackwellisedParticleStatePrediction(Prediction, RaoBlackwellisedParticleState):
+    """RaoBlackwellisedParticleStatePrediction type
+
+    This is a simple Rao Blackwellised Particle state prediction object.
+    """
+
+
 class EnsembleStatePrediction(Prediction, EnsembleState):
     """EnsembleStatePrediction type