Merge pull request #204 from dstl/state_vectors

Add StateVectors type and modify array handling

setup.py

@@ -26,7 +26,7 @@
       ],
       packages=find_packages(exclude=('docs', '*.tests')),
       install_requires=[
-          'ruamel.yaml>=0.15.45', 'scipy', 'matplotlib', 'utm', 'pymap3d'],
+          'ruamel.yaml>=0.15.45', 'numpy>=1.17', 'scipy', 'matplotlib', 'utm', 'pymap3d'],
       extras_require={
           'dev': [
               'pytest-flake8', 'pytest-cov', 'Sphinx', 'sphinx_rtd_theme',

stonesoup/functions.py

@@ -5,7 +5,7 @@
 from copy import copy
 
 from .types.numeric import Probability
-from .types.array import Matrix
+from .types.array import StateVector, StateVectors, CovarianceMatrix
 
 
 def tria(matrix):
@@ -95,7 +95,7 @@ def gauss2sigma(state, alpha=1.0, beta=2.0, kappa=None):
         (default is 1)
     beta : float, optional
         Used to incorporate prior knowledge of the distribution
-        2 is optimal is the state is normally distributed.
+        2 is optimal if the state is normally distributed.
         (default is 2)
     kappa : float, optional
         Secondary spread scaling parameter
@@ -129,9 +129,7 @@ def gauss2sigma(state, alpha=1.0, beta=2.0, kappa=None):
     c = ndim_state + lamda
 
     # Calculate sigma point locations
-    sigma_points = np.tile(state.state_vector, (1, 2 * ndim_state + 1))
-    # as sigma_points is a 2d it should no longer be a StateVector
-    sigma_points = Matrix(sigma_points)
+    sigma_points = StateVectors([state.state_vector for _ in range(2 * ndim_state + 1)])
     # Can't use in place addition/subtraction as casting issues may arise when mixing float/int
     sigma_points[:, 1:(ndim_state + 1)] = \
         sigma_points[:, 1:(ndim_state + 1)] + sqrt_sigma*np.sqrt(c)
@@ -142,7 +140,7 @@ def gauss2sigma(state, alpha=1.0, beta=2.0, kappa=None):
     sigma_points_states = []
     for sigma_point in sigma_points.T:
         state_copy = copy(state)
-        state_copy.state_vector = np.atleast_2d(sigma_point).T
+        state_copy.state_vector = StateVector(sigma_point)
         sigma_points_states.append(state_copy)
 
     # Calculate weights
@@ -160,7 +158,7 @@ def sigma2gauss(sigma_points, mean_weights, covar_weights, covar_noise=None):
 
     Parameters
     ----------
-    sigma_points : :class:`numpy.ndarray` of shape `(Ns, 2*Ns+1)`
+    sigma_points : :class:`~.StateVectors` of shape `(Ns, 2*Ns+1)`
         An array containing the locations of the sigma points
     mean_weights : :class:`numpy.ndarray` of shape `(2*Ns+1,)`
         An array containing the sigma point mean weights
@@ -172,20 +170,20 @@ def sigma2gauss(sigma_points, mean_weights, covar_weights, covar_noise=None):
 
     Returns
     -------
-    : :class:`numpy.ndarray` of shape `(Ns, 1)`
+    : :class:`~.StateVector` of shape `(Ns, 1)`
         Calculated mean
     : :class:`~.CovarianceMatrix` of shape `(Ns, Ns)`
         Calculated covariance
     """
 
-    mean = sigma_points@mean_weights[:, np.newaxis]
+    mean = np.average(sigma_points, axis=1, weights=mean_weights)
 
     points_diff = sigma_points - mean
 
     covar = points_diff@(np.diag(covar_weights))@(points_diff.T)
     if covar_noise is not None:
         covar = covar + covar_noise
-    return mean, covar
+    return mean.view(StateVector), covar.view(CovarianceMatrix)
 
 
 def unscented_transform(sigma_points_states, mean_weights, covar_weights,
@@ -199,7 +197,7 @@ def unscented_transform(sigma_points_states, mean_weights, covar_weights,
 
     Parameters
     ----------
-    sigma_points : :class:`numpy.ndarray` of shape `(Ns, 2*Ns+1)`
+    sigma_points : :class:`~.StateVectors` of shape `(Ns, 2*Ns+1)`
         An array containing the locations of the sigma points
     mean_weights : :class:`numpy.ndarray` of shape `(2*Ns+1,)`
         An array containing the sigma point mean weights
@@ -218,53 +216,43 @@ def unscented_transform(sigma_points_states, mean_weights, covar_weights,
 
     Returns
     -------
-    : :class:`numpy.ndarray` of shape `(Ns, 1)`
+    : :class:`~.StateVector` of shape `(Ns, 1)`
         Transformed mean
     : :class:`~.CovarianceMatrix` of shape `(Ns, Ns)`
         Transformed covariance
     : :class:`~.CovarianceMatrix` of shape `(Ns,Nm)`
         Calculated cross-covariance matrix
-    : :class:`numpy.ndarray` of shape `(Ns, 2*Ns+1)`
+    : :class:`~.StateVectors` of shape `(Ns, 2*Ns+1)`
         An array containing the locations of the transformed sigma points
     : :class:`numpy.ndarray` of shape `(2*Ns+1,)`
         An array containing the transformed sigma point mean weights
     : :class:`numpy.ndarray` of shape `(2*Ns+1,)`
         An array containing the transformed sigma point covariance weights
     """
-
-    n_points = len(sigma_points_states)
-    ndim_state = len(sigma_points_states[0].state_vector)
-
     # Reconstruct the sigma_points matrix
-    sigma_points = np.empty((ndim_state, 0))
-    for sigma_points_state in sigma_points_states:
-        sigma_points = np.c_[sigma_points, sigma_points_state.state_vector]
+    sigma_points = StateVectors([
+        sigma_points_state.state_vector for sigma_points_state in sigma_points_states])
 
     # Transform points through f
-    sigma_points_t = np.zeros((ndim_state, n_points))
     if points_noise is None:
-        sigma_points_t = np.asarray(
-            [fun(sigma_points_states[i])
-             for i in range(n_points)]).squeeze(2).T
+        sigma_points_t = StateVectors([
+            fun(sigma_points_state) for sigma_points_state in sigma_points_states])
     else:
-        sigma_points_t = np.asarray(
-            [fun(sigma_points_states[i], points_noise[:, i:i+1])
-             for i in range(n_points)]).squeeze(2).T
-    sigma_points_t = sigma_points_t.view(Matrix)
+        sigma_points_t = StateVectors([
+            fun(sigma_points_state, points_noise)
+            for sigma_points_state, point_noise in zip(sigma_points_states, points_noise.T)])
 
     # Calculate mean and covariance approximation
-    mean, covar = sigma2gauss(
-        sigma_points_t, mean_weights, covar_weights, covar_noise)
+    mean, covar = sigma2gauss(sigma_points_t, mean_weights, covar_weights, covar_noise)
 
     # Calculate cross-covariance
     cross_covar = (
         (sigma_points-sigma_points[:, 0:1])
         @np.diag(mean_weights)
         @(sigma_points_t-mean).T
-    )
+    ).view(CovarianceMatrix)
 
-    return mean, covar, cross_covar,\
-        sigma_points_t, mean_weights, covar_weights
+    return mean, covar, cross_covar, sigma_points_t, mean_weights, covar_weights
 
 
 def cart2pol(x, y):
@@ -495,40 +483,31 @@ def gm_reduce_single(means, covars, weights):
 
     Parameters
     ----------
-    means : np.array of shape (num_components, num_dims)
+    means : :class:`~.StateVectors`
         The means of the GM components
-    covars : np.array of shape (num_components, num_dims, num_dims)
+    covars : np.array of shape (num_dims, num_dims, num_components)
         The covariance matrices of the GM components
     weights : np.array of shape (num_components,)
         The weights of the GM components
 
     Returns
     -------
-    np.array of shape (num_dims, 1)
+    : :class:`~.StateVector`
         The mean of the reduced/single Gaussian
-    np.array of shape (num_dims, num_dims)
+    : :class:`~.CovarianceMatrix`
         The covariance of the reduced/single Gaussian
     """
-
-    # Compute dimensionality variables
-    num_components, num_dims = np.shape(means)
-
     # Normalise weights such that they sum to 1
     weights = weights/Probability.sum(weights)
 
     # Calculate mean
-    mean = np.average(means, axis=0, weights=weights).astype(np.float_)
-    mean.shape = (1, num_dims)
+    mean = np.average(means, axis=1, weights=weights)
 
     # Calculate covar
-    covar = np.zeros((num_dims, num_dims))
-    for i in range(num_components):
-        v = means[i, :] - mean
-        a = np.add(covars[i], v.T@v)
-        b = weights[i]
-        covar = np.add(covar, b*a)
-
-    return mean.transpose(), covar
+    delta_means = means - mean
+    covar = np.sum(covars*weights, axis=2, dtype=np.float_) + weights*delta_means@delta_means.T
+
+    return mean.view(StateVector), covar.view(CovarianceMatrix)
 
 
 def mod_bearing(x):

stonesoup/models/base.py

@@ -6,7 +6,7 @@
 
 from ..base import Base
 from ..functions import jacobian as compute_jac
-from ..types.array import Matrix, StateVector
+from ..types.array import StateVector, StateVectors
 from ..types.numeric import Probability
 
 
@@ -196,7 +196,7 @@ def rvs(self, num_samples=1, **kwargs):
         if num_samples == 1:
             return noise.view(StateVector)
         else:
-            return noise.view(Matrix)
+            return noise.view(StateVectors)
 
     def pdf(self, state1, state2, **kwargs):
         r"""Model pdf/likelihood evaluation function

stonesoup/models/measurement/nonlinear.py

@@ -1,6 +1,6 @@
 # -*- coding: utf-8 -*-
 from abc import ABC
-from typing import List
+from typing import List, Union
 import copy
 
 import numpy as np
@@ -11,7 +11,7 @@
 from ...functions import cart2pol, pol2cart, \
     cart2sphere, sphere2cart, cart2angles, \
     rotx, roty, rotz
-from ...types.array import StateVector, CovarianceMatrix, Matrix
+from ...types.array import StateVector, CovarianceMatrix, StateVectors
 from ...types.angle import Bearing, Elevation
 from ..base import LinearModel, NonLinearModel, GaussianModel, ReversibleModel
 from .base import MeasurementModel
@@ -81,13 +81,13 @@ def covar(self, **kwargs) -> CovarianceMatrix:
             *(model.covar(**kwargs) for model in self.model_list)
             ).view(CovarianceMatrix)
 
