Update to platform to allow it to control the positions of its sensors

stonesoup/functions.py

@@ -1,11 +1,12 @@
 # -*- coding: utf-8 -*-
 """Mathematical functions used within Stone Soup"""
+from typing import List, Union
 
 import numpy as np
 from copy import copy
 
 from .types.numeric import Probability
-from .types.array import Matrix
+from .types.array import Matrix, StateVector
 
 
 def tria(matrix):
@@ -566,3 +567,26 @@ def mod_elevation(x):
     elif N == 3:
         x = x - 2.0 * np.pi
     return x
+
+
+def coerce_to_valid_mapping(mapping: Union[List[int], np.ndarray, StateVector]):
+    """Function to take a mapping and convert it to a suitable form for indexing a
+    :class:`StateVector`. Needed because if you index a :class:`StateVector` with another
+    :class:`StateVector` (which would be the obvious technique) you get a 3d :class:`ndarray`.
+
+    The input as either a List, :class:`ndarray` or a :class:`StateVector`, all of which should
+    contain integers. It returns a 1d :class:`ndarray` of with `shape == (ndim,)` where `ndim` is
+    the number of elements in the mapping array.
+
+    Parameters
+    ----------
+    mapping: :class:`Union[List, np.ndarray]`
+        A mapping that needs to be formatted to a valid mapping shape
+
+    Returns
+    -------
+    mapping: :class:`np.ndarray`
+        A flattened np.ndarray suitable for indexing a :class:`StateVector`
+    """
+
+    return np.asarray(mapping).flatten()

stonesoup/models/measurement/base.py

@@ -1,18 +1,22 @@
 # -*- coding: utf-8 -*-
-from abc import abstractmethod
+from abc import abstractmethod, ABC
 
-import scipy as sp
+import numpy as np
 
+from stonesoup.functions import coerce_to_valid_mapping
 from ..base import Model
 from ...base import Property
 
 
-class MeasurementModel(Model):
+class MeasurementModel(Model, ABC):
     """Measurement Model base class"""
 
     ndim_state = Property(int, doc="Number of state dimensions")
-    mapping = Property(
-        sp.ndarray, doc="Mapping between measurement and state dims")
+    mapping = Property(np.ndarray, doc="Mapping between measurement and state dims")
+
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+        self.mapping = coerce_to_valid_mapping(self.mapping)
 
     @property
     def ndim(self):

stonesoup/models/measurement/nonlinear.py

@@ -1,4 +1,6 @@
 # -*- coding: utf-8 -*-
+from abc import ABC
+from typing import List
 import copy
 
 import numpy as np
@@ -15,8 +17,7 @@
 from .base import MeasurementModel
 
 
-class CombinedReversibleGaussianMeasurementModel(
-        ReversibleModel, GaussianModel, MeasurementModel):
+class CombinedReversibleGaussianMeasurementModel(ReversibleModel, GaussianModel, MeasurementModel):
     r"""Combine multiple models into a single model by stacking them.
 
     The assumption is that all models are Gaussian, and must be combination of
@@ -29,8 +30,7 @@ class CombinedReversibleGaussianMeasurementModel(
     :class:`~.LinearModel` or :class:`~.ReversibleModel`.
     """
     mapping = None
-    model_list = Property(
-        [MeasurementModel], doc="List of Measurement Models.")
+    model_list = Property(List[MeasurementModel], doc="List of Measurement Models.")
 
     def __init__(self, *args, **kwargs):
         super().__init__(*args, **kwargs)
@@ -90,9 +90,7 @@ def rvs(self, num_samples=1, **kwargs):
             return rvs_vectors.view(Matrix)
 
 
-class NonLinearGaussianMeasurement(MeasurementModel,
-                                   NonLinearModel,
-                                   GaussianModel):
+class NonLinearGaussianMeasurement(MeasurementModel, NonLinearModel, GaussianModel, ABC):
     r"""This class combines the MeasurementModel, NonLinearModel and \
     GaussianModel classes. It is not meant to be instantiated directly \
     but subclasses should be derived from this class.
@@ -272,8 +270,7 @@ def rvs(self, num_samples=1, **kwargs):
         return out
 
 
-class CartesianToBearingRange(
-        NonLinearGaussianMeasurement, ReversibleModel):
+class CartesianToBearingRange(NonLinearGaussianMeasurement, ReversibleModel):
     r"""This is a class implementation of a time-invariant measurement model, \
     where measurements are assumed to be received in the form of bearing \
     (:math:`\phi`) and range (:math:`r`), with Gaussian noise in each dimension.

