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)])