-    def rvs(self, num_samples=1, **kwargs) -> np.ndarray:
+    def rvs(self, num_samples=1, **kwargs) -> Union[StateVector, StateVectors]:
         rvs_vectors = np.vstack([model.rvs(num_samples, **kwargs)
                                  for model in self.model_list])
         if num_samples == 1:
             return rvs_vectors.view(StateVector)
         else:
-            return rvs_vectors.view(Matrix)
+            return rvs_vectors.view(StateVectors)
 
 
 class NonLinearGaussianMeasurement(MeasurementModel, NonLinearModel, GaussianModel, ABC):
@@ -278,7 +278,7 @@ def inverse_function(self, detection, **kwargs) -> StateVector:
 
         return res
 
-    def rvs(self, num_samples=1, **kwargs) -> np.ndarray:
+    def rvs(self, num_samples=1, **kwargs) -> Union[StateVector, StateVectors]:
         out = super().rvs(num_samples, **kwargs)
         out = np.array([[Elevation(0.)], [Bearing(0.)], [0.]]) + out
         return out
@@ -424,7 +424,7 @@ def function(self, state, noise=False, **kwargs) -> StateVector:
 
         return StateVector([[Bearing(phi)], [rho]]) + noise
 
-    def rvs(self, num_samples=1, **kwargs) -> np.ndarray:
+    def rvs(self, num_samples=1, **kwargs) -> Union[StateVector, StateVectors]:
         out = super().rvs(num_samples, **kwargs)
         out = np.array([[Bearing(0)], [0.]]) + out
         return out
