Merge pull request #218 from dstl/multi_transition_model

Added MultiTransitionMovingPlatform class

stonesoup/platform/base.py

@@ -428,6 +428,77 @@ def move(self, timestamp=None, **kwargs) -> None:
             timestamp=timestamp)
 
 
+class MultiTransitionMovingPlatform(MovingPlatform):
+    """Moving platform with multiple transition models
+
+    A list of transition models are given with corresponding transition times, dictating the
+    movement behaviour of the platform for given durations.
+    """
+
+    transition_models = Property([TransitionModel], doc="List of transition models")
+    transition_times = Property([datetime.timedelta], doc="Durations for each listed transition "
+                                                          "model")
+
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+        if len(self.transition_models) != len(self.transition_times):
+            raise AttributeError('transition_models and transition_times must be same length')
+
+        self.transition_index = 0
+        self.current_interval = self.transition_times[0]
+
+    @property
+    def transition_model(self):
+        return self.transition_models[self.transition_index]
+
+    def move(self, timestamp=None, **kwargs) -> None:
+        """Propagate the platform position using the :attr:`transition_model`.
+
+        Parameters
+        ----------
+        timestamp: :class:`datetime.datetime`, optional
+            A timestamp signifying the end of the maneuver (the default is ``None``)
+
+        Notes
+        -----
+        This methods updates the value of :attr:`position`.
+
+        Any provided ``kwargs`` are forwarded to the :attr:`transition_model`.
+
+        If :attr:`transition_model` or ``timestamp`` is ``None``, the method has
+        no effect, but will return successfully.
+
+        This method updates :attr:`transition_model`, :attr:`transition_index` and
+        :attr:`current_interval`:
+        If the timestamp provided gives a time delta greater than :attr:`current_interval` the
+        :attr:`transition_model` is called for the rest of its corresponding duration, and the move
+        method is called again on the next transition model (by incrementing
+        :attr:`transition_index`) in :attr:`transition_models` with the residue time delta.
+        If the time delta is less than :attr:`current_interval` the :attr:`transition_model` is
+        called for that duration and :attr:`current_interval` is reduced accordingly.
+        """
+        if self.state.timestamp is None:
+            self.state.timestamp = timestamp
+            return
+        try:
+            time_interval = timestamp - self.state.timestamp
+        except TypeError:
+            # TypeError: (timestamp or prior.timestamp) is None
+            return
+
+        while time_interval != 0:
+            if time_interval >= self.current_interval:
+                super().move(timestamp=self.state.timestamp+self.current_interval, **kwargs)
+                time_interval -= self.current_interval
+                self.transition_index = (self.transition_index + 1) % len(self.transition_models)
+                self.current_interval = self.transition_times[self.transition_index]
+
+            else:
+                super().move(timestamp=self.state.timestamp+time_interval, **kwargs)
+                self.current_interval -= time_interval
+                time_interval = 0
+
+
 def _get_rotation_matrix(vel: StateVector) -> np.ndarray:
     """ Generates a rotation matrix which can be used to determine the
     corrected sensor offsets.

stonesoup/platform/tests/test_platform_base.py

@@ -4,12 +4,12 @@
 import numpy as np
 import pytest
 
+from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, \
+    ConstantVelocity, ConstantTurn
 from stonesoup.sensor.sensor import Sensor
 from stonesoup.types.array import StateVector
+from ..base import MovingPlatform, FixedPlatform, MultiTransitionMovingPlatform
 from ...types.state import State
-from ...models.transition.linear import (
-    ConstantVelocity, CombinedLinearGaussianTransitionModel)
-from ..base import MovingPlatform, FixedPlatform
 
 
 def test_base():
@@ -520,3 +520,96 @@ def test_mapping_types(mapping_type):
                               position_mapping=mapping_type([0, 2, 4]))
     assert np.array_equal(platform.position, StateVector([2, 2, 2]))
     assert np.array_equal(platform.velocity, StateVector([1, -1, 0]))
+
+
+def test_multi_transition():
+    transition_model1 = CombinedLinearGaussianTransitionModel(
+        (ConstantVelocity(0), ConstantVelocity(0)))
+    transition_model2 = ConstantTurn((0, 0), np.radians(4.5))
+
+    transition_models = [transition_model1, transition_model2]
+    transition_times = [datetime.timedelta(seconds=10), datetime.timedelta(seconds=20)]
+
+    platform_state = State(state_vector=[[0], [1], [0], [0]], timestamp=datetime.datetime.now())
+
+    platform = MultiTransitionMovingPlatform(transition_models=transition_models,
+                                             transition_times=transition_times,
+                                             state=platform_state,
+                                             position_mapping=[0, 2],
+                                             sensors=None)
+
+    assert len(platform.transition_models) == 2
+    assert len(platform.transition_times) == 2
+
+    px, py = platform.position[0], platform.position[1]
+    time = datetime.datetime.now()
+    # Starting transition model is index 0
+    assert platform.transition_index == 0
+
+    time += datetime.timedelta(seconds=10)
+    platform.move(timestamp=time)
+    x, y = platform.position[0], platform.position[1]
+    # Platform initially moves horizontally
+    assert x > px
+    assert np.allclose(y, py, atol=1e-6)
+    px, py = x, y
+    # Transition model changes after corresponding interval is done/ Next transition is left-turn
+    assert platform.transition_index == 1
+
+    time += datetime.timedelta(seconds=10)
+    platform.move(timestamp=time)
+    x, y = platform.position[0], platform.position[1]
+    # Platform starts turning left to 45 degrees
+    assert x > px
+    assert y > py
+    # Transition interval is not done. Next transition is left-turn
+    assert platform.transition_index == 1
+
+    time += datetime.timedelta(seconds=10)
+    platform.move(timestamp=time)
+    x, y = platform.position[0], platform.position[1]
+    px, py = x, y
+    # Platform turned left to 90 degrees
+    # Transition interval is done. Next transition is straight-on
+    assert platform.transition_index == 0
+
+    time += datetime.timedelta(seconds=10)
+    platform.move(timestamp=time)
+    x, y = platform.position[0], platform.position[1]
+    # Platform travelling vertically up
+    assert np.allclose(x, px, atol=1e-6)
+    assert y > py
+    # Next transition is left-turn
+    assert platform.transition_index == 1
+
+    # Add new transition model (right-turn) to list
+    transition_model3 = ConstantTurn((0, 0), np.radians(-9))
+    platform.transition_models.append(transition_model3)
+    platform.transition_times.append(datetime.timedelta(seconds=10))
+
+    # New model and transition interval are added to model list and to interval list
+    assert len(platform.transition_models) == 3
+    assert len(platform.transition_times) == 3
+
+    time += datetime.timedelta(seconds=20)
+    platform.move(timestamp=time)
+    # Platform turned left by 90 degrees (now travelling in -x direction)
+    px, py = platform.position[0], platform.position[1]
+    # Next transition is right-turn
+    assert platform.transition_index == 2
+
+    time += datetime.timedelta(seconds=10)
+    platform.move(timestamp=time)
+    x, y = platform.position[0], platform.position[1]
+    px, py = x, y
+    # Next transition straight-on, travelling vertically up again
+    assert platform.transition_index == 0
+
+    time += datetime.timedelta(seconds=10)
+    platform.move(timestamp=time)
+    x, y = platform.position[0], platform.position[1]
+    # Platform travelled vertically up
+    assert np.allclose(x, px, atol=1e-6)
+    assert y > py
+    # Next transition is left-turn
+    assert platform.transition_index == 1