Merge pull request #1014 from dstl/akkf

Adaptive kernel Kalman filter (AKKF)

Author: sdhiscocks

docs/source/stonesoup.kernel.rst

@@ -0,0 +1,4 @@
+Kernel
+======
+
+.. automodule:: stonesoup.kernel

docs/source/stonesoup.predictor.rst

@@ -19,6 +19,12 @@ Particle
 .. automodule:: stonesoup.predictor.particle
     :show-inheritance:
 
+Kernel
+------
+
+.. automodule:: stonesoup.predictor.kernel
+    :show-inheritance:
+
 Ensemble
 --------
 

docs/source/stonesoup.rst

@@ -42,6 +42,7 @@ Algorithm Components
     stonesoup.dataassociator
     stonesoup.deleter
     stonesoup.gater
+    stonesoup.kernel
     stonesoup.hypothesiser
     stonesoup.initiator
     stonesoup.mixturereducer

docs/source/stonesoup.updater.rst

@@ -19,6 +19,12 @@ Particle
 .. automodule:: stonesoup.updater.particle
     :show-inheritance:
 
+Kernel
+------
+
+.. automodule:: stonesoup.updater.kernel
+    :show-inheritance:
+
 Ensemble
 --------
 

docs/tutorials/filters/AKKF.py

