def test_velocity_properties(velocity_mapping): model_1d = ConstantVelocity(0.0) model_3d = CombinedLinearGaussianTransitionModel( [model_1d, model_1d, model_1d]) timestamp = datetime.datetime.now() timediff = 2 # 2sec new_timestamp = timestamp + datetime.timedelta(seconds=timediff) platform_state = State(np.array([[2], [0], [2], [0], [0], [0]]), timestamp) platform = MovingPlatform(states=platform_state, transition_model=model_3d, position_mapping=[0, 2, 4]) old_position = platform.position assert not platform.is_moving assert np.array_equal(platform.velocity, StateVector([0, 0, 0])) # check it doesn't move with timestamp = None platform.move(timestamp=None) assert np.array_equal(platform.position, old_position) # check it doesn't move (as it has zero velocity) platform.move(timestamp) assert np.array_equal(platform.position, old_position) platform.move(new_timestamp) assert np.array_equal(platform.position, old_position) platform_state = State(np.array([[2], [1], [2], [1], [0], [1]]), timestamp) platform = MovingPlatform(states=platform_state, transition_model=None, position_mapping=[0, 2, 4]) assert platform.is_moving assert np.array_equal(platform.velocity, StateVector([1, 1, 1])) old_position = platform.position # check it doesn't move with timestamp = None platform.move(None) assert np.array_equal(platform.position, old_position) with pytest.raises(AttributeError): platform.move(timestamp) # moving platform without velocity defined platform_state = State(np.array([[2], [2], [0]]), timestamp) platform = MovingPlatform(states=platform_state, transition_model=None, position_mapping=[0, 2, 4]) with pytest.raises(AttributeError): _ = platform.velocity # pass in a velocity mapping platform_state = State(np.array([[2], [1], [2], [1], [0], [1]]), timestamp) platform = MovingPlatform(states=platform_state, transition_model=None, position_mapping=[0, 2, 4], velocity_mapping=velocity_mapping) assert platform.is_moving assert np.array_equal(platform.velocity, StateVector([1, 1, 1])) old_position = platform.position # check it doesn't move with timestamp = None platform.move(None) assert np.array_equal(platform.position, old_position) with pytest.raises(AttributeError): platform.move(timestamp) # moving platform without velocity defined platform_state = State(np.array([[2], [2], [0]]), timestamp) platform = MovingPlatform(states=platform_state, transition_model=None, position_mapping=[0, 2, 4], velocity_mapping=velocity_mapping) with pytest.raises(AttributeError): _ = platform.velocity
# have the following :math:`\mathbf{x}`. # # .. math:: # \mathbf{x} = \begin{bmatrix} # x\\ \dot{x}\\ y\\ \dot{y}\\ z\\ \dot{z} \end{bmatrix} # = \begin{bmatrix} # 0\\ 0\\ 0\\ 0\\ 0\\ 0 \end{bmatrix} # # Because the platform is static we only need to define :math:`(x, y, z)`, any internal interaction with the platform # which requires knowledge of platform velocity :math:`(\dot{x}, \dot{y}, \dot{z})` will be returned :math:`(0, 0, 0)`. # First import the fixed platform from stonesoup.platform.base import FixedPlatform # Define the initial platform position, in this case the origin platform_state_vector = StateVector([[0], [0], [0]]) position_mapping = (0, 1, 2) # Create the initial state (position, time), notice it is set to the simulation start time defined earlier platform_state = State(platform_state_vector, start_time) # create our fixed platform platform = FixedPlatform(states=platform_state, position_mapping=position_mapping) # %% # We have now created a platform within Stone Soup and located it at the origin of our state space. As previously stated # the platform will have a velocity :math:`(\dot{x}, \dot{y}, \dot{z})` of :math:`(0, 0, 0)` which we can check: platform.velocity # %%
def test_moving_with_no_initial_timestamp(): timestamp = datetime.datetime.now() platform_state = State(StateVector([2, 1, 1, 1, 2, 0]), timestamp=None) platform = MovingPlatform(states=platform_state, position_mapping=[0, 2, 4], transition_model=None) assert platform.timestamp is None platform.move(timestamp=timestamp) assert platform.timestamp == timestamp orientation_tests_3d = [ (StateVector([0, 1, 0, 0, 0, 0]), StateVector([0, 0, 0])), (StateVector([0, 0, 0, 0, 0, 1]), StateVector([0, np.pi / 2, 0])), (StateVector([0, 0, 0, 1, 0, 0]), StateVector([0, 0, np.pi / 2])), (StateVector([0, 2, 0, 0, 0, 0]), StateVector([0, 0, 0])), (StateVector([0, 0, 0, 0, 0, 2]), StateVector([0, np.pi / 2, 0])), (StateVector([0, 0, 0, 2, 0, 0]), StateVector([0, 0, np.pi / 2])), (StateVector([0, 1, 0, 0, 0, 1]), StateVector([0, np.pi / 4, 0])), (StateVector([0, 0, 0, 1, 0, 1]), StateVector([0, np.pi / 4, np.pi / 2])), (StateVector([0, 1, 0, 1, 0, 1]), StateVector([0, np.arctan(1 / np.sqrt(2)), np.pi / 4])), (StateVector([0, -1, 0, 0, 0, 0]), StateVector([0, 0, np.pi])), (StateVector([0, 0, 0, -1, 0, 0]), StateVector([0, 0, -np.pi / 2])), (StateVector([0, 0, 0, 0, 0, -1]), StateVector([0, -np.pi / 2, 0])), (StateVector([0, -2, 0, 0, 0, 0]), StateVector([0, 0, np.pi])), (StateVector([0, 0, 0, -2, 0, 0]), StateVector([0, 0, -np.pi / 2])), (StateVector([0, 0, 0, 0, 0, -2]), StateVector([0, -np.pi / 2, 0])),
# %% # Separate out the imports import numpy as np import datetime # %% # Initialise ground truth # ^^^^^^^^^^^^^^^^^^^^^^^ # Here are some configurable parameters associated with the ground truth, e.g. defining where # tracks are born and at what rate, death probability. This follows similar logic to the code # in previous tutorial section :ref:`auto_tutorials/09_Initiators_&_Deleters:Simulating Multiple # Targets`. from stonesoup.types.array import StateVector, CovarianceMatrix from stonesoup.types.state import GaussianState initial_state_mean = StateVector([[0], [0], [0], [0]]) initial_state_covariance = CovarianceMatrix(np.diag([4, 0.5, 4, 0.5])) timestep_size = datetime.timedelta(seconds=5) number_of_steps = 20 birth_rate = 0.3 death_probability = 0.05 initial_state = GaussianState(initial_state_mean, initial_state_covariance) # %% # Create the transition model - default set to 2d nearly-constant velocity with small (0.05) # variance. from stonesoup.models.transition.linear import ( CombinedLinearGaussianTransitionModel, ConstantVelocity) transition_model = CombinedLinearGaussianTransitionModel( [ConstantVelocity(0.05), ConstantVelocity(0.05)])