From f807f44d52607b4071dc4550b142d4584720a96f Mon Sep 17 00:00:00 2001
From: jordi <jmbarr@dstl.gov.uk>
Date: Fri, 27 May 2022 22:18:02 +0100
Subject: [PATCH 1/7] First attempt at a PDAUpdater class.

---
 stonesoup/updater/probability.py | 100 +++++++++++++++++++++++++++++++
 1 file changed, 100 insertions(+)
 create mode 100644 stonesoup/updater/probability.py

diff --git a/stonesoup/updater/probability.py b/stonesoup/updater/probability.py
new file mode 100644
index 000000000..a9cfade6a
--- /dev/null
+++ b/stonesoup/updater/probability.py
@@ -0,0 +1,100 @@
+# -*- coding: utf-8 -*-
+import warnings
+import numpy as np
+import scipy.linalg as la
+from functools import lru_cache
+
+from .kalman import ExtendedKalmanUpdater
+
+
+class PDAUpdater(ExtendedKalmanUpdater):
+    r"""An updater which undertakes probabilistic data association (PDA), as defined in [1]. It
+    differs slightly from the Kalman updater it inherits from in that instead of a single
+    hypothesis object, the :meth:`update` method it takes a hypotheses object returned by a
+    :class:`~.PDA` (or similar) data associator. Functionally this is a list of single hypothesis
+    objects which group tracks together with associated measuments and probabilities.
+
+    The :class:`~.ExtendedKalmanUpdater` is used in order to inherit the ability to cope with
+    (slight) non-linearities.
+
+    The update step proceeds as:
+
+    .. math::
+
+        \mathbf{x}_{k|k} = \mathbf{x}_{k|k-1} + K_k \mathbf{y}_{k}
+
+        P_{k|k} = \beta_0 P_{k|k-1} + (1 - \beta_0) P_^C_{k|k} + \tilde{P}
+
+    where :math:`K_k` and :math:`P^C_{k|k} are the Kalman gain and posterior covariance
+    respectively returned by the single-target Kalman update, :math:`\beta_0` is the probability
+    of missed detection. In this instance :math:`\mathbf{y}_k` is the {\em combined} innovation,
+    over :math:`m_k` detections:
+
+    .. math::
+
+        \mathbf{y}_k = \Sigma_{i=1}^{m_k} \beta_i \mathbf{y}_{k,i}.
+
+    The posterior covariance is composed of a term to account for the covariance due to missed
+    detection, that due to the true detection, and a term (:math:`\tilde{P}`) which quantifies the
+    effect of the measurement origin uncertainty.
+
+    .. math::
+
+        \tilde{P} \def K_k \[ \Sigma_{i=1}^{m_k} \beta_i \mathbf{y}_{k,i} \beta_i
+        \mathbf{y}_{k,i}^T - \mathbf{y}_k \mathbf{y}_k^T \] K_k^T
+
+    """
+
+    def update(self, hypotheses, **kwargs):
+        r"""Of n hypotheses there should be 1 prediction (a missed detection) and n-1 different
+        measurement associations. Because we don't know where in the list this occurs, but we do
+        know that there's only one, we iterate til we find it, then start the calculation from
+        there, then return to the values we missed."""
+
+        position_prediction = False  # flag to record whether we have the prediction
+        for n, hypothesis in enumerate(hypotheses):
+
+            # Check for the existence of an associated measurement. Because of the way the
+            # hypothesis is constructed, you can do this:
+            if not hypothesis:
+
+                # Predict the measurement and attach it to the hypothesis
+                hypothesis.measurement_prediction = self.predict_measurement(hypothesis.prediction)
+                # Now get P_k|k
+                posterior_covariance, kalman_gain = self._posterior_covariance(hypothesis)
+
+                # Add the weighted prediction to the weighted posterior
+                posterior_covariance = hypothesis.probability * hypothesis.prediction.covar + \
+                                       (1 - hypothesis.probability) * posterior_covariance
+
+                posterior_mean = hypothesis.prediction.state_vector
+                total_innovation = 0 * hypothesis.measurement_prediction.state_vector
+                weighted_innovation_cov = 0 * total_innovation @ total_innovation.T
+
+                # record the position in the list of the prediction
+                position_prediction = n
+            else:
+                if not position_prediction:
+                    continue
+                else:
+                    innovation = hypothesis.measurement.state_vector - hypothesis.measurement_prediction.state_vector
+                    total_innovation += hypothesis.probability * innovation
+                    weighted_innovation_cov += hypothesis.probability * innovation @ innovation.T
+
+        # and then return to do those elements on the list that weren't covered prior to the prediction.
+        for n, hypothesis in enumerate(hypotheses):
+            if n == position_prediction:
+                break
+            else:
+                innovation = hypothesis.measurement.state_vector - hypothesis.measurement_prediction.state_vector
+                total_innovation += hypothesis.probability * innovation
+                weighted_innovation_cov += hypothesis.probability * innovation @ innovation.T
+
+        posterior_mean += kalman_gain @ total_innovation
+        posterior_covariance += \
+            kalman_gain @ (weighted_innovation_cov - total_innovation @ total_innovation.T) @ kalman_gain.T
+
+        return Update.from_state(
+            hypotheses[0].prediction,
+            posterior_mean, posterior_covariance,
+            timestamp=hypotheses[0].measurement.timestamp, hypothesis=hypotheses[0])
\ No newline at end of file

From ee56f9a2896bec4aa6636a696222dcb47425e290 Mon Sep 17 00:00:00 2001
From: jordi <jmbarr@dstl.gov.uk>
Date: Tue, 14 Jun 2022 22:52:41 +0100
Subject: [PATCH 2/7] Separated into two ways of doing the PDA update in
 advance of testing which works best