@@ -0,0 +1,159 @@
+from abc import abstractmethod
+
+import numpy as np
+
+from .base import Base, Property
+from .types.array import StateVectors
+from .types.state import State
+
+
+class Kernel(Base):
+    """Kernel base type
+
+    A Kernel provides a means to translate state space or measurement space into kernel space.
+    """
+
+    @abstractmethod
+    def __call__(self, state1, state2=None):
+        r"""
+        Compute the kernel state of a pair of :class:`~.State` objects
+
+        Parameters
+        ----------
+        state1 : :class:`~.State`
+        state2 : :class:`~.State`
+
+        Returns
+        -------
+        StateVectors
+            kernel state of a pair of input :class:`~.State` objects
+
+        """
+        raise NotImplementedError
+
+    @staticmethod
+    def _get_state_vectors(state1, state2):
+        if isinstance(state1, State):
+            state_vector1 = state1.state_vector
+        else:
+            state_vector1 = state1
+        if state2 is None:
+            state_vector2 = state_vector1
+        else:
+            if isinstance(state2, State):
+                state_vector2 = state2.state_vector
+            else:
+                state_vector2 = state2
+        return state_vector1, state_vector2
+
+
+class QuadraticKernel(Kernel):
+    r"""Quadratic Kernel type
+
+    This kernel returns the quadratic kernel state vector from a pair of
+    :class:`~.KernelParticleState` state vectors.
+
+    The Quadratic kernel of state vectors :math:`\mathbf{x}` and
+    :math:`\mathbf{x}'` is defined as:
+
+    .. math::
+         \mathtt{k}\left(\mathbf{x}, \mathbf{x}'\right) =
+         \left(\alpha \langle \mathbf{x}, \mathbf{x}' \rangle + c\right)^2
+    """
+    c: float = Property(
+        default=1,
+        doc="Free parameter trading off the influence of higher-order versus lower-order "
+            "terms in the polynomial. Default is 1.")
+    ialpha: float = Property(default=1e1, doc="Slope. Range is [1e0, 1e4].")
+
+    def __call__(self, state1, state2=None):
+        r"""Calculate the Quadratic Kernel transformation for a pair of state vectors
+
+        Parameters
+        ----------
+        state1 : :class:`~.KernelParticleState`
+        state2 : :class:`~.KernelParticleState`
+
+        Returns
+        -------
+        StateVectors
+            Transformed state vector in kernel space.
+        """
+        state_vector1, state_vector2 = self._get_state_vectors(state1, state2)
+        return (state_vector1.T@state_vector2/self.ialpha + self.c) ** 2
+
+
+class QuarticKernel(Kernel):
+    r"""Quartic Kernel
+
+    This kernel returns the quartic kernel state from a pair of
+    :class:`~.KernelParticleState` objects.
+
+    The Quartic kernel of state vectors :math:`\mathbf{x}` and
+    :math:`\mathbf{x}'` is defined as:
+
+    .. math::
+         \mathtt{k}(\mathbf{x}, \mathbf{x}') =
+         \left(\alpha \langle \mathbf{x}, \mathbf{x}' \rangle + c\right)^4
+    """
+    c: float = Property(
+        default=1,
+        doc="Free parameter trading off the influence of higher-order versus lower-order "
+            "terms in the polynomial. Default is 1.")
+    ialpha: float = Property(default=1e1, doc="Slope. Range is [1e0, 1e4].")
+
+    def __call__(self, state1, state2=None):
+        r"""Calculate the Quartic Kernel transformation for a pair of state vectors
+
+        Parameters
+        ----------
+        state1 : :class:`~.KernelParticleState`
+        state2 : :class:`~.KernelParticleState`
+
+        Returns
+        -------
+        StateVectors
+            Transformed state in kernel space.
+        """
+        state_vector1, state_vector2 = self._get_state_vectors(state1, state2)
+        return (state_vector1.T@state_vector2/self.ialpha + self.c) ** 4
+
+
+class GaussianKernel(Kernel):
+    r"""Gaussian Kernel
+
+    This kernel returns the Gaussian kernel state vector from a pair of
+    :class:`~.KernelParticleState` state vectors.
+
+    The Gaussian kernel of state vectors :math:`\mathbf{x}` and
+    :math:`\mathbf{x}'` is defined as:
+
+    .. math::
+         \mathtt{k}(\mathbf{x}, \mathbf{x}') =
+         \mathrm{exp}\left(-\frac{||\mathbf{x} - \mathbf{x}'||^{2}}{2\pi\sigma^2}\right)
+    """
+    variance: float = Property(
+        default=1e1,
+        doc=r"Denoted as :math:`\sigma^2` in the equation above. Determines the width of the "
+            r"Gaussian kernel. Range is [1e0, 1e2].")
+
+    def __call__(self, state1, state2=None):
+        r"""Calculate the Gaussian Kernel transformation for a pair of state vectors
+
+        Parameters
+        ----------
+        state1 : :class:`~.KernelParticleState`
+        state2 : :class:`~.KernelParticleState`
+
+        Returns
+        -------
+        StateVectors
+            Transformed state vector in kernel space.
+        """
+        state_vector1, state_vector2 = self._get_state_vectors(state1, state2)
+        diff_tilde_x = (state_vector1.T[:, :, None] - state_vector2.T[:, None, :]) ** 2
+        diff_tilde_x_sum = np.sum(diff_tilde_x, axis=0)
+
+        k_tilde_x = np.exp(-diff_tilde_x_sum/(2*self.variance)) / np.sqrt(2*np.pi*self.variance)
+
+        return StateVectors(k_tilde_x)

stonesoup/kernel.py

