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
temp_state = self.state
while time_interval != 0:
if time_interval >= self.current_interval:
temp_state = State(state_vector=self.transition_model.function(
state=temp_state,
noise=True,
time_interval=self.current_interval,
**kwargs),
timestamp=timestamp)
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:
temp_state = State(state_vector=self.transition_model.function(
state=temp_state,
noise=True,
time_interval=time_interval,
**kwargs),
timestamp=timestamp)
self.current_interval -= time_interval
time_interval = 0
self.states.append(temp_state)
def test_multi_transition_movable_errors():
# First check no error
models = [ConstantVelocity(0), ConstantTurn(0, np.pi/2)]
now = datetime.now()
times = [timedelta(seconds=10), timedelta(seconds=10)]
_ = MultiTransitionMovable(states=State(StateVector([0, 0, 0, 0, 0, 0])),
position_mapping=[0, 2, 4],
velocity_mapping=[1, 2, 5],
transition_models=models,
transition_times=times,
)
with pytest.raises(AttributeError,
match='transition_models and transition_times must be same length'):
_ = MultiTransitionMovable(states=State(StateVector([0, 0, 0, 0, 0, 0])),
position_mapping=[0, 2, 4],
velocity_mapping=[1, 2, 5],
transition_models=[models[0]],
transition_times=times,
)
with pytest.raises(AttributeError,
match='transition_models and transition_times must be same length'):
_ = MultiTransitionMovable(states=State(StateVector([0, 0, 0, 0, 0, 0])),
position_mapping=[0, 2, 4],
velocity_mapping=[1, 2, 5],
transition_models=models,
transition_times=[now],
)
def test_transitions_list(coords, turn_rate):
x_coords = coords[0]
y_coords = coords[1]
x = x_coords[0]
y = y_coords[0]
vx = (x_coords[1] - x_coords[0]) / 10 # set sensible initial velocity
vy = (y_coords[1] - y_coords[0]) / 10
state_vector = StateVector([x, vx, y, vy])
target = State(state_vector=state_vector, timestamp=start)
transition_models, transition_times = create_smooth_transition_models(initial_state=target,
x_coords=x_coords,
y_coords=y_coords,
times=times,
turn_rate=turn_rate)
for transition_model in transition_models:
assert transition_model.ndim_state == 4
for i in range(len(transition_models)):
target.state_vector = transition_models[i].function(state=target,
time_interval=transition_times[i])
target.timestamp += transition_times[i]
if target.timestamp in times:
index = times.index(target.timestamp)
# expect target to be at next coordinates at each time-step
assert np.isclose(target.state_vector[0], x_coords[index])
assert np.isclose(target.state_vector[2], y_coords[index])
def test_fixed_movable_velocity_mapping_error():
# test no error for MovingMovable
_ = MovingMovable(states=State(StateVector([0, 0, 0, 0, 0, 0])),
position_mapping=[0, 2, 4],
velocity_mapping=[1, 2, 5],
transition_model=None,
)
with pytest.raises(ValueError, match='Velocity mapping should not be set for a FixedMovable'):
_ = FixedMovable(states=State(StateVector([0, 0, 0, 0, 0, 0])),
position_mapping=[0, 2, 4],
velocity_mapping=[1, 2, 5],
)
def test_multi_transition_movable_move():
input_state_vector = StateVector([0, 1, 2.2, 78.6])
pre_state = State(input_state_vector, timestamp=None)
models = [CombinedLinearGaussianTransitionModel((ConstantVelocity(0), ConstantVelocity(0))),
ConstantTurn([0, 0], turn_rate=np.pi / 2)]
times = [timedelta(seconds=10), timedelta(seconds=10)]
movable = MultiTransitionMovable(states=pre_state,
position_mapping=[0, 2],
velocity_mapping=[1, 3],
transition_models=models,
transition_times=times,
)
assert movable.state.state_vector is input_state_vector
assert movable.state.timestamp is None
now = datetime.now()
movable.move(now)
assert movable.state.state_vector is input_state_vector
assert movable.state.timestamp is now
movable.move(None)
assert movable.state.state_vector is input_state_vector
assert movable.state.timestamp is now
movable.move(now + timedelta(seconds=10))
assert movable.state.state_vector is not input_state_vector
assert movable.state.timestamp is not now
def test_empty_state_error():
# first, check no error
_ = MovingMovable(states=State(StateVector([0, 0, 0, 0, 0, 0])),
position_mapping=[0, 2, 4],
velocity_mapping=[1, 2, 5],
transition_model=None,
)
with pytest.raises(ValueError, match='States must not be empty'):
_ = MovingMovable(position_mapping=[0, 2, 4],
velocity_mapping=[1, 2, 5],
transition_model=None,
)
with pytest.raises(ValueError, match='States must not be empty'):
_ = MovingMovable(states=[],
position_mapping=[0, 2, 4],
velocity_mapping=[1, 2, 5],
transition_model=None,
)
def __init__(self, *args, **kwargs):
position = kwargs.pop('position', None)
orientation = kwargs.pop('orientation', None)
self.__has_internal_controller = False
super().__init__(*args, **kwargs)
if position is not None or orientation is not None:
if position is None:
# assuming 3d for a default platform
position = StateVector([0, 0, 0])
if orientation is None:
orientation = StateVector([0, 0, 0])
controller = FixedMovable(states=State(state_vector=position),
position_mapping=list(
range(len(position))),
orientation=orientation)
self._property_movement_controller = controller
self.__has_internal_controller = True
self._set_mounting_rotation_defaults()
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 when 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 `timestamp`` is ``None``, the method has no effect, but will return successfully.
"""
if self.state.timestamp is None:
self.state.timestamp = timestamp
return
# Compute time_interval
try:
time_interval = timestamp - self.state.timestamp
except TypeError:
# TypeError: (timestamp or prior.timestamp) is None
return
if self.transition_model is None:
raise AttributeError(
'Platform without a transition model cannot be moved')
self.states.append(
State(state_vector=self.transition_model.function(
state=self.state,
noise=True,
timestamp=timestamp,
time_interval=time_interval,
**kwargs),
timestamp=timestamp))
def test_coordinate_length():
x_coords = np.zeros(3)
y_coords = np.zeros(2)
times = [start + timedelta(seconds=10*i) for i in range(3)]
x = x_coords[0]
y = y_coords[0]
vx = (x_coords[1] - x_coords[0]) / 10 # set sensible initial velocity
vy = (y_coords[1] - y_coords[0]) / 10
turn_rate = 5
state_vector = StateVector([x, vx, y, vy])
target = State(state_vector=state_vector, timestamp=start)
with pytest.raises(ValueError):
# x and y lists not equal in length (x longer)
create_smooth_transition_models(initial_state=target,
x_coords=x_coords,
y_coords=y_coords,
times=times,
turn_rate=turn_rate)
with pytest.raises(ValueError):
# x and y lists not equal in length (y longer)
create_smooth_transition_models(initial_state=target,
x_coords=y_coords,
y_coords=x_coords,
times=times,
turn_rate=turn_rate)
times = [start + timedelta(seconds=10*i) for i in range(4)]
with pytest.raises(ValueError):
# times not equal to coordinates lists
create_smooth_transition_models(initial_state=target,
x_coords=x_coords,
y_coords=x_coords,
times=times,
turn_rate=turn_rate)
from matplotlib import pyplot as plt
start = datetime.now()
X = Y = np.array([0, -10, -20])
Y = np.array([0, 10, 0])
times = [start, start + timedelta(seconds=10), start + timedelta(seconds=30)]
# %%
# Plotting the three target destinations (in blue) and the target initial bearing in green:
from stonesoup.types.array import StateVector
from stonesoup.types.state import State
ux = 1 # initial x-speed
uy = 1 # initial y-speed
platform_state = State(StateVector((X[0], ux, Y[0], uy)), timestamp=start)
fig = plt.figure(figsize=(10, 6))
ax = fig.add_subplot(1, 1, 1)
ax.set_xlabel("$x$")
ax.set_ylabel("$y$")
ax.axis('equal')
ax.scatter(X, Y, color='lightskyblue', s=100, edgecolors='black')
ax.plot((X[0], X[0] + 2 * ux), (Y[0], Y[0] + 2 * uy),
color='lightgreen',
linewidth=2)
# %%
# :meth:`~simulator.transition.get_smooth_transition_models` requires the initial state of the
# platform, x and y coordinates, times to be at each respective coordinate and the platform
# turn-rate.
from stonesoup.models.transition.linear import (
CombinedLinearGaussianTransitionModel, ConstantVelocity)
from stonesoup.platform.base import MovingPlatform
from stonesoup.types.state import State
time_step = datetime.timedelta(seconds=0.1)
time_init = datetime.datetime.now()
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(1.),
ConstantVelocity(1.),
ConstantVelocity(1.)])
red = MovingPlatform(position_mapping=[0, 2, 4],
velocity_mapping=[0, 2, 4],
states=State([50., 0., -50., 10., 0., 0.],
timestamp=time_init),
transition_model=transition_model)
# %%
# Move target
for s in range(1, 100):
red.move(time_init + s * time_step)
# %%
# Measure target states
# ^^^^^^^^^^^^^^^^^^^^^
# The states are measured with and without noise to show the real position with the measured one.
measurements = []
noiseless_measurements = []
for state in red.states:
# .. math::
# F_{k} = \begin{bmatrix}
# 1 & \triangle k & 0 & 0 & 0 & 0\\
# 0 & 1 & 0 & 0 & 0 & 0\\
# 0 & 0 & 1 & \triangle k & 0 & 0\\
# 0 & 0 & 0 & 1 & 0 & 0\\
# 0 & 0 & 0 & 0 & 1 & \triangle k \\
# 0 & 0 & 0 & 0 & 0 & 1\\
# \end{bmatrix}
# First import the Moving platform
from stonesoup.platform.base import MovingPlatform
# Define the initial platform position, in this case the origin
initial_loc = StateVector([[0], [0], [0], [50], [8000], [0]])
initial_state = State(initial_loc, start_time)
# Define transition model and position for 3D platform
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(0.), ConstantVelocity(0.), ConstantVelocity(0.)])
# create our fixed platform
sensor_platform = MovingPlatform(states=initial_state,
position_mapping=(0, 2, 4),
velocity_mapping=(1, 3, 5),
transition_model=transition_model)
# %%
# With our platform generated we now need to build a set of sensors which will be mounted onto the platform. In this
# case we will exploit a :class:`~.RadarElevationBearingRangeRate` and a :class:`~.PassiveElevationBearing` sensor
# (e.g. an optical sensor, which has no capability to directly measure range).
# 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
# %%
# We can also query the platform orientation:
platform.orientation
# %%
# Create specific transition model for example moving platform
transition_models = [transition_modelStraight, transition_modelLeft]
transition_times = [
datetime.timedelta(seconds=160),
datetime.timedelta(seconds=20)
]
# List sensors in stationary platforms (sensor orientations are overwritten)
stationarySensors = [
RadarRotatingBearingRange(
ndim_state=6,
position_mapping=(0, 2),
noise_covar=np.diag([np.radians(1)**2,
7**2]), # The sensor noise covariance matrix
dwell_center=State(StateVector([[-np.pi]])),
rpm=12.5,
max_range=100000,
fov_angle=np.radians(360)),
RadarRotatingBearingRange(ndim_state=6,
position_mapping=(0, 2),
noise_covar=np.diag([np.radians(1)**2, 7**2]),
dwell_center=State(StateVector([[-np.pi]])),
rpm=12.5,
max_range=100000,
fov_angle=np.radians(360)),
]
# List sensors in moving platform (sensor orientations are overwritten)
movingPlatformSensors = [
RadarRotatingBearingRange(ndim_state=6,
def test_alphabeta(measurement_model, prediction, measurement, alpha, beta):
# Time delta
timediff = timedelta(seconds=2)
# Calculate evaluation variables - converts
# to measurement from prediction space
eval_measurement_prediction = StateMeasurementPrediction(
measurement_model.matrix() @ prediction.state_vector)
eval_posterior_position = prediction.state_vector[[0, 2]] + \
alpha * (measurement.state_vector - eval_measurement_prediction.state_vector)
eval_posterior_velocity = prediction.state_vector[[1, 3]] + \
beta/timediff.total_seconds() * (measurement.state_vector -
eval_measurement_prediction.state_vector)
eval_state_vect = np.concatenate(
(eval_posterior_position, eval_posterior_velocity))
eval_posterior = State(eval_state_vect[[0, 2, 1, 3]])
# Initialise an Alpha-Beta updater
updater = AlphaBetaUpdater(measurement_model=measurement_model,
alpha=alpha,
beta=beta)
# Get and assert measurement prediction
measurement_prediction = updater.predict_measurement(prediction)
assert (np.allclose(measurement_prediction.state_vector,
eval_measurement_prediction.state_vector,
0,
atol=1.e-14))
# Perform and assert state update (without measurement prediction)
posterior = updater.update(SingleHypothesis(prediction=prediction,
measurement=measurement),
time_interval=timediff)
assert (np.allclose(posterior.state_vector,
eval_posterior.state_vector,
0,
atol=1.e-14))
assert (np.array_equal(posterior.hypothesis.prediction, prediction))
assert (np.array_equal(posterior.hypothesis.measurement, measurement))
assert (posterior.timestamp == prediction.timestamp)
# Check that the vmap parameter can be set
# Check a measurement prediction can be added
updater.vmap = np.array([1, 3])
posterior = updater.update(SingleHypothesis(
prediction=prediction,
measurement=measurement,
measurement_prediction=measurement_prediction),
time_interval=timediff)
assert (np.allclose(posterior.state_vector,
eval_posterior.state_vector,
0,
atol=1.e-14))
# Finally check that no model in the updater raises correct error
updater.measurement_model = None
with pytest.raises(ValueError):
updater._check_measurement_model(None)
# ------------------------------------
# The sensor converts the Cartesian coordinates into range, bearing and elevation.
# The sensor is then mounted onto a platform (stationary in this case)
from stonesoup.platform.base import FixedPlatform
from stonesoup.sensor.radar.radar import RadarElevationBearingRange
from stonesoup.simulator.platform import PlatformDetectionSimulator
from stonesoup.types.state import State
sensor = RadarElevationBearingRange(
[0, 2, 4],
meas_covar,
6,
)
platform = FixedPlatform(
State([0, 0, 0, 0, 0, 0]), # Sensor at reference point, zero velocity
[0, 2, 4],
sensors=[sensor])
# Create the detector and initialize it.
detector = PlatformDetectionSimulator(ground_truth_reader, [platform])
# %%
# Setup Initiator class for the Tracker
# ---------------------------------------
# This is just an heuristic initiation:
# Assume most of the deviation is caused by the Bearing measurement error.
# This is then converted into x, y components using the target bearing. For the
# deviation in z,
# we simply use :math:`R\times\sigma_{elev}` (ignore any bearing and range
# deviation components). Velocity covariances are simply based on the expected