---
 docs/source/stonesoup.updater.rst |   7 +-
 stonesoup/updater/probability.py  | 121 +++++++++++++++++++++---------
 2 files changed, 90 insertions(+), 38 deletions(-)

diff --git a/docs/source/stonesoup.updater.rst b/docs/source/stonesoup.updater.rst
index 742000a9e..5b3e24a14 100644
--- a/docs/source/stonesoup.updater.rst
+++ b/docs/source/stonesoup.updater.rst
@@ -68,7 +68,7 @@ Categorical
     :show-inheritance:
 
 Composite
------------
+---------
 
 .. automodule:: stonesoup.updater.composite
     :show-inheritance:
@@ -77,4 +77,9 @@ Chernoff
 --------
 
 .. automodule:: stonesoup.updater.chernoff
+
+Probabilistic
+-------------
+
+.. automodule:: stonesoup.updater.probability
     :show-inheritance:
diff --git a/stonesoup/updater/probability.py b/stonesoup/updater/probability.py
index a9cfade6a..a63ae7bd1 100644
--- a/stonesoup/updater/probability.py
+++ b/stonesoup/updater/probability.py
@@ -5,17 +5,20 @@
 from functools import lru_cache
 
 from .kalman import ExtendedKalmanUpdater
+from ..types.update import Update
+from ..types.array import StateVectors
+from ..functions import gm_reduce_single
 
 
 class PDAUpdater(ExtendedKalmanUpdater):
     r"""An updater which undertakes probabilistic data association (PDA), as defined in [1]. It
     differs slightly from the Kalman updater it inherits from in that instead of a single
-    hypothesis object, the :meth:`update` method it takes a hypotheses object returned by a
+    hypothesis object, the :meth:`update` method takes a hypotheses object returned by a
     :class:`~.PDA` (or similar) data associator. Functionally this is a list of single hypothesis
     objects which group tracks together with associated measuments and probabilities.
 
     The :class:`~.ExtendedKalmanUpdater` is used in order to inherit the ability to cope with
-    (slight) non-linearities.
+    (slight) non-linearities. Other inheritance structures should be trivial to implement.
 
     The update step proceeds as:
 
@@ -45,56 +48,100 @@ class PDAUpdater(ExtendedKalmanUpdater):
 
     """
 