@@ -0,0 +1,107 @@
+import copy
+
+import numpy as np
+
+from ._utils import predict_lru_cache
+from .kalman import KalmanPredictor
+from ..base import Property
+from ..kernel import Kernel, QuadraticKernel
+from ..types.prediction import Prediction
+from ..types.state import State
+from ..types.update import KernelParticleStateUpdate
+
+
+class AdaptiveKernelKalmanPredictor(KalmanPredictor):
+    r"""An implementation of the adaptive kernel Kalman filter (AKKF) predictor. Here, the AKKF draws
+    inspiration from the concepts of kernel mean embeddings (KME) and Kalman Filter to address
+    tracking problems in nonlinear systems.
+
+    In the state space, at time :math:`k`, the prior state
+    particles are generated by passing the proposal particles at time :math:`k-1`, i.e.,
+    :math:`\tilde{\mathbf{x}}_{k-1}^{\{i=1:M\}}`, through the motion model as
+
+    .. math::
+
+      \mathbf{x}_k^{\{i\}} =  \mathtt{f}\left(\tilde{\mathbf{x}}_{k-1}^{\{i\}},
+      \mathbf{u}_{k}^{\{i\}} \right).
+
+    In the kernel space, :math:`{\mathbf{x}}_{k}^{\{i=1:M_{\text{A}}\}}` are mapped as feature
+    mappings :math:`\Phi_k`.
+    Then, the predictive kernel weight vector :math:`\mathbf{w}^{-}_{k}`, and covariance matrix
+    :math:`{S}_{k}^{-}`, are calculated as
+
+    .. math::
+      \mathbf{w}^{-}_{k} &= \Gamma_{k}  \mathbf{w}^{+}_{k-1}\\
+      {S}_{k}^{-} &= \Gamma_{k} {S}^{+}_{k-1} \Gamma_{k} ^{\mathrm{T}} +V_{k}.
+
+    Here, :math:`\mathbf{w}^{+}_{k-1}` and :math:`{S}_{k-1}^{+}` are the posterior kernel weight
+    mean vector and covariance matrix at time :math:`k-1`, respectively.
+    The transition matrix :math:`\Gamma_{k}` represents the change of sample representation, and
+    :math:`{V}_{k}` represents the finite matrix representation of the transition residual matrix.
+    """
+    kernel: Kernel = Property(
+        default=None,
+        doc="Default is None. If None, the default :class:`~QuadraticKernel` is used.")
+    lambda_predictor: float = Property(
+        default=1e-3,
+        doc=r":math:`\lambda_{\tilde{K}}`. Regularisation parameter used to stabilise the inverse "
+            r"Gram matrix. Range is :math:`\left[10^{-4}, 10^{-2}\right]`")
+
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+
+        if self.kernel is None:
+            self.kernel = QuadraticKernel()
+
+    @predict_lru_cache()
+    def predict(self, prior, timestamp=None, proposal=None, **kwargs):
+        r"""The adaptive kernel version of the predict step
+
+        Parameters
+        ----------
+        prior : :class:`~.KernelParticleState`
+            Prior state, :math:`\mathbf{x}_{k-1}`
+        timestamp : :class:`datetime.datetime`
+            Time to transit to (:math:`k`)
+        proposal : :class:`~.KernelParticleState`
+            Proposal state, :math:`\mathbf{x}_{k-1}`
+        **kwargs : various, optional
+            These are passed to :meth:`~.TransitionModel.covar`
+
+        Returns
+        -------
+        : :class:`~.KernelParticleStatePrediction`
+            The predicted state :math:`\mathbf{x}_{k|k-1}` and the predicted
+            state covariance :math:`P_{k|k-1}`
+        """
+        if proposal is None:
+            if isinstance(prior, KernelParticleStateUpdate):
+                proposal = State(state_vector=prior.proposal)
+            else:
+                proposal = copy.copy(prior)
+
+        # Get the prediction interval
+        predict_over_interval = self._predict_over_interval(prior, timestamp)
+        new_state_vector = self.transition_model.function(
+            proposal,
+            time_interval=predict_over_interval,
+            **kwargs)
+
+        k_tilde_tilde = self.kernel(proposal)
+        k_tilde_nontilde = self.kernel(proposal, prior)
+
+        I = np.identity(len(prior))  # noqa: E741
+        inv_val = np.linalg.pinv(k_tilde_tilde + self.lambda_predictor * I)
+
+        kernel_t = inv_val @ k_tilde_nontilde
+        prediction_weights = kernel_t @ prior.weight
+        new_val = inv_val @ k_tilde_tilde - I
+        v = new_val@new_val.T / len(prior)
+
+        prediction_covariance = kernel_t @ prior.kernel_covar @ kernel_t.T + v
+        return Prediction.from_state(prior,
+                                     state_vector=new_state_vector,
+                                     weight=prediction_weights,
+                                     kernel_covar=prediction_covariance,
+                                     timestamp=timestamp,
+                                     transition_model=self.transition_model)