Add Nonparametric PDA and gate out non valid measurements

stonesoup/dataassociator/tests/conftest.py

@@ -33,7 +33,8 @@ def updater(measurement_model):
 def probability_hypothesiser(predictor, updater):
     return PDAHypothesiser(predictor, updater,
                            clutter_spatial_density=1.2e-2,
-                           prob_detect=0.9, prob_gate=0.99)
+                           prob_detect=0.9, prob_gate=0.99,
+                           include_all=True)
 
 
 @pytest.fixture()

stonesoup/gater/tests/test_distance.py

@@ -41,7 +41,8 @@ def test_distance(predictor, updater, detections, gate_threshold, num_gated):
 
     timestamp = datetime.datetime.now()
 
-    hypothesiser = PDAHypothesiser(predictor, updater, clutter_spatial_density=0.000001)
+    hypothesiser = PDAHypothesiser(
+        predictor, updater, clutter_spatial_density=0.000001, include_all=True)
     gater = DistanceGater(hypothesiser, measure=measure, gate_threshold=gate_threshold)
 
     track = Track([GaussianStateUpdate(

stonesoup/hypothesiser/probability.py

@@ -1,7 +1,13 @@
-from scipy.stats import multivariate_normal as mn
+from functools import lru_cache
+
+from scipy.stats import multivariate_normal, chi2
+from scipy.linalg import det
+from scipy.special import gamma
+import numpy as np
 
 from .base import Hypothesiser
 from ..base import Property
+from ..measures import SquaredMahalanobis
 from ..types.detection import MissedDetection
 from ..types.hypothesis import SingleProbabilityHypothesis
 from ..types.multihypothesis import MultipleHypothesis
@@ -21,14 +27,27 @@ class PDAHypothesiser(Hypothesiser):
     predictor: Predictor = Property(doc="Predict tracks to detection times")
     updater: Updater = Property(doc="Updater used to get measurement prediction")
     clutter_spatial_density: float = Property(
-        doc="Spatial density of clutter - tied to probability of false detection")
+        default=None,
+        doc="Spatial density of clutter - tied to probability of false detection. Default is None "
+            "where the clutter spatial density is calculated based on assumption that "
+            "all but one measurement within the validation region of the track are clutter.")
     prob_detect: Probability = Property(
         default=Probability(0.85),
         doc="Target Detection Probability")
     prob_gate: Probability = Property(
         default=Probability(0.95),
         doc="Gate Probability - prob. gate contains true measurement "
             "if detected")
+    include_all: bool = Property(
+        default=False,
+        doc="If `True`, hypotheses outside probability gates will be returned. This requires "
+            "that the clutter spatial density is also provided, as it may not be possible to"
+            "estimate this. Default `False`")
+
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+        if self.include_all and self.clutter_spatial_density is None:
+            raise ValueError("Must provide clutter spatial density if including all hypotheses")
 
     def hypothesise(self, track, detections, timestamp, **kwargs):
         r"""Evaluate and return all track association hypotheses.
@@ -104,6 +123,8 @@ def hypothesise(self, track, detections, timestamp, **kwargs):
         """
 
         hypotheses = list()
+        validated_measurements = 0
+        measure = SquaredMahalanobis(state_covar_inv_cache_size=None)
 
         # Common state & measurement prediction
         prediction = self.predictor.predict(track, timestamp=timestamp, **kwargs)
@@ -126,18 +147,48 @@ def hypothesise(self, track, detections, timestamp, **kwargs):
                 prediction, detection.measurement_model, **kwargs)
             # Calculate difference before to handle custom types (mean defaults to zero)
             # This is required as log pdf coverts arrays to floats
-            log_pdf = mn.logpdf(
+            log_pdf = multivariate_normal.logpdf(
                 (detection.state_vector - measurement_prediction.state_vector).ravel(),
                 cov=measurement_prediction.covar)
             pdf = Probability(log_pdf, log_value=True)
-            probability = (pdf * self.prob_detect)/self.clutter_spatial_density
 
-            # True detection hypothesis
-            hypotheses.append(
-                SingleProbabilityHypothesis(
-                    prediction,
-                    detection,
-                    probability,
-                    measurement_prediction))
+            if measure(measurement_prediction, detection) \
+                    <= self._gate_threshold(self.prob_gate, measurement_prediction.ndim):
+                validated_measurements += 1
+                valid_measurement = True
+            else:
+                # Will be gated out unless include_all is set
+                valid_measurement = False
+
+            if self.include_all or valid_measurement:
+                probability = pdf * self.prob_detect
+                if self.clutter_spatial_density is not None:
+                    probability /= self.clutter_spatial_density
+
+                # True detection hypothesis
+                hypotheses.append(
+                    SingleProbabilityHypothesis(
+                        prediction,
+                        detection,
+                        probability,
+                        measurement_prediction))
+
+        if self.clutter_spatial_density is None:
+            for hypothesis in hypotheses[1:]:  # Skip missed detection
+                hypothesis.probability *= self._validation_region_volume(
+                    self.prob_gate, hypothesis.measurement_prediction) / validated_measurements
 
         return MultipleHypothesis(hypotheses, normalise=True, total_weight=1)
+
+    @classmethod
+    @lru_cache()
+    def _validation_region_volume(cls, prob_gate, meas_pred):
+        n = meas_pred.ndim
+        gate_threshold = cls._gate_threshold(prob_gate, n)
+        c_z = np.pi**(n/2) / gamma(n/2 + 1)
+        return c_z * gate_threshold**(n/2) * np.sqrt(det(meas_pred.covar))
+
+    @staticmethod
+    @lru_cache()
+    def _gate_threshold(prob_gate, n):
+        return chi2.ppf(float(prob_gate), n)

stonesoup/hypothesiser/tests/test_composite.py

@@ -25,11 +25,11 @@ def make_categorical_measurement_model(ndim_state, ndim_meas):
 def test_composite(predictor, updater, dummy_category_predictor, dummy_category_updater):
     sub_hypothesisers = [
         PDAHypothesiser(predictor, updater, clutter_spatial_density=1.2e-2, prob_detect=0.9,
-                        prob_gate=0.99),
+                        prob_gate=0.99, include_all=True),
         HMMHypothesiser(dummy_category_predictor, dummy_category_updater,
                         prob_detect=0.7, prob_gate=0.95),
         PDAHypothesiser(predictor, updater, clutter_spatial_density=1.4e-2, prob_detect=0.5,
-                        prob_gate=0.98),
+                        prob_gate=0.98, include_all=True),
         HMMHypothesiser(dummy_category_predictor, dummy_category_updater,
                         prob_detect=0.8, prob_gate=0.97)
     ]