-    def update(self, hypotheses, **kwargs):
-        r"""Of n hypotheses there should be 1 prediction (a missed detection) and n-1 different
+    def update(self, hypotheses, gm_method=False, **kwargs):
+        r"""The update step.
+
+        Parameters
+        ----------
+        hypotheses : :class:`~.MultipleHypothesis`
+            the prediction-measurement association hypotheses. This hypotheses object carries
+            tracks, associated sets of measurements for each track together with a probability
+            measure which enumerates the likelihood of each track-measurement pair. (This is most
+            likely output by the :class:`~.PDA` associator).
+
+            In a single case (the missed detection hypothesis), the hypothesis will not have an
+            associated measurement or measurement prediction.
+            latter case a predicted measurement will be calculated.
+        gm_method : bool
+
+        **kwargs : various
+            These are passed to :meth:`predict_measurement`
+
+        Returns
+        -------
+        : :class:`GaussianMeasurementPrediction`
+            The measurement prediction, :math:`\mathbf{z}_{k|k-1}`
+
+        """
+        if gm_method:
+            posterior_mean, posterior_covariance = self._update_via_GM_reduction(hypotheses,**kwargs)
+        else:
+            posterior_mean, posterior_covariance = self._update_via_innovation(hypotheses, **kwargs)
+
+        # Note that this does not check if all hypotheses are of the same type.
+        # It also assumes that all measurements have the same timestamp (updates are
+        # contemporaneous).
+        return Update.from_state(
+            hypotheses[0].prediction,
+            posterior_mean, posterior_covariance,
+            timestamp=hypotheses[0].measurement.timestamp, hypothesis=hypotheses)
+
+
+    def _update_via_GM_reduction(self, hypotheses, **kwargs):
+        """This method delivers the same as :meth:`update()` by way of a slightly different
+        calculation. It's also more intuitive."""
+
+        posterior_states = []
+        posterior_state_weights = []
+        for hypothesis in hypotheses:
+            if not hypothesis:
+                posterior_states.append(hypothesis.prediction)
+            else:
+                posterior_state = super().update(hypothesis, **kwargs)  # Use the EKF update
+                posterior_states.append(posterior_state)
+            posterior_state_weights.append(hypothesis.probability)
+
+        means = StateVectors([state.state_vector for state in posterior_states])
+        covars = np.stack([state.covar for state in posterior_states], axis=2)
+        weights = np.asarray(posterior_state_weights)
+
+        # Reduce mixture of states to one posterior estimate Gaussian.
+        post_mean, post_covar = gm_reduce_single(means, covars, weights)
+
+        return post_mean, post_covar
+
+    def _update_via_innovation(self, hypotheses, **kwargs):
+        """Of n hypotheses there should be 1 prediction (a missed detection) and n-1 different
         measurement associations. Because we don't know where in the list this occurs, but we do
-        know that there's only one, we iterate til we find it, then start the calculation from
-        there, then return to the values we missed."""
+        know that there's only one, we iterate til we find it, then start the calculation of the
+        posterior mean and covariance from there, then return to the values we missed."""
 
         position_prediction = False  # flag to record whether we have the prediction
         for n, hypothesis in enumerate(hypotheses):
-
             # Check for the existence of an associated measurement. Because of the way the
             # hypothesis is constructed, you can do this:
             if not hypothesis:
-
-                # Predict the measurement and attach it to the hypothesis
                 hypothesis.measurement_prediction = self.predict_measurement(hypothesis.prediction)
-                # Now get P_k|k
+                innovation = hypothesis.measurement_prediction.state_vector - \
+                             hypothesis.measurement_prediction.state_vector  # is zero in this case
                 posterior_covariance, kalman_gain = self._posterior_covariance(hypothesis)
-
                 # Add the weighted prediction to the weighted posterior
                 posterior_covariance = hypothesis.probability * hypothesis.prediction.covar + \
                                        (1 - hypothesis.probability) * posterior_covariance
-
                 posterior_mean = hypothesis.prediction.state_vector
-                total_innovation = 0 * hypothesis.measurement_prediction.state_vector
-                weighted_innovation_cov = 0 * total_innovation @ total_innovation.T
-
-                # record the position in the list of the prediction
-                position_prediction = n
-            else:
-                if not position_prediction:
-                    continue
-                else:
-                    innovation = hypothesis.measurement.state_vector - hypothesis.measurement_prediction.state_vector
-                    total_innovation += hypothesis.probability * innovation
-                    weighted_innovation_cov += hypothesis.probability * innovation @ innovation.T
-
-        # and then return to do those elements on the list that weren't covered prior to the prediction.
-        for n, hypothesis in enumerate(hypotheses):
-            if n == position_prediction:
-                break
             else:
                 innovation = hypothesis.measurement.state_vector - hypothesis.measurement_prediction.state_vector
-                total_innovation += hypothesis.probability * innovation
-                weighted_innovation_cov += hypothesis.probability * innovation @ innovation.T
 
-        posterior_mean += kalman_gain @ total_innovation
+            if n == 0:  # probably exists a less clunky way of doing this
+                sum_of_innovations = hypothesis.probability * innovation
+                sum_of_weighted_cov = hypothesis.probability * innovation @ innovation.T
+            else:
+                sum_of_innovations += hypothesis.probability * innovation
+                sum_of_weighted_cov += hypothesis.probability * innovation @ innovation.T
+
+        posterior_mean += kalman_gain @ sum_of_innovations
         posterior_covariance += \
-            kalman_gain @ (weighted_innovation_cov - total_innovation @ total_innovation.T) @ kalman_gain.T
+                kalman_gain @ (sum_of_weighted_cov - sum_of_innovations @ sum_of_innovations.T) \
+                @ kalman_gain.T
 
-        return Update.from_state(
-            hypotheses[0].prediction,
-            posterior_mean, posterior_covariance,
-            timestamp=hypotheses[0].measurement.timestamp, hypothesis=hypotheses[0])
\ No newline at end of file
+        return posterior_mean, posterior_covariance
\ No newline at end of file

From 9e33529d62e122926c82d9af4f5742befac3cd1f Mon Sep 17 00:00:00 2001
From: jordi <jmbarr@dstl.gov.uk>
Date: Fri, 17 Jun 2022 14:02:44 +0100
Subject: [PATCH 3/7] Tidied up PDAUpdater into two (competing) update methods,
 for comparison.

---
 stonesoup/updater/probability.py | 72 ++++++++++++++++++++++++++------
 1 file changed, 60 insertions(+), 12 deletions(-)

diff --git a/stonesoup/updater/probability.py b/stonesoup/updater/probability.py
index a63ae7bd1..ba8358442 100644
--- a/stonesoup/updater/probability.py
+++ b/stonesoup/updater/probability.py
@@ -63,7 +63,8 @@ def update(self, hypotheses, gm_method=False, **kwargs):
             associated measurement or measurement prediction.
             latter case a predicted measurement will be calculated.
         gm_method : bool
-
+            either use the innovation-based update methods (detailed above and in [1]), if False,
+            or use the GM-reduction (True).
         **kwargs : various
             These are passed to :meth:`predict_measurement`
 
@@ -86,10 +87,37 @@ def update(self, hypotheses, gm_method=False, **kwargs):
             posterior_mean, posterior_covariance,
             timestamp=hypotheses[0].measurement.timestamp, hypothesis=hypotheses)
 
-
     def _update_via_GM_reduction(self, hypotheses, **kwargs):
-        """This method delivers the same as :meth:`update()` by way of a slightly different
-        calculation. It's also more intuitive."""
+        """This method delivers the same outcome as what's described above. It's slightly
+        different, but potentially more intuitive.
+
+        Here, each the hypotheses, including missed detection, are updated and then a weighted
+        Gaussian reduction is used to resolve the hypotheses to a single Gaussian distribution.
+
+        The reason this is equivalent is:
+            TBA
+
+        Parameters
+        ----------
+        hypotheses : :class:`~.MultipleHypothesis`
+            the prediction-measurement association hypotheses. This hypotheses object carries
+            tracks, associated sets of measurements for each track together with a probability
+            measure which enumerates the likelihood of each track-measurement pair. (This is most
+            likely output by the :class:`~.PDA` associator).
+
+            In a single case (the missed detection hypothesis), the hypothesis will not have an
+            associated measurement or measurement prediction.
+            latter case a predicted measurement will be calculated.
+         **kwargs : various
+            These are passed to :class:`~.ExtendedKalmanUpdater`:meth:`update`
+
+        Returns
+        -------
+        : :class:`~.StateVector`
+            The mean of the reduced/single Gaussian
+        : :class:`~.CovarianceMatrix`
+            The covariance of the reduced/single Gaussian
+        """
 
         posterior_states = []
         posterior_state_weights = []