@@ -548,7 +548,7 @@ def function(self, state, noise=False, **kwargs) -> StateVector:
 
         return StateVector([[Elevation(theta)], [Bearing(phi)]]) + noise
 
-    def rvs(self, num_samples=1, **kwargs) -> np.ndarray:
+    def rvs(self, num_samples=1, **kwargs) -> Union[StateVector, StateVectors]:
         out = super().rvs(num_samples, **kwargs)
         out = np.array([[Elevation(0.)], [Bearing(0.)]]) + out
         return out
@@ -692,7 +692,7 @@ def function(self, state, noise=False, **kwargs) -> StateVector:
 
         return StateVector([[Bearing(phi)], [rho], [rr]]) + noise
 
-    def rvs(self, num_samples=1, **kwargs) -> np.ndarray:
+    def rvs(self, num_samples=1, **kwargs) -> Union[StateVector, StateVectors]:
         out = super().rvs(num_samples, **kwargs)
         out = np.array([[Bearing(0)], [0.], [0.]]) + out
         return out
@@ -864,7 +864,7 @@ def inverse_function(self, detection, **kwargs) -> StateVector:
 
         return out_vector
 
-    def rvs(self, num_samples=1, **kwargs) -> np.ndarray:
+    def rvs(self, num_samples=1, **kwargs) -> Union[StateVector, StateVectors]:
         out = super().rvs(num_samples, **kwargs)
         out = np.array([[Elevation(0)], [Bearing(0)], [0.], [0.]]) + out
         return out