stonesoup/hypothesiser/tests/test_probability.py

@@ -1,6 +1,7 @@
 import datetime
 
 import numpy as np
+import pytest
 
 from ..probability import PDAHypothesiser
 from ...types.detection import Detection, MissedDetection
@@ -24,20 +25,60 @@ def test_pda(predictor, updater):
     mulltihypothesis = \
         hypothesiser.hypothesise(track, detections, timestamp)
 
-    # There are 3 weighted hypotheses - Detections 1 and 2, MissedDectection
+    # There are 2 weighted hypotheses - Detections 1 and MissedDetection
+    # Detection 2 is not a "valid measurement" and gated out
+    assert len(mulltihypothesis) == 2
+
+    # Each hypothesis has a probability/weight attribute
+    assert all(hypothesis.probability >= 0 and isinstance(hypothesis.probability, Probability)
+               for hypothesis in mulltihypothesis)
+
+    #  Detection 1 and MissedDetection are present
+    assert detection1 in {hypothesis.measurement for hypothesis in mulltihypothesis}
+    assert any(isinstance(hypothesis.measurement, MissedDetection)
+               for hypothesis in mulltihypothesis)
+
+    hypothesiser = PDAHypothesiser(predictor, updater,
+                                   clutter_spatial_density=1.2e-2,
+                                   prob_detect=0.9, prob_gate=0.99,
+                                   include_all=True)
+
+    mulltihypothesis = \
+        hypothesiser.hypothesise(track, detections, timestamp)
+
+    # There are 3 weighted hypotheses - Detections 1 and 2, MissedDetection
     assert len(mulltihypothesis) == 3
 
     # Each hypothesis has a probability/weight attribute
-    assert all(hypothesis.probability >= 0 and
-               isinstance(hypothesis.probability, Probability)
-               for hypothesis in
-               mulltihypothesis)
+    assert all(hypothesis.probability >= 0 and isinstance(hypothesis.probability, Probability)
+               for hypothesis in mulltihypothesis)
 
     #  Detection 1, 2 and MissedDetection are present
-    assert any(hypothesis.measurement is detection1 for hypothesis in
-               mulltihypothesis)
-    assert any(hypothesis.measurement is detection2 for hypothesis in
-               mulltihypothesis)
+    assert {detection1, detection2} < {hypothesis.measurement for hypothesis in mulltihypothesis}
+    assert any(isinstance(hypothesis.measurement, MissedDetection)
+               for hypothesis in mulltihypothesis)
+
+    hypothesiser = PDAHypothesiser(predictor, updater,
+                                   prob_detect=0.9, prob_gate=0.99)
+
+    mulltihypothesis = \
+        hypothesiser.hypothesise(track, detections, timestamp)
+
+    # There are 2 weighted hypotheses - Detections 1 and MissedDetection
+    # Detection 2 is not a "valid measurement" and gated out
+    assert len(mulltihypothesis) == 2
+
+    # Each hypothesis has a probability/weight attribute
+    assert all(hypothesis.probability >= 0 and isinstance(hypothesis.probability, Probability)
+               for hypothesis in mulltihypothesis)
+
+    #  Detection 1 and MissedDetection are present
+    assert detection1 in {hypothesis.measurement for hypothesis in mulltihypothesis}
     assert any(isinstance(hypothesis.measurement, MissedDetection)
-               for hypothesis in
-               mulltihypothesis)
+               for hypothesis in mulltihypothesis)
+
+
+def test_invalid_pda_arguments(predictor, updater):
+    with pytest.raises(ValueError):
+        PDAHypothesiser(predictor, updater,
+                        prob_detect=0.9, prob_gate=0.99, include_all=True)