From 7e13f30027b39a51c3da574a0f2418f63b339568 Mon Sep 17 00:00:00 2001 From: Steven Hiscocks Date: Fri, 24 Apr 2020 17:21:51 +0100 Subject: [PATCH 1/2] Add StateVectors type and modify array handling This includes a number of changes to how Matrix type is implemented which will allow greater customisation if required. In particular, a new StateVectors type has been created which handles the case where calculations of averages (and in turn covariance) were incorrect when using Angle types. Angle types now include a average method, which returns a circular mean. This removes an issue when calculating means near the point of angle wrapping. These changes were primarily done for bug identified when calculating mean of particles, and such some minor changes to PartcileState and Particle type have been made. Some functions have also been updated, as these are a set of state vectors, so can take advantage of the new type. This includes updating the Mixture Tracker The array changes have changed minimum requirement for NumPy to 1.17, or 1.16 with NUMPY_EXPERIMENTAL_ARRAY_FUNCTION environment variable set to 1. --- setup.py | 2 +- stonesoup/functions.py | 67 +++------ stonesoup/models/base.py | 4 +- stonesoup/models/measurement/nonlinear.py | 4 +- .../models/measurement/tests/test_models.py | 5 +- stonesoup/tests/test_functions.py | 15 +- stonesoup/tracker/simple.py | 23 +-- stonesoup/types/angle.py | 57 ++++++-- stonesoup/types/array.py | 138 ++++++++++++++++-- stonesoup/types/particle.py | 6 + stonesoup/types/state.py | 19 ++- stonesoup/types/tests/test_angle.py | 20 ++- stonesoup/types/tests/test_state.py | 74 ++++++---- stonesoup/updater/tests/test_particle.py | 34 ++--- 14 files changed, 306 insertions(+), 162 deletions(-) diff --git a/setup.py b/setup.py index 60e00b2c7..dacdfa4d1 100644 --- a/setup.py +++ b/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', diff --git a/stonesoup/functions.py b/stonesoup/functions.py index b14c4f1e5..cfbb17166 100644 --- a/stonesoup/functions.py +++ b/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): @@ -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 @@ -178,14 +176,14 @@ def sigma2gauss(sigma_points, mean_weights, covar_weights, covar_noise=None): 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, @@ -231,30 +229,21 @@ def unscented_transform(sigma_points_states, mean_weights, covar_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 = ( @@ -263,8 +252,7 @@ def unscented_transform(sigma_points_states, mean_weights, covar_weights, @(sigma_points_t-mean).T ) - 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): diff --git a/stonesoup/models/base.py b/stonesoup/models/base.py index 3a11c4fd5..c4754d141 100644 --- a/stonesoup/models/base.py +++ b/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 diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py index 7431b4cad..da0aae289 100644 --- a/stonesoup/models/measurement/nonlinear.py +++ b/stonesoup/models/measurement/nonlinear.py @@ -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 @@ -87,7 +87,7 @@ def rvs(self, num_samples=1, **kwargs): 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): diff --git a/stonesoup/models/measurement/tests/test_models.py b/stonesoup/models/measurement/tests/test_models.py index bda4cbab7..512a5c30f 100644 --- a/stonesoup/models/measurement/tests/test_models.py +++ b/stonesoup/models/measurement/tests/test_models.py @@ -11,7 +11,7 @@ from ....types.state import State 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 @@ -212,8 +212,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 diff --git a/stonesoup/tests/test_functions.py b/stonesoup/tests/test_functions.py index e82fe1d75..184ab1207 100644 --- a/stonesoup/tests/test_functions.py +++ b/stonesoup/tests/test_functions.py @@ -4,6 +4,7 @@ from ..functions import ( jacobian, gm_reduce_single, mod_bearing, mod_elevation, gauss2sigma) +from ..types.array import StateVector, StateVectors from ..types.state import State, GaussianState @@ -11,7 +12,7 @@ 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 @@ -41,10 +42,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 @@ -65,7 +66,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 @@ -76,10 +77,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) diff --git a/stonesoup/tracker/simple.py b/stonesoup/tracker/simple.py index 771d8381d..20ee8d86d 100644 --- a/stonesoup/tracker/simple.py +++ b/stonesoup/tracker/simple.py @@ -8,6 +8,7 @@ from ..reader import DetectionReader from ..initiator import Initiator from ..updater import Updater +from ..types.array import StateVectors from ..types.prediction import GaussianStatePrediction from ..types.update import GaussianStateUpdate from ..functions import gm_reduce_single @@ -195,34 +196,26 @@ def tracks_gen(self): posterior_state_weights.append( hypothesis.probability) - means = np.array([state.state_vector for state - in posterior_states]) - means = np.reshape(means, np.shape(means)[:-1]) - covars = np.array([state.covar for state - in posterior_states]) - covars = np.reshape(covars, (np.shape(covars))) - weights = np.array([weight for weight - in posterior_state_weights]) - weights = np.reshape(weights, np.shape(weights)) + 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) - post_mean, post_covar = gm_reduce_single(means, - covars, weights) + post_mean, post_covar = gm_reduce_single(means, covars, weights) - missed_detection_weight = next( - hyp.weight for hyp in multihypothesis if not hyp) + missed_detection_weight = next(hyp.weight for hyp in multihypothesis if not hyp) # Check if at least one reasonable measurement... if any(hypothesis.weight > missed_detection_weight for hypothesis in multihypothesis): # ...and if so use update type track.append(GaussianStateUpdate( - np.array(post_mean), np.array(post_covar), + post_mean, post_covar, multihypothesis, multihypothesis[0].measurement.timestamp)) else: # ...and if not, treat as a prediction track.append(GaussianStatePrediction( - np.array(post_mean), np.array(post_covar), + post_mean, post_covar, multihypothesis[0].prediction.timestamp)) # any detections in multihypothesis that had an diff --git a/stonesoup/types/angle.py b/stonesoup/types/angle.py index a67493b6a..1d19dd9e0 100644 --- a/stonesoup/types/angle.py +++ b/stonesoup/types/angle.py @@ -1,9 +1,12 @@ # -*- coding: utf-8 -*- -import numpy as np from numbers import Real from numpy import float64 from math import trunc, ceil, floor + +import numpy as np + from ..functions import mod_bearing, mod_elevation +from .numeric import Probability class Angle(Real): @@ -23,14 +26,14 @@ def __init__(self, value): self._value = float64(self.mod_angle(value)) def __add__(self, other): - out = self._value + float64(other) + out = self._value + other return self.__class__(self.mod_angle(out)) def __radd__(self, other): return self.__class__.__add__(self, other) def __sub__(self, other): - out = self._value - float64(other) + out = self._value - other return self.__class__(self.mod_angle(out)) def __rsub__(self, other): @@ -40,10 +43,10 @@ def __float__(self): return float(self._value) def __mul__(self, other): - return self._value * float64(other) + return self._value * other def __rmul__(self, other): - return self._value * float64(other) + return self._value * other def __str__(self): return str(self._value) @@ -55,31 +58,31 @@ def __neg__(self): return self.__class__(-self._value) def __truediv__(self, other): - return self._value / float64(other) + return self._value / other def __rtruediv__(self, other): - return float64(other) / self._value + return other / self._value def __eq__(self, other): - return self._value == float64(other) + return self._value == other def __ne__(self, other): - return self._value != float64(other) + return self._value != other def __abs__(self): return self.__class__(abs(self._value)) def __le__(self, other): - return self._value <= float64(other) + return self._value <= other def __lt__(self, other): - return self._value < float64(other) + return self._value < other def __ge__(self, other): - return self._value >= float64(other) + return self._value >= other def __gt__(self, other): - return self._value > float64(other) + return self._value > other def __floor__(self): return floor(self._value) @@ -135,6 +138,34 @@ def tanh(self): def rad2deg(self): return np.rad2deg(self._value) + @classmethod + def average(cls, angles, weights=None): + """Calculated the circular mean for sequence of angles + + Parameters + ---------- + angles : sequence of :class:`~.Angle` + Angles which to calculate the mean of. + weights : sequence of float, optional + Weights to calculate weighted mean. Default `None`, where no weights applied. + + Returns + ------- + : :class:`Angle` + Circular mean of angles + """ + if weights is None: + weight_sum = 1 + weights = 1 + else: + weight_sum = Probability.sum(weights) + + result = np.arctan2( + float(np.sum(np.sin(angles) * weights) / weight_sum), + float(np.sum(np.cos(angles) * weights) / weight_sum)) + + return cls(result) + class Bearing(Angle): """Bearing angle class. diff --git a/stonesoup/types/array.py b/stonesoup/types/array.py index d9c899791..dad312898 100644 --- a/stonesoup/types/array.py +++ b/stonesoup/types/array.py @@ -1,4 +1,6 @@ # -*- coding: utf-8 -*- +from collections.abc import Sequence + import numpy as np @@ -14,30 +16,37 @@ def __new__(cls, *args, **kwargs): return array.view(cls) def __array_wrap__(self, array): - return Matrix._cast(np.asarray(array)) + return self._cast(array) @classmethod def _cast(cls, val): - # This tries to cast the result as either a StateVector or - # Matrix type if applicable. + # This tries to cast the result as either a StateVector or Matrix type if applicable. if isinstance(val, np.ndarray): - if val.ndim == 2: - if val.shape[1] == 1: - return val.view(StateVector) - else: - return val.view(Matrix) + if val.ndim == 2 and val.shape[1] == 1: + return val.view(StateVector) else: return val.view(Matrix) else: return val - def __matmul__(self, other): - out = np.matmul(np.asfarray(self), np.asfarray(other)) - return self._cast(out) - - def __rmatmul__(self, other): - out = np.matmul(np.asfarray(other), np.asfarray(self)) - return self._cast(out) + def __array_ufunc__(self, ufunc, method, *inputs, **kwargs): + if ufunc in (np.isfinite, np.matmul): + # Custom types break here, so simply convert to floats. + inputs = [np.asfarray(input_) if isinstance(input_, Matrix) else input_ + for input_ in inputs] + else: + # Change to standard ndarray + inputs = [np.asarray(input_) if isinstance(input_, Matrix) else input_ + for input_ in inputs] + if 'out' in kwargs: + kwargs['out'] = tuple(np.asarray(out) if isinstance(out, Matrix) else out + for out in kwargs['out']) + + result = super().__array_ufunc__(ufunc, method, *inputs, **kwargs) + if result is NotImplemented: + return NotImplemented + else: + return self._cast(result) class StateVector(Matrix): @@ -99,6 +108,105 @@ def __setitem__(self, key, value): return super().__setitem__(key, value) +class StateVectors(Matrix): + """Wrapper for :class:`numpy.ndarray for multiple State Vectors` + + This class returns a view to a :class:`numpy.ndarray` that is in shape + (num_dimensions, num_components), customising some numpy functions to ensure + custom types are handled correctly. This can be initialised by a sequence + type (list, tuple; not array) that contains :class:`StateVector`, otherwise + it's called same as :func:`numpy.asarray`. + """ + + def __new__(cls, states, *args, **kwargs): + if isinstance(states, Sequence) and not isinstance(states, np.ndarray): + if isinstance(states[0], StateVector): + return np.hstack(states).view(cls) + array = np.asarray(states, *args, **kwargs) + return array.view(cls) + + @classmethod + def _cast(cls, val): + out = super()._cast(val) + if type(out) == Matrix: + # Assume still set of State Vectors + return out.view(StateVectors) + else: + return out + + def __array_function__(self, func, types, args, kwargs): + if func is np.average: + return self._average(*args, **kwargs) + elif func is np.mean: + return self._mean(*args, **kwargs) + elif func is np.cov: + return self._cov(*args, **kwargs) + else: + return super().__array_function__(func, types, args, kwargs) + + @staticmethod + def _mean(state_vectors, axis=None, dtype=None, out=None, keepdims=np._NoValue): + if state_vectors.dtype != np.object_: + # Can just use standard numpy mean if not using custom objects + return np.mean(axis, dtype, out, keepdims) + elif axis == 1 and out is None: + state_vector = np.average(state_vectors, axis) + if dtype: + return state_vector.astype(dtype) + else: + return state_vector + else: + return NotImplemented + + @staticmethod + def _average(state_vectors, axis=None, weights=None, returned=False): + if state_vectors.dtype != np.object_: + # Can just use standard numpy averaging if not using custom objects + state_vector = np.average(np.asarray(state_vectors), axis=axis, weights=weights) + # Convert type as may have type of weights + state_vector = StateVector(state_vector.astype(np.float_, copy=False)) + elif axis == 1: # Need to handle special cases of averaging potentially + state_vector = StateVector( + np.empty((state_vectors.shape[0], 1), dtype=state_vectors.dtype)) + for dim, row in enumerate(state_vectors): + type_ = type(row[0]) # Assume all the same type + if hasattr(type_, 'average'): + # Check if type has custom average method + state_vector[dim, 0] = type_.average(row, weights=weights) + else: + # Else use numpy built in + state_vector[dim, 0] = type_(np.average(np.asarray(row), weights=weights)) + else: + return NotImplemented + + if returned: + return state_vector, np.sum(weights) + else: + return state_vector + + @staticmethod + def _cov(state_vectors, y=None, rowvar=True, bias=False, ddof=None, fweights=None, + aweights=None): + + if state_vectors.dtype != np.object_: + # Can just use standard numpy averaging if not using custom objects + cov = np.cov(np.asarray(state_vectors), y, rowvar, bias, ddof, fweights, aweights) + elif y is None and rowvar and not bias and ddof == 0 and fweights is None: + # Only really handle simple usage here + avg, w_sum = np.average(state_vectors, axis=1, weights=aweights, returned=True) + + X = np.asfarray(state_vectors - avg) + if aweights is None: + X_T = X.T + else: + X_T = (X*np.asfarray(aweights)).T + cov = X @ X_T.conj() + cov *= np.true_divide(1, float(w_sum)) + else: + return NotImplemented + return CovarianceMatrix(np.atleast_2d(cov)) + + class CovarianceMatrix(Matrix): """Covariance matrix wrapper for :class:`numpy.ndarray`. diff --git a/stonesoup/types/particle.py b/stonesoup/types/particle.py index 69cfca99e..27963b68c 100644 --- a/stonesoup/types/particle.py +++ b/stonesoup/types/particle.py @@ -18,5 +18,11 @@ class Particle(Type): 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) + + @property + def ndim(self): + return self.state_vector.shape[0] Particle.parent.cls = Particle # noqa:E305 diff --git a/stonesoup/types/state.py b/stonesoup/types/state.py index c98c7437e..299c02a8a 100644 --- a/stonesoup/types/state.py +++ b/stonesoup/types/state.py @@ -5,7 +5,7 @@ import numpy as np from ..base import Property -from .array import StateVector, CovarianceMatrix +from .array import StateVector, StateVectors, CovarianceMatrix from .base import Type from .particle import Particle @@ -156,18 +156,22 @@ class ParticleState(Type): This is a particle state object which describes the state as a distribution of particles""" - timestamp = Property(datetime.datetime, default=None, - doc="Timestamp of the state. Default None.") particles = Property([Particle], doc='List of particles representing state') + timestamp = Property(datetime.datetime, default=None, + doc="Timestamp of the state. Default None.") + + @property + def ndim(self): + return self.particles[0].ndim @property def mean(self): """The state mean, equivalent to state vector""" - result = np.average([p.state_vector for p in self.particles], axis=0, + result = np.average(StateVectors([p.state_vector for p in self.particles]), axis=1, weights=[p.weight for p in self.particles]) # Convert type as may have type of weights - return result.astype(np.float, copy=False) + return result @property def state_vector(self): @@ -176,9 +180,8 @@ def state_vector(self): @property def covar(self): - cov = np.cov(np.hstack([p.state_vector for p in self.particles]), + cov = np.cov(StateVectors([p.state_vector for p in self.particles]), ddof=0, aweights=[p.weight for p in self.particles]) # Fix one dimensional covariances being returned with zero dimension - if not cov.shape: - cov = cov.reshape(1, 1) return cov +State.register(ParticleState) # noqa: E305 diff --git a/stonesoup/types/tests/test_angle.py b/stonesoup/types/tests/test_angle.py index 5d534dbd7..c656ce068 100644 --- a/stonesoup/types/tests/test_angle.py +++ b/stonesoup/types/tests/test_angle.py @@ -1,12 +1,12 @@ # -*- coding: utf-8 -*- +from math import trunc, ceil, floor -from pytest import approx +from pytest import approx, xfail from numpy import deg2rad import numpy as np -from ...functions import (mod_elevation, mod_bearing) -from math import trunc, ceil, floor from ..angle import Bearing, Elevation, Latitude, Longitude +from ...functions import (mod_elevation, mod_bearing) def pytest_generate_tests(metafunc): @@ -114,3 +114,17 @@ def test_misc(self, class_, func): def test_degrees(self, class_, func): b1 = class_(np.pi/4) # pi/4 radians = 45 degrees assert b1.degrees == 45.0 + + def test_average(self, class_, func): + val = np.pi/4 + b1 = class_(val) - 0.1 + b2 = class_(val) + 0.1 + assert class_.average([b1, b2]) == approx(val) + + if func is mod_bearing: + val = -np.pi + b1 = class_(val) - 0.1 + b2 = class_(val) + 0.1 + assert class_.average([b1, b2]) == approx(val) + else: + raise xfail("Can't handle average when wrapping over ±pi") diff --git a/stonesoup/types/tests/test_state.py b/stonesoup/types/tests/test_state.py index 483226b5e..f7ed3a4da 100644 --- a/stonesoup/types/tests/test_state.py +++ b/stonesoup/types/tests/test_state.py @@ -4,6 +4,8 @@ import numpy as np import pytest +from ..angle import Bearing +from ..array import StateVector, CovarianceMatrix from ..numeric import Probability from ..particle import Particle from ..state import State, GaussianState, ParticleState, \ @@ -15,7 +17,7 @@ def test_state(): State() # Test state initiation without timestamp - state_vector = np.array([[0], [1]]) + state_vector = StateVector([[0], [1]]) state = State(state_vector) assert np.array_equal(state.state_vector, state_vector) @@ -27,7 +29,7 @@ def test_state(): def test_state_invalid_vector(): with pytest.raises(ValueError): - State(np.array([[[1, 2, 3, 4]]])) + State(StateVector([[[1, 2, 3, 4]]])) def test_gaussianstate(): @@ -36,11 +38,11 @@ def test_gaussianstate(): with pytest.raises(TypeError): GaussianState() - mean = np.array([[-1.8513], [0.9994], [0], [0]]) * 1e4 - covar = np.array([[2.2128, 0, 0, 0], - [0.0002, 2.2130, 0, 0], - [0.3897, -0.00004, 0.0128, 0], - [0, 0.3897, 0.0013, 0.0135]]) * 1e3 + mean = StateVector([[-1.8513], [0.9994], [0], [0]]) * 1e4 + covar = CovarianceMatrix([[2.2128, 0, 0, 0], + [0.0002, 2.2130, 0, 0], + [0.3897, -0.00004, 0.0128, 0], + [0, 0.3897, 0.0013, 0.0135]]) * 1e3 timestamp = datetime.datetime.now() # Test state initiation without timestamp @@ -59,15 +61,15 @@ def test_gaussianstate(): def test_gaussianstate_invalid_covar(): - mean = np.array([[1], [2], [3], [4]]) # 4D - covar = np.diag([1, 2, 3]) # 3D + mean = StateVector([[1], [2], [3], [4]]) # 4D + covar = CovarianceMatrix(np.diag([1, 2, 3])) # 3D with pytest.raises(ValueError): GaussianState(mean, covar) def test_weighted_gaussian_state(): - mean = np.array([[1], [2], [3], [4]]) # 4D - covar = np.diag([1, 2, 3]) # 3D + mean = StateVector([[1], [2], [3], [4]]) # 4D + covar = CovarianceMatrix(np.diag([1, 2, 3])) # 3D weight = 0.3 with pytest.raises(ValueError): a = WeightedGaussianState(mean, covar, weight) @@ -80,8 +82,8 @@ def test_particlestate(): # 1D num_particles = 10 - state_vector1 = np.array([[0]]) - state_vector2 = np.array([[100]]) + state_vector1 = StateVector([[0.]]) + state_vector2 = StateVector([[100.]]) weight = Probability(1/num_particles) particles = [] particles.extend(Particle( @@ -91,19 +93,19 @@ def test_particlestate(): # Test state without timestamp state = ParticleState(particles) - assert np.allclose(state.state_vector, np.array([[50]])) - assert np.allclose(state.covar, np.array([[2500]])) + assert np.allclose(state.state_vector, StateVector([[50]])) + assert np.allclose(state.covar, CovarianceMatrix([[2500]])) # Test state with timestamp timestamp = datetime.datetime.now() state = ParticleState(particles, timestamp=timestamp) - assert np.allclose(state.state_vector, np.array([[50]])) - assert np.allclose(state.covar, np.array([[2500]])) + assert np.allclose(state.state_vector, StateVector([[50]])) + assert np.allclose(state.covar, CovarianceMatrix([[2500]])) assert state.timestamp == timestamp # 2D - state_vector1 = np.array([[0], [0]]) - state_vector2 = np.array([[100], [200]]) + state_vector1 = StateVector([[0.], [0.]]) + state_vector2 = StateVector([[100.], [200.]]) particles = [] particles.extend(Particle( state_vector1, weight=weight) for _ in range(num_particles//2)) @@ -111,19 +113,19 @@ def test_particlestate(): state_vector2, weight=weight) for _ in range(num_particles//2)) state = ParticleState(particles) - assert np.allclose(state.state_vector, np.array([[50], [100]])) - assert np.allclose(state.covar, np.array([[2500, 5000], [5000, 10000]])) + assert np.allclose(state.state_vector, StateVector([[50], [100]])) + assert np.allclose(state.covar, CovarianceMatrix([[2500, 5000], [5000, 10000]])) def test_particlestate_weighted(): num_particles = 10 # Half particles at high weight at 0 - state_vector1 = np.array([[0]]) + state_vector1 = StateVector([[0.]]) weight1 = Probability(0.75 / (num_particles / 2)) # Other half of particles low weight at 100 - state_vector2 = np.array([[100]]) + state_vector2 = StateVector([[100]]) weight2 = Probability(0.25 / (num_particles / 2)) particles = [] @@ -137,12 +139,30 @@ def test_particlestate_weighted(): # Test state vector is now weighted towards 0 from 50 (non-weighted mean) state = ParticleState(particles) - assert np.allclose(state.state_vector, np.array([[25]])) - assert np.allclose(state.covar, np.array([[1875]])) + assert np.allclose(state.state_vector, StateVector([[25]])) + assert np.allclose(state.covar, CovarianceMatrix([[1875]])) + + +def test_particlestate_angle(): + num_particles = 10 + + state_vector1 = StateVector([[Bearing(np.pi + 0.1)], [-10.]]) + state_vector2 = StateVector([[Bearing(np.pi - 0.1)], [20.]]) + weight = Probability(1/num_particles) + particles = [] + particles.extend(Particle( + state_vector1, weight=weight) for _ in range(num_particles//2)) + particles.extend(Particle( + state_vector2, weight=weight) for _ in range(num_particles//2)) + + # Test state without timestamp + state = ParticleState(particles) + assert np.allclose(state.state_vector, StateVector([[np.pi], [5.]])) + assert np.allclose(state.covar, CovarianceMatrix([[0.01, -1.5], [-1.5, 225]])) def test_state_mutable_sequence_state(): - state_vector = np.array([[0]]) + state_vector = StateVector([[0]]) timestamp = datetime.datetime(2018, 1, 1, 14) delta = datetime.timedelta(minutes=1) sequence = StateMutableSequence( @@ -158,7 +178,7 @@ def test_state_mutable_sequence_state(): def test_state_mutable_sequence_slice(): - state_vector = np.array([[0]]) + state_vector = StateVector([[0]]) timestamp = datetime.datetime(2018, 1, 1, 14) delta = datetime.timedelta(minutes=1) sequence = StateMutableSequence( diff --git a/stonesoup/updater/tests/test_particle.py b/stonesoup/updater/tests/test_particle.py index 39200d2f3..b78222bb1 100644 --- a/stonesoup/updater/tests/test_particle.py +++ b/stonesoup/updater/tests/test_particle.py @@ -18,33 +18,24 @@ def test_particle(): lg = LinearGaussian(ndim_state=2, mapping=[0], noise_covar=np.array([[0.04]])) timestamp = datetime.datetime.now() - particles = [Particle(np.array([[10], [10]]), - 1 / 9), - Particle(np.array([[10], [20]]), - 1 / 9), - Particle(np.array([[10], [30]]), - 1 / 9), - Particle(np.array([[20], [10]]), - 1 / 9), - Particle(np.array([[20], [20]]), - 1 / 9), - Particle(np.array([[20], [30]]), - 1 / 9), - Particle(np.array([[30], [10]]), - 1 / 9), - Particle(np.array([[30], [20]]), - 1 / 9), - Particle(np.array([[30], [30]]), - 1 / 9), + particles = [Particle([[10], [10]], 1 / 9), + Particle([[10], [20]], 1 / 9), + Particle([[10], [30]], 1 / 9), + Particle([[20], [10]], 1 / 9), + Particle([[20], [20]], 1 / 9), + Particle([[20], [30]], 1 / 9), + Particle([[30], [10]], 1 / 9), + Particle([[30], [20]], 1 / 9), + Particle([[30], [30]], 1 / 9), ] prediction = ParticleStatePrediction(particles, timestamp=timestamp) - measurement = Detection(np.array([[20]]), timestamp=timestamp) + measurement = Detection([[20]], timestamp=timestamp) resampler = SystematicResampler() updater = ParticleUpdater(lg, resampler) eval_measurement_prediction = ParticleMeasurementPrediction([ - Particle(i.state_vector[0], 1 / 9) + Particle(i.state_vector[0, :], 1 / 9) for i in particles], timestamp=timestamp) @@ -68,5 +59,4 @@ def test_particle(): == measurement_prediction assert updated_state.hypothesis.prediction == prediction assert updated_state.hypothesis.measurement == measurement - assert np.all( - np.isclose(updated_state.state_vector, np.array([[20], [20]]))) + assert np.allclose(updated_state.state_vector, np.array([[20], [20]])) From 8e7c251c108f441ac85b19f6e35d878eed0844e9 Mon Sep 17 00:00:00 2001 From: Steven Hiscocks Date: Mon, 4 May 2020 09:04:22 +0100 Subject: [PATCH 2/2] Correct return type and documentation in functions --- stonesoup/functions.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/stonesoup/functions.py b/stonesoup/functions.py index cfbb17166..5bdc3e43c 100644 --- a/stonesoup/functions.py +++ b/stonesoup/functions.py @@ -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 @@ -158,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 @@ -170,7 +170,7 @@ 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 @@ -197,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 @@ -216,13 +216,13 @@ 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 @@ -250,7 +250,7 @@ def unscented_transform(sigma_points_states, mean_weights, covar_weights, (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