stonesoup/models/measurement/tests/test_models.py

@@ -13,7 +13,7 @@
 from ....types.state import State, CovarianceMatrix
 from ....functions import jacobian as compute_jac
 from ....types.angle import Bearing, Elevation
-from ....types.array import StateVector, Matrix
+from ....types.array import StateVector, StateVectors
 from ....functions import pol2cart
 
 
@@ -187,8 +187,7 @@ def fun(x):
     assert isinstance(rvs, StateVector)
     rvs = model.rvs(10)
     assert rvs.shape == (model.ndim_meas, 10)
-    assert isinstance(rvs, Matrix)
-    # StateVector is subclass of Matrix, so need to check explicitly.
+    assert isinstance(rvs, StateVectors)
     assert not isinstance(rvs, StateVector)
 
     # Project a state through the model
@@ -437,7 +436,7 @@ def fun(x):
     assert isinstance(rvs, StateVector)
     rvs = model.rvs(10)
     assert rvs.shape == (model.ndim_meas, 10)
-    assert isinstance(rvs, Matrix)
+    assert isinstance(rvs, StateVectors)
     # StateVector is subclass of Matrix, so need to check explicitly.
     assert not isinstance(rvs, StateVector)
 

stonesoup/tests/test_functions.py

@@ -6,14 +6,15 @@
 from ..functions import (
     jacobian, gm_reduce_single, mod_bearing, mod_elevation, gauss2sigma,
     rotx, roty, rotz, cart2sphere, cart2angles, pol2cart, sphere2cart)
+from ..types.array import StateVector, StateVectors
 from ..types.state import State, GaussianState
 
 
 def test_jacobian():
     """ jacobian function test """
 
     # State related variables
-    state_mean = np.array([[3.0], [1.0]])
+    state_mean = StateVector([[3.0], [1.0]])
 
     def f(x):
         return np.array([[1, 1], [0, 1]])@x.state_vector
@@ -43,10 +44,10 @@ def fun2d(vec):
         return out
 
     x = 3
-    jac = jacobian(fun, State(np.array([[x]])))
+    jac = jacobian(fun, State(StateVector([[x]])))
     assert np.allclose(jac, 4*x)
 
-    x = np.array([[1], [2]])
+    x = StateVector([[1], [2]])
     # Tolerance value to use to test if arrays are equal
     tol = 1.0e-5
 
@@ -67,7 +68,7 @@ def fun2d(vec):
 
 def test_jacobian_large_values():
     # State related variables
-    state = State(np.array([[1E10], [1.0]]))
+    state = State(StateVector([[1E10], [1.0]]))
 
     def f(x):
         return x.state_vector**2
@@ -78,10 +79,10 @@ def f(x):
 
 def test_gm_reduce_single():
 
-    means = np.array([[1, 2], [3, 4], [5, 6]])
-    covars = np.array([[[1, 1], [1, 0.7]],
+    means = StateVectors([StateVector([1, 2]), StateVector([3, 4]), StateVector([5, 6])])
+    covars = np.stack([[[1, 1], [1, 0.7]],
                        [[1.2, 1.4], [1.3, 2]],
-                       [[2, 1.4], [1.2, 1.2]]])
+                       [[2, 1.4], [1.2, 1.2]]], axis=2)
     weights = np.array([1, 2, 5])
 
     mean, covar = gm_reduce_single(means, covars, weights)