stonesoup/platform/base.py

@@ -1,21 +1,143 @@
 # -*- coding: utf-8 -*-
+from abc import abstractmethod, ABC
 
+from ..functions import cart2sphere, cart2pol, coerce_to_valid_mapping
+from ..types.array import StateVector
 from ..base import Base, Property
 from ..types.state import State
 from ..models.transition import TransitionModel
+import numpy as np
 
 
-class Platform(Base):
-    """Platform base class
+class Platform(Base, ABC):
+    state = Property(State, doc="The platform state at any given point")
+    mapping = Property(np.ndarray, doc="Mapping between platform position and state dims")
+
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+        self.mapping = coerce_to_valid_mapping(self.mapping)
+
+    @property
+    def state_vector(self):
+        return self.state.state_vector
+
+    @property
+    def position(self):
+        return self.state_vector[self.mapping]
+
+    @position.setter
+    def position(self, value):
+        self._set_position(value)
+
+    @property
+    def ndim(self):
+        return len(self.mapping)
+
+    @property
+    @abstractmethod
+    def orientation(self):
+        raise NotImplementedError
+
+    @property
+    @abstractmethod
+    def velocity(self):
+        raise NotImplementedError
+
+    @property
+    @abstractmethod
+    def acceleration(self):
+        raise NotImplementedError
+
+    @abstractmethod
+    def is_moving(self):
+        raise NotImplementedError
+
+    @abstractmethod
+    def move(self, timestamp):
+        raise NotImplementedError
+
+    @abstractmethod
+    def _set_position(self, value):
+        raise NotImplementedError
+
+
+class FixedPlatform(Platform):
+    orientation = Property(StateVector, default=StateVector([0, 0, 0]),
+                           doc='A fixed orientation of the static platform')
+
+    def _set_position(self, value):
+        self.state_vector[self.mapping] = value
+
+    @property
+    def velocity(self):
+        return StateVector([0] * self.ndim)
+
+    @property
+    def acceleration(self):
+        return StateVector([0] * self.ndim)
+
+    @property
+    def is_moving(self):
+        return False
+
+    def move(self, timestamp):
+        # Return without moving static platforms
+        self.state.timestamp = timestamp
+
+
+class MovingPlatform(Platform):
+    """Moving platform base class
 
     A platform represents a random object defined as a :class:`~.State`
     that moves according to a given :class:`~.TransitionModel`.
     """
-
-    state = Property(State, doc="The platform state at any given point")
     transition_model = Property(
         TransitionModel, doc="Transition model")
 
+    @property
+    def velocity(self):
+        # TODO docs
+        # TODO return zeros?
+        try:
+            return self.state_vector[self.mapping + 1]
+        except IndexError:
+            raise AttributeError('Velocity is not defined for this platform')
+
+    @property
+    def acceleration(self):
+        # TODO docs
+        # TODO return zeros?
+        try:
+            return self.state_vector[self.mapping + 2]
+        except IndexError:
+            raise AttributeError('Acceleration is not defined for this platform')
+
+    @property
+    def orientation(self):
+        # TODO docs
+        # TODO handle roll?
+        if not self.is_moving:
+            raise AttributeError('Orientation of a zero-velocity moving platform is not defined')
+        velocity = self.velocity
+
+        if self.ndim == 3:
+            _, bearing, elevation = cart2sphere(*velocity.flat)
+            return StateVector([0, bearing, elevation])
+        elif self.ndim == 2:
+            _, bearing = cart2pol(*velocity.flat)
+            return StateVector([0, bearing])
+
+    @property
+    def is_moving(self):
+        # TODO docs
+        # Note: a platform without a transition model can be given a velocity as part of it's
+        # StateVector. It just won't move
+        # This inconsistency will be handled in the move logic
+        return np.any(self.velocity != 0)
+
+    def _set_position(self, value):
+        raise AttributeError('Cannot set the position of a moving platform')
+
     def move(self, timestamp=None, **kwargs):
         """Propagate the platform position using the :attr:`transition_model`.
 
@@ -40,12 +162,10 @@ def move(self, timestamp=None, **kwargs):
             time_interval = timestamp - self.state.timestamp
         except TypeError:
             # TypeError: (timestamp or prior.timestamp) is None
-            time_interval = None
+            return
 
-        # Return without moving static platforms
         if self.transition_model is None:
-            self.state.timestamp = timestamp
-            return self
+            raise AttributeError('Platform without a transition model cannot be moved')
 
         self.state = State(
             state_vector=self.transition_model.function(