@@ -111,17 +139,37 @@ def _update_via_GM_reduction(self, hypotheses, **kwargs):
         return post_mean, post_covar
 
     def _update_via_innovation(self, hypotheses, **kwargs):
-        """Of n hypotheses there should be 1 prediction (a missed detection) and n-1 different
-        measurement associations. Because we don't know where in the list this occurs, but we do
-        know that there's only one, we iterate til we find it, then start the calculation of the
-        posterior mean and covariance from there, then return to the values we missed."""
+        """Of n hypotheses there should be 1 prediction (a missed detection hypothesis) and n-1
+        different measurement associations. The update proceeds as described above.
+
+        Parameters
+        ----------
+        hypotheses : :class:`~.MultipleHypothesis`
+            the prediction-measurement association hypotheses. This hypotheses object carries
+            tracks, associated sets of measurements for each track together with a probability
+            measure which enumerates the likelihood of each track-measurement pair. (This is most
+            likely output by the :class:`~.PDA` associator).
+
+            In a single case (the missed detection hypothesis), the hypothesis will not have an
+            associated measurement or measurement prediction.
+            latter case a predicted measurement will be calculated.
+         **kwargs : various
+            These are passed to :meth:`predict_measurement`
+
+        Returns
+        -------
+        : :class:`~.StateVector`
+            The mean of the reduced/single Gaussian
+        : :class:`~.CovarianceMatrix`
+            The covariance of the reduced/single Gaussian
+        """
 
-        position_prediction = False  # flag to record whether we have the prediction
         for n, hypothesis in enumerate(hypotheses):
             # Check for the existence of an associated measurement. Because of the way the
             # hypothesis is constructed, you can do this:
             if not hypothesis:
-                hypothesis.measurement_prediction = self.predict_measurement(hypothesis.prediction)
+                hypothesis.measurement_prediction = self.predict_measurement(hypothesis.prediction,
+                                                                             **kwargs)
                 innovation = hypothesis.measurement_prediction.state_vector - \
                              hypothesis.measurement_prediction.state_vector  # is zero in this case
                 posterior_covariance, kalman_gain = self._posterior_covariance(hypothesis)
@@ -132,7 +180,7 @@ def _update_via_innovation(self, hypotheses, **kwargs):
             else:
                 innovation = hypothesis.measurement.state_vector - hypothesis.measurement_prediction.state_vector
 
-            if n == 0:  # probably exists a less clunky way of doing this
+            if n == 0:  # probably exists a less clunky way of doing this using exists() or overwritten +=
                 sum_of_innovations = hypothesis.probability * innovation
                 sum_of_weighted_cov = hypothesis.probability * innovation @ innovation.T
             else:
@@ -144,4 +192,4 @@ def _update_via_innovation(self, hypotheses, **kwargs):
                 kalman_gain @ (sum_of_weighted_cov - sum_of_innovations @ sum_of_innovations.T) \
                 @ kalman_gain.T
 
-        return posterior_mean, posterior_covariance
\ No newline at end of file
+        return posterior_mean, posterior_covariance

From 155341e806c2704ee2b32af7ab176ec42a821159 Mon Sep 17 00:00:00 2001
From: jordi <jmbarr@dstl.gov.uk>
Date: Thu, 23 Jun 2022 21:38:26 +0100
Subject: [PATCH 4/7] Beginning PDAUpdater tests.

---
 stonesoup/updater/probability.py            |  1 -
 stonesoup/updater/tests/test_probability.py | 26 +++++++++++++++++++++
 2 files changed, 26 insertions(+), 1 deletion(-)
 create mode 100644 stonesoup/updater/tests/test_probability.py

diff --git a/stonesoup/updater/probability.py b/stonesoup/updater/probability.py
index ba8358442..b23944626 100644
--- a/stonesoup/updater/probability.py
+++ b/stonesoup/updater/probability.py
@@ -47,7 +47,6 @@ class PDAUpdater(ExtendedKalmanUpdater):
         \mathbf{y}_{k,i}^T - \mathbf{y}_k \mathbf{y}_k^T \] K_k^T
 
     """
-
     def update(self, hypotheses, gm_method=False, **kwargs):
         r"""The update step.
 
diff --git a/stonesoup/updater/tests/test_probability.py b/stonesoup/updater/tests/test_probability.py
new file mode 100644
index 000000000..e34f82cb6
--- /dev/null
+++ b/stonesoup/updater/tests/test_probability.py
@@ -0,0 +1,26 @@
+
+
+
+def test_pda(UpdaterClass, measurement_model, prediction, measurement):
+
+    timestamp = datetime.datetime.now()
+    track = Track([GaussianState(np.array([[0]]), np.array([[1]]), timestamp)])
+    detection1 = Detection(np.array([[2]]))
+    detection2 = Detection(np.array([[8]]))
+    detections = {detection1, detection2}
+
+    hypothesiser = PDAHypothesiser(predictor, updater,
+                                   clutter_spatial_density=1.2e-2,
+                                   prob_detect=0.9, prob_gate=0.99)
+
+    mulltihypothesis = \
+        hypothesiser.hypothesise(track, detections, timestamp)
+
+    data_associator = PDA(hypothesiser=hypothesiser)
+
+    hypotheses = data_associator.associate({track}, detections,
+                                           start_time + timedelta(seconds=1))
+
+    posterior_state = pdaupdater.update(hypotheses, gm_method=True)
+
+

From 6d2fd181bfaaeeb1d0c20d6189190fd90b4a7682 Mon Sep 17 00:00:00 2001
From: jordi <jmbarr@dstl.gov.uk>
Date: Fri, 24 Jun 2022 23:15:16 +0100
Subject: [PATCH 5/7] PDAUpdater documentation

---
 stonesoup/updater/probability.py | 46 +++++++++++++-------------------
 1 file changed, 18 insertions(+), 28 deletions(-)

diff --git a/stonesoup/updater/probability.py b/stonesoup/updater/probability.py
index b23944626..4d9900da7 100644
--- a/stonesoup/updater/probability.py
+++ b/stonesoup/updater/probability.py
@@ -11,7 +11,7 @@
 
 
 class PDAUpdater(ExtendedKalmanUpdater):
-    r"""An updater which undertakes probabilistic data association (PDA), as defined in [1]. It
+    r"""An updater which undertakes probabilistic data association (PDA), as defined in [#]_. It
     differs slightly from the Kalman updater it inherits from in that instead of a single
     hypothesis object, the :meth:`update` method takes a hypotheses object returned by a
     :class:`~.PDA` (or similar) data associator. Functionally this is a list of single hypothesis
@@ -24,13 +24,13 @@ class PDAUpdater(ExtendedKalmanUpdater):
 
     .. math::
 
-        \mathbf{x}_{k|k} = \mathbf{x}_{k|k-1} + K_k \mathbf{y}_{k}
+        \mathbf{x}_{k|k} &= \mathbf{x}_{k|k-1} + K_k \mathbf{y}_k
 
-        P_{k|k} = \beta_0 P_{k|k-1} + (1 - \beta_0) P_^C_{k|k} + \tilde{P}
+        P_{k|k} &= \beta_0 P_{k|k-1} + (1 - \beta_0) P_{k|k} + \tilde{P}
 
-    where :math:`K_k` and :math:`P^C_{k|k} are the Kalman gain and posterior covariance
+    where :math:`K_k` and :math:`P_{k|k}` are the Kalman gain and posterior covariance
     respectively returned by the single-target Kalman update, :math:`\beta_0` is the probability
-    of missed detection. In this instance :math:`\mathbf{y}_k` is the {\em combined} innovation,
+    of missed detection. In this instance :math:`\mathbf{y}_k` is the *combined* innovation,
     over :math:`m_k` detections:
 
     .. math::
@@ -43,9 +43,14 @@ class PDAUpdater(ExtendedKalmanUpdater):
 
     .. math::
 
-        \tilde{P} \def K_k \[ \Sigma_{i=1}^{m_k} \beta_i \mathbf{y}_{k,i} \beta_i
-        \mathbf{y}_{k,i}^T - \mathbf{y}_k \mathbf{y}_k^T \] K_k^T
+        \tilde{P} \triangleq K_k [ \Sigma_{i=1}^{m_k} \beta_i \mathbf{y}_{k,i}\mathbf{y}_{k,i}^T -
+        \mathbf{y}_k \mathbf{y}_k^T ] K_k^T
 
+
+    References
+    ----------
+    .. [#] Bar-Shalom Y, Daum F, Huang F 2009, The Probabilistic Data Association Filter, IEEE
+           Control Systems Magazine
     """
     def update(self, hypotheses, gm_method=False, **kwargs):
         r"""The update step.
@@ -53,24 +58,23 @@ def update(self, hypotheses, gm_method=False, **kwargs):
         Parameters
         ----------
         hypotheses : :class:`~.MultipleHypothesis`
-            the prediction-measurement association hypotheses. This hypotheses object carries
+            The prediction-measurement association hypotheses. This hypotheses object carries
             tracks, associated sets of measurements for each track together with a probability
             measure which enumerates the likelihood of each track-measurement pair. (This is most
             likely output by the :class:`~.PDA` associator).
 
             In a single case (the missed detection hypothesis), the hypothesis will not have an
             associated measurement or measurement prediction.
-            latter case a predicted measurement will be calculated.
         gm_method : bool
-            either use the innovation-based update methods (detailed above and in [1]), if False,
+            Either use the innovation-based update methods (detailed above and in [1]), if False,
             or use the GM-reduction (True).
         **kwargs : various
             These are passed to :meth:`predict_measurement`
 
         Returns
         -------
-        : :class:`GaussianMeasurementPrediction`
-            The measurement prediction, :math:`\mathbf{z}_{k|k-1}`
+        : :class:`GaussianUpdate`
+            The update, :math:`\mathbf{x}_{k|k}, P_{k|k}`
 
         """
         if gm_method:
@@ -99,14 +103,7 @@ def _update_via_GM_reduction(self, hypotheses, **kwargs):
         Parameters
         ----------
         hypotheses : :class:`~.MultipleHypothesis`
-            the prediction-measurement association hypotheses. This hypotheses object carries
-            tracks, associated sets of measurements for each track together with a probability
-            measure which enumerates the likelihood of each track-measurement pair. (This is most
-            likely output by the :class:`~.PDA` associator).
-
-            In a single case (the missed detection hypothesis), the hypothesis will not have an
-            associated measurement or measurement prediction.
-            latter case a predicted measurement will be calculated.
+            As in :meth:`update` method
          **kwargs : various
             These are passed to :class:`~.ExtendedKalmanUpdater`:meth:`update`
 
@@ -144,14 +141,7 @@ def _update_via_innovation(self, hypotheses, **kwargs):
         Parameters
         ----------
         hypotheses : :class:`~.MultipleHypothesis`
-            the prediction-measurement association hypotheses. This hypotheses object carries
-            tracks, associated sets of measurements for each track together with a probability
-            measure which enumerates the likelihood of each track-measurement pair. (This is most
-            likely output by the :class:`~.PDA` associator).
-
-            In a single case (the missed detection hypothesis), the hypothesis will not have an
-            associated measurement or measurement prediction.
-            latter case a predicted measurement will be calculated.
+            As in :meth:`update` method
          **kwargs : various
             These are passed to :meth:`predict_measurement`
 

From 7b4e8c16cf3be03689b976cebdfd81e4668259b9 Mon Sep 17 00:00:00 2001
From: jordi <jmbarr@dstl.gov.uk>
Date: Tue, 19 Dec 2023 09:36:28 +0000
Subject: [PATCH 6/7] PDAUpdater tests and documentation fixes

---
 stonesoup/updater/probability.py            | 41 +++++++-----
 stonesoup/updater/tests/test_probability.py | 69 +++++++++++++++++----
 2 files changed, 81 insertions(+), 29 deletions(-)

diff --git a/stonesoup/updater/probability.py b/stonesoup/updater/probability.py
index 4d9900da7..1af85964c 100644
--- a/stonesoup/updater/probability.py
+++ b/stonesoup/updater/probability.py
@@ -46,11 +46,16 @@ class PDAUpdater(ExtendedKalmanUpdater):
         \tilde{P} \triangleq K_k [ \Sigma_{i=1}^{m_k} \beta_i \mathbf{y}_{k,i}\mathbf{y}_{k,i}^T -
         \mathbf{y}_k \mathbf{y}_k^T ] K_k^T
 
+    A method for updating via a Gaussian mixture reduction is also provided. In this latter case,
+    each of the hypotheses, including that for a missed detection, is updated and then a weighted
+    Gaussian reduction is used to resolve the hypotheses to a single Gaussian distribution. The
+    reason this is equivalent to the innovation-based method is shown in [#]_.
 
     References
     ----------
     .. [#] Bar-Shalom Y, Daum F, Huang F 2009, The Probabilistic Data Association Filter, IEEE
            Control Systems Magazine
+    .. [#] https://gist.github.com/jmbarr/92dc83e28c04026136d4f8706a1159c1
     """
     def update(self, hypotheses, gm_method=False, **kwargs):
         r"""The update step.
@@ -66,8 +71,7 @@ def update(self, hypotheses, gm_method=False, **kwargs):
             In a single case (the missed detection hypothesis), the hypothesis will not have an
             associated measurement or measurement prediction.
         gm_method : bool
-            Either use the innovation-based update methods (detailed above and in [1]), if False,
-            or use the GM-reduction (True).
+            Use the innovation-based update method if False (default), or the GM-reduction (True).
         **kwargs : various
             These are passed to :meth:`predict_measurement`
 
@@ -78,9 +82,11 @@ def update(self, hypotheses, gm_method=False, **kwargs):
 
         """
         if gm_method:
-            posterior_mean, posterior_covariance = self._update_via_GM_reduction(hypotheses,**kwargs)
+            posterior_mean, posterior_covariance = self._update_via_GM_reduction(hypotheses,
+                                                                                 **kwargs)
         else:
-            posterior_mean, posterior_covariance = self._update_via_innovation(hypotheses, **kwargs)
+            posterior_mean, posterior_covariance = self._update_via_innovation(hypotheses,
+                                                                               **kwargs)
 
         # Note that this does not check if all hypotheses are of the same type.
         # It also assumes that all measurements have the same timestamp (updates are
@@ -94,11 +100,10 @@ def _update_via_GM_reduction(self, hypotheses, **kwargs):
         """This method delivers the same outcome as what's described above. It's slightly
         different, but potentially more intuitive.
 
-        Here, each the hypotheses, including missed detection, are updated and then a weighted
+        Here, each of the hypotheses, including missed detection, are updated and then a weighted
         Gaussian reduction is used to resolve the hypotheses to a single Gaussian distribution.
 
-        The reason this is equivalent is:
-            TBA
+        The reason this is equivalent is shown in _[#]
 
         Parameters
         ----------
@@ -157,28 +162,30 @@ def _update_via_innovation(self, hypotheses, **kwargs):
             # Check for the existence of an associated measurement. Because of the way the
             # hypothesis is constructed, you can do this:
             if not hypothesis:
-                hypothesis.measurement_prediction = self.predict_measurement(hypothesis.prediction,
-                                                                             **kwargs)
+                hypothesis.measurement_prediction = self.predict_measurement(
+                    hypothesis.prediction, **kwargs)
                 innovation = hypothesis.measurement_prediction.state_vector - \
-                             hypothesis.measurement_prediction.state_vector  # is zero in this case
+                    hypothesis.measurement_prediction.state_vector  # is zero in this case
                 posterior_covariance, kalman_gain = self._posterior_covariance(hypothesis)
                 # Add the weighted prediction to the weighted posterior
                 posterior_covariance = hypothesis.probability * hypothesis.prediction.covar + \
-                                       (1 - hypothesis.probability) * posterior_covariance
+                    (1 - hypothesis.probability) * posterior_covariance
                 posterior_mean = hypothesis.prediction.state_vector
             else:
-                innovation = hypothesis.measurement.state_vector - hypothesis.measurement_prediction.state_vector
+                innovation = hypothesis.measurement.state_vector - \
+                             hypothesis.measurement_prediction.state_vector
 
-            if n == 0:  # probably exists a less clunky way of doing this using exists() or overwritten +=
+            # probably exists a less clunky way of doing this using exists() or overwritten +=
+            if n == 0:
                 sum_of_innovations = hypothesis.probability * innovation
-                sum_of_weighted_cov = hypothesis.probability * innovation @ innovation.T
+                sum_of_weighted_cov = hypothesis.probability * (innovation @ innovation.T)
             else:
                 sum_of_innovations += hypothesis.probability * innovation
-                sum_of_weighted_cov += hypothesis.probability * innovation @ innovation.T
+                sum_of_weighted_cov += hypothesis.probability * (innovation @ innovation.T)
 
         posterior_mean += kalman_gain @ sum_of_innovations
         posterior_covariance += \
-                kalman_gain @ (sum_of_weighted_cov - sum_of_innovations @ sum_of_innovations.T) \
-                @ kalman_gain.T
+            kalman_gain @ (sum_of_weighted_cov - sum_of_innovations @ sum_of_innovations.T) \
+            @ kalman_gain.T
 
         return posterior_mean, posterior_covariance
diff --git a/stonesoup/updater/tests/test_probability.py b/stonesoup/updater/tests/test_probability.py
index e34f82cb6..0e56c4dc5 100644
--- a/stonesoup/updater/tests/test_probability.py
+++ b/stonesoup/updater/tests/test_probability.py
@@ -1,26 +1,71 @@
+"""Test for updater.kalman module"""
 
+import pytest
+import numpy as np
 
+from datetime import datetime, timedelta
 
-def test_pda(UpdaterClass, measurement_model, prediction, measurement):
+from stonesoup.models.transition.linear import ConstantVelocity
+from stonesoup.models.measurement.linear import LinearGaussian
+from stonesoup.types.detection import Detection
+from stonesoup.types.track import Track
+from stonesoup.predictor.kalman import ExtendedKalmanPredictor
+from stonesoup.updater.kalman import ExtendedKalmanUpdater
+from stonesoup.hypothesiser.probability import PDAHypothesiser
+from stonesoup.dataassociator.probability import PDA
+from stonesoup.types.prediction import (
+    GaussianStatePrediction, GaussianMeasurementPrediction)
+from stonesoup.types.state import GaussianState, SqrtGaussianState
+from stonesoup.updater.probability import PDAUpdater
 
-    timestamp = datetime.datetime.now()
-    track = Track([GaussianState(np.array([[0]]), np.array([[1]]), timestamp)])
-    detection1 = Detection(np.array([[2]]))
-    detection2 = Detection(np.array([[8]]))
+
+'''@pytest.mark.parametrize(
+    "UpdaterClass, measurement_model, prediction, measurement, predictor, updater",
+    [
+        (   # Schmidt Kalman
+            PDAUpdater,
+            LinearGaussian(ndim_state=2, mapping=[0],
+                           noise_covar=np.array([[0.04]])),
+            GaussianStatePrediction(np.array([[-6.45], [0.7]]),
+                                    np.array([[4.1123, 0.0013],
+                                              [0.0013, 0.0365]])),
+            Detection(np.array([[-6.23]])),
+            ExtendedKalmanPredictor,
+            ExtendedKalmanUpdater
+        ),
+    ],
+    ids=["pdaupdater"]
+)'''
+#def test_pda(UpdaterClass, measurement_model, prediction, measurement, predictor, updater):
+def test_pda():
+    start_time = datetime.now()
+    track = Track([GaussianState(np.array([[-6.45], [0.7]]),
+                                 np.array([[4.1123, 0.0013], [0.0013, 0.0365]]), start_time)])
+    detection1 = Detection(np.array([[-5]]), timestamp= start_time+ timedelta(seconds=1))
+    detection2 = Detection(np.array([[-8]]), timestamp= start_time+ timedelta(seconds=1))
     detections = {detection1, detection2}
 
-    hypothesiser = PDAHypothesiser(predictor, updater,
+    transition_model = ConstantVelocity(0.005)
+    measurement_model = LinearGaussian(ndim_state=2, mapping=[0], noise_covar=np.array([[0.04]]))
+
+    predictor = ExtendedKalmanPredictor(transition_model)
+    updater = ExtendedKalmanUpdater(measurement_model)
+
+    hypothesiser = PDAHypothesiser(predictor=predictor, updater=updater,
                                    clutter_spatial_density=1.2e-2,
-                                   prob_detect=0.9, prob_gate=0.99)
+                                   prob_detect=0.9)
 
-    mulltihypothesis = \
-        hypothesiser.hypothesise(track, detections, timestamp)
+    #   multihypothesis = \
+    #       hypothesiser.hypothesise(track, detections, timestamp)
 
     data_associator = PDA(hypothesiser=hypothesiser)
 
-    hypotheses = data_associator.associate({track}, detections,
-                                           start_time + timedelta(seconds=1))
+    hypotheses = data_associator.associate({track}, detections, start_time + timedelta(seconds=1))
+    hypotheses = hypotheses[track]
+
+    pdaupdater = PDAUpdater(measurement_model)
 
-    posterior_state = pdaupdater.update(hypotheses, gm_method=True)
+    posterior_state = pdaupdater.update(hypotheses, gm_method=False)
+    print(posterior_state)
 
 

From e2edd269f89c669e90af0a8008409bffaa7525b0 Mon Sep 17 00:00:00 2001
From: jordi <jmbarr@dstl.gov.uk>
Date: Tue, 19 Dec 2023 23:08:37 +0000
Subject: [PATCH 7/7] Completed PDAUpdater and tests

---
 stonesoup/updater/probability.py            | 21 +++++------
 stonesoup/updater/tests/test_probability.py | 40 +++++----------------
 2 files changed, 19 insertions(+), 42 deletions(-)

diff --git a/stonesoup/updater/probability.py b/stonesoup/updater/probability.py
index 1af85964c..c471e514a 100644
--- a/stonesoup/updater/probability.py
+++ b/stonesoup/updater/probability.py
@@ -1,8 +1,6 @@
 # -*- coding: utf-8 -*-
-import warnings
 import numpy as np
-import scipy.linalg as la
-from functools import lru_cache
+from copy import copy
 
 from .kalman import ExtendedKalmanUpdater
 from ..types.update import Update
@@ -168,20 +166,23 @@ def _update_via_innovation(self, hypotheses, **kwargs):
                     hypothesis.measurement_prediction.state_vector  # is zero in this case
                 posterior_covariance, kalman_gain = self._posterior_covariance(hypothesis)
                 # Add the weighted prediction to the weighted posterior
-                posterior_covariance = hypothesis.probability * hypothesis.prediction.covar + \
-                    (1 - hypothesis.probability) * posterior_covariance
-                posterior_mean = hypothesis.prediction.state_vector
+                posterior_covariance = float(hypothesis.probability) * \
+                    hypothesis.prediction.covar + (1 - float(hypothesis.probability)) * \
+                    posterior_covariance
+                posterior_mean = copy(hypothesis.prediction.state_vector)
             else:
                 innovation = hypothesis.measurement.state_vector - \
                              hypothesis.measurement_prediction.state_vector
 
             # probably exists a less clunky way of doing this using exists() or overwritten +=
+            # All these floats should be redundant if/when the bug in Probability.__mult__() is
+            # fixed.
             if n == 0:
-                sum_of_innovations = hypothesis.probability * innovation
-                sum_of_weighted_cov = hypothesis.probability * (innovation @ innovation.T)
+                sum_of_innovations = float(hypothesis.probability) * innovation
+                sum_of_weighted_cov = float(hypothesis.probability) * (innovation @ innovation.T)
             else:
-                sum_of_innovations += hypothesis.probability * innovation
-                sum_of_weighted_cov += hypothesis.probability * (innovation @ innovation.T)
+                sum_of_innovations += float(hypothesis.probability) * innovation
+                sum_of_weighted_cov += float(hypothesis.probability) * (innovation @ innovation.T)
 
         posterior_mean += kalman_gain @ sum_of_innovations
         posterior_covariance += \
diff --git a/stonesoup/updater/tests/test_probability.py b/stonesoup/updater/tests/test_probability.py
index 0e56c4dc5..5f5f2f4aa 100644
--- a/stonesoup/updater/tests/test_probability.py
+++ b/stonesoup/updater/tests/test_probability.py
@@ -1,6 +1,5 @@
 """Test for updater.kalman module"""
 
-import pytest
 import numpy as np
 
 from datetime import datetime, timedelta
@@ -13,36 +12,16 @@
 from stonesoup.updater.kalman import ExtendedKalmanUpdater
 from stonesoup.hypothesiser.probability import PDAHypothesiser
 from stonesoup.dataassociator.probability import PDA
-from stonesoup.types.prediction import (
-    GaussianStatePrediction, GaussianMeasurementPrediction)
-from stonesoup.types.state import GaussianState, SqrtGaussianState
+from stonesoup.types.state import GaussianState
 from stonesoup.updater.probability import PDAUpdater
 
 
-'''@pytest.mark.parametrize(
-    "UpdaterClass, measurement_model, prediction, measurement, predictor, updater",
-    [
-        (   # Schmidt Kalman
-            PDAUpdater,
-            LinearGaussian(ndim_state=2, mapping=[0],
-                           noise_covar=np.array([[0.04]])),
-            GaussianStatePrediction(np.array([[-6.45], [0.7]]),
-                                    np.array([[4.1123, 0.0013],
-                                              [0.0013, 0.0365]])),
-            Detection(np.array([[-6.23]])),
-            ExtendedKalmanPredictor,
-            ExtendedKalmanUpdater
-        ),
-    ],
-    ids=["pdaupdater"]
-)'''
-#def test_pda(UpdaterClass, measurement_model, prediction, measurement, predictor, updater):
 def test_pda():
     start_time = datetime.now()
     track = Track([GaussianState(np.array([[-6.45], [0.7]]),
-                                 np.array([[4.1123, 0.0013], [0.0013, 0.0365]]), start_time)])
-    detection1 = Detection(np.array([[-5]]), timestamp= start_time+ timedelta(seconds=1))
-    detection2 = Detection(np.array([[-8]]), timestamp= start_time+ timedelta(seconds=1))
+                                 np.array([[0.41123, 0.0013], [0.0013, 0.0365]]), start_time)])
+    detection1 = Detection(np.array([[-6]]), timestamp=start_time + timedelta(seconds=1))
+    detection2 = Detection(np.array([[-5]]), timestamp=start_time + timedelta(seconds=1))
     detections = {detection1, detection2}
 
     transition_model = ConstantVelocity(0.005)
@@ -55,9 +34,6 @@ def test_pda():
                                    clutter_spatial_density=1.2e-2,
                                    prob_detect=0.9)
 
-    #   multihypothesis = \
-    #       hypothesiser.hypothesise(track, detections, timestamp)
-
     data_associator = PDA(hypothesiser=hypothesiser)
 
     hypotheses = data_associator.associate({track}, detections, start_time + timedelta(seconds=1))
@@ -65,7 +41,7 @@ def test_pda():
 
     pdaupdater = PDAUpdater(measurement_model)
 
-    posterior_state = pdaupdater.update(hypotheses, gm_method=False)
-    print(posterior_state)
-
-
+    posterior_state = pdaupdater.update(hypotheses, gm_method=True)
+    posterior_state2 = pdaupdater.update(hypotheses, gm_method=False)
+    assert np.allclose(posterior_state.state_vector, posterior_state2.state_vector)
+    assert np.allclose(posterior_state.covar, posterior_state2.covar)