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 __init__(self,
start_time,
prior: GaussianState,
sigma_process_radar=0.01,
sigma_process_ais=0.01,
sigma_meas_radar=3,
sigma_meas_ais=1):
"""
:param measurements_radar:
:param measurements_ais:
:param start_time:
:param prior:
:param sigma_process_radar:
:param sigma_process_ais:
:param sigma_meas_radar:
:param sigma_meas_ais:
"""
self.start_time = start_time
# transition models (process models)
self.transition_model_radar = CombinedLinearGaussianTransitionModel([
ConstantVelocity(sigma_process_radar),
ConstantVelocity(sigma_process_radar)
])
self.transition_model_ais = CombinedLinearGaussianTransitionModel([
ConstantVelocity(sigma_process_ais),
ConstantVelocity(sigma_process_ais)
])
# Specify measurement model for radar
self.measurement_model_radar = LinearGaussian(
ndim_state=4, # number of state dimensions
mapping=(0, 2), # mapping measurement vector index to state index
noise_covar=np.array([
[sigma_meas_radar, 0], # covariance matrix for Gaussian PDF
[0, sigma_meas_radar]
]))
# Specify measurement model for AIS
self.measurement_model_ais = LinearGaussian(ndim_state=4,
mapping=(0, 2),
noise_covar=np.array(
[[sigma_meas_ais, 0],
[0, sigma_meas_ais]]))
# specify predictors
self.predictor_radar = KalmanPredictor(self.transition_model_radar)
self.predictor_ais = KalmanPredictor(self.transition_model_ais)
# specify updaters
self.updater_radar = KalmanUpdater(self.measurement_model_radar)
self.updater_ais = KalmanUpdater(self.measurement_model_ais)
# create prior, both trackers use the same starting point
self.prior_radar = prior
self.prior_ais = prior
def __init__(self,
measurements_radar,
measurements_ais,
start_time,
prior: GaussianState,
sigma_process=0.01,
sigma_meas_radar=3,
sigma_meas_ais=1):
"""
:param measurements_radar:
:param measurements_ais:
:param start_time:
:param prior:
:param sigma_process:
:param sigma_meas_radar:
:param sigma_meas_ais:
"""
# measurements and start time
self.measurements_radar = measurements_radar
self.measurements_ais = measurements_ais
self.start_time = start_time
# transition model
self.transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(sigma_process),
ConstantVelocity(sigma_process)])
# same measurement models as used when generating the measurements
# Specify measurement model for radar
self.measurement_model_radar = LinearGaussian(
ndim_state=4, # number of state dimensions
mapping=(0, 2), # mapping measurement vector index to state index
noise_covar=np.array([
[sigma_meas_radar, 0], # covariance matrix for Gaussian PDF
[0, sigma_meas_radar]
]))
# Specify measurement model for AIS
self.measurement_model_ais = LinearGaussian(ndim_state=4,
mapping=(0, 2),
noise_covar=np.array(
[[sigma_meas_ais, 0],
[0, sigma_meas_ais]]))
# specify predictor
self.predictor = KalmanPredictor(self.transition_model)
# specify updaters
self.updater_radar = KalmanUpdater(self.measurement_model_radar)
self.updater_ais = KalmanUpdater(self.measurement_model_ais)
# create prior todo move later and probably rename
self.prior = prior
def test_platform_getitem():
timestamp = datetime.datetime.now()
state_before = State(np.array([[2], [1], [2], [1], [0], [1]]), timestamp)
cv_model = CombinedLinearGaussianTransitionModel(
(ConstantVelocity(0), ConstantVelocity(0), ConstantVelocity(0)))
platform = MovingPlatform(states=state_before,
transition_model=cv_model,
position_mapping=[0, 2, 4],
velocity_mapping=[1, 3, 5])
platform.move(timestamp + datetime.timedelta(seconds=1))
state_after = platform.state
assert platform[0] is state_before
assert platform[1] is state_after
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_platform_orientation_2d(state, orientation):
model_1d = ConstantVelocity(0.0)
model_2d = CombinedLinearGaussianTransitionModel([model_1d, model_1d])
timestamp = datetime.datetime.now()
timediff = 2 # 2sec
new_timestamp = timestamp + datetime.timedelta(seconds=timediff)
platform_state = State(state, timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=model_2d,
position_mapping=[0, 2])
assert np.allclose(platform.orientation, orientation)
# moving with a constant velocity model should not change the orientation
platform.move(new_timestamp)
assert np.allclose(platform.orientation, orientation)
#from IPython import get_ipython
#get_ipython().run_line_magic('matplotlib', 'inline')
import matplotlib
from matplotlib import pyplot as plt
plt.rcParams['figure.figsize'] = (10, 6)
plt.style.use('seaborn-colorblind')
# 2. Generate Data
#MODELS
# Linear Gaussian transition model like in the 01-Kalman example
from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity
transition_model = CombinedLinearGaussianTransitionModel(
(ConstantVelocity(1), ConstantVelocity(1)))
# Use a Linear Gaussian measurement model like in 01-Kalman example
from stonesoup.models.measurement.linear import LinearGaussian
measurement_model = LinearGaussian(
ndim_state=4, # Number of state dimensions (position and velocity in 2D)
mapping=[0, 2], # Mapping measurement vector index to state index
noise_covar=np.diag([10, 10]) # Covariance matrix for Gaussian PDF
)
#SIMULATORS
# GROUND TRUTH SIMULATOR
# Before running the Detection Simulator, a Ground Truth reader/simulator must be used in order to generate
# Ground Truth Data which is used as an input to the Detection Simulator.
def generate_scenario_1(seed=1996,
permanent_save=True,
sigma_process=0.01,
sigma_meas_radar=3,
sigma_meas_ais=1):
"""
Generates scenario 1. Todo define scenario 1
:param seed:
:param permanent_save:
:param sigma_process:
:param sigma_meas_radar:
:param sigma_meas_ais:
:return:
"""
# specify seed to be able repeat example
start_time = datetime.now()
np.random.seed(seed)
# combine two 1-D CV models to create a 2-D CV model
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(sigma_process),
ConstantVelocity(sigma_process)])
# starting at 0,0 and moving NE
truth = GroundTruthPath(
[GroundTruthState([0, 1, 0, 1], timestamp=start_time)])
# generate truth using transition_model and noise
for k in range(1, 21):
truth.append(
GroundTruthState(transition_model.function(
truth[k - 1], noise=True, time_interval=timedelta(seconds=1)),
timestamp=start_time + timedelta(seconds=k)))
# Simulate measurements
# Specify measurement model for radar
measurement_model_radar = LinearGaussian(
ndim_state=4, # number of state dimensions
mapping=(0, 2), # mapping measurement vector index to state index
noise_covar=np.array([
[sigma_meas_radar, 0], # covariance matrix for Gaussian PDF
[0, sigma_meas_radar]
]))
# Specify measurement model for AIS
measurement_model_ais = LinearGaussian(ndim_state=4,
mapping=(0, 2),
noise_covar=np.array(
[[sigma_meas_ais, 0],
[0, sigma_meas_ais]]))
# generate "radar" measurements
measurements_radar = []
for state in truth:
measurement = measurement_model_radar.function(state, noise=True)
measurements_radar.append(
Detection(measurement, timestamp=state.timestamp))
# generate "AIS" measurements
measurements_ais = []
state_num = 0
for state in truth:
state_num += 1
if not state_num % 2: # measurement every second time step
measurement = measurement_model_ais.function(state, noise=True)
measurements_ais.append(
Detection(measurement, timestamp=state.timestamp))
if permanent_save:
save_folder_name = seed.__str__()
else:
save_folder_name = "temp"
save_folder = "../scenarios/scenario1/" + save_folder_name + "/"
# save the ground truth and the measurements for the radar and the AIS
store_object.store_object(truth, save_folder, "ground_truth.pk1")
store_object.store_object(measurements_radar, save_folder,
"measurements_radar.pk1")
store_object.store_object(measurements_ais, save_folder,
"measurements_ais.pk1")
store_object.store_object(start_time, save_folder, "start_time.pk1")
store_object.store_object(measurement_model_radar, save_folder,
"measurement_model_radar.pk1")
store_object.store_object(measurement_model_ais, save_folder,
"measurement_model_ais.pk1")
store_object.store_object(transition_model, save_folder,
"transition_model.pk1")
def generate_scenario_3(seed=1996,
permanent_save=True,
radar_meas_rate=1,
ais_meas_rate=5,
sigma_process=0.01,
sigma_meas_radar=3,
sigma_meas_ais=1,
timesteps=20):
"""
Generates scenario 3. Scenario 3 consists of radar and ais measurements with different sampling rate. The sampling
rate is specified in the input params. A groundtruth is generated for each second.
:param seed:
:param permanent_save:
:param radar_meas_rate:
:param ais_meas_rate:
:param sigma_process:
:param sigma_meas_radar:
:param sigma_meas_ais:
:param timesteps: The amount of measurements from the slowest sensor
:return: Nothing. Saves the scenario to a specified folder
"""
start_time = datetime.now()
# specify seed to be able to repeat the example
np.random.seed(seed)
# combine two 1-D CV models to create a 2-D CV model
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(sigma_process),
ConstantVelocity(sigma_process)])
# starting at 0,0 and moving NE
truth = GroundTruthPath(
[GroundTruthState([0, 1, 0, 1], timestamp=start_time)])
# generate truth using transition_model and noise
end_time = start_time + timedelta(seconds=timesteps *
max(radar_meas_rate, ais_meas_rate))
time = start_time + timedelta(seconds=1)
while time < end_time:
truth.append(
GroundTruthState(transition_model.function(
truth[-1], noise=True, time_interval=timedelta(seconds=1)),
timestamp=time))
time += timedelta(seconds=1)
# Simulate measurements
# Specify measurement model for radar
measurement_model_radar = LinearGaussian(
ndim_state=4, # number of state dimensions
mapping=(0, 2), # mapping measurement vector index to state index
noise_covar=np.array([
[sigma_meas_radar, 0], # covariance matrix for Gaussian PDF
[0, sigma_meas_radar]
]))
# Specify measurement model for AIS (Same as for radar)
measurement_model_ais = LinearGaussian(ndim_state=4,
mapping=(0, 2),
noise_covar=np.array(
[[sigma_meas_ais, 0],
[0, sigma_meas_ais]]))
# generate "radar" measurements
measurements_radar = []
measurements_ais = []
next_radar_meas_time = start_time
next_ais_meas_time = start_time
for state in truth:
# check whether we want to generate a measurement from this gt
if state.timestamp == next_radar_meas_time:
measurement = measurement_model_radar.function(state, noise=True)
measurements_radar.append(
Detection(measurement, timestamp=state.timestamp))
next_radar_meas_time += timedelta(seconds=radar_meas_rate)
if state.timestamp == next_ais_meas_time:
measurement = measurement_model_ais.function(state, noise=True)
measurements_ais.append(
Detection(measurement, timestamp=state.timestamp))
next_ais_meas_time += timedelta(seconds=ais_meas_rate)
if permanent_save:
save_folder_name = seed.__str__()
else:
save_folder_name = "temp"
save_folder = "../scenarios/scenario3/" + save_folder_name + "/"
# save the ground truth and the measurements for the radar and the AIS
store_object.store_object(truth, save_folder, "ground_truth.pk1")
store_object.store_object(measurements_radar, save_folder,
"measurements_radar.pk1")
store_object.store_object(measurements_ais, save_folder,
"measurements_ais.pk1")
store_object.store_object(start_time, save_folder, "start_time.pk1")
store_object.store_object(measurement_model_radar, save_folder,
"measurement_model_radar.pk1")
store_object.store_object(measurement_model_ais, save_folder,
"measurement_model_ais.pk1")
store_object.store_object(transition_model, save_folder,
"transition_model.pk1")
[0., 0., 0., 0.]))
# %%
# Create target
# -------------
# Then a target is created for the model to measure
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)
# %%
def test_kalman_smoother(SmootherClass):
# First create a track from some detections and then smooth - check the output.
# Setup list of Detections
start = datetime.now()
times = [start + timedelta(seconds=i) for i in range(0, 5)]
measurements = [
np.array([[2.486559674128609]]),
np.array([[2.424165626519697]]),
np.array([[6.603176662762473]]),
np.array([[9.329099124074590]]),
np.array([[14.637975326666801]]),
]
detections = [
Detection(m, timestamp=timest)
for m, timest in zip(measurements, times)
]
# Setup models.
trans_model = ConstantVelocity(noise_diff_coeff=1)
meas_model = LinearGaussian(ndim_state=2,
mapping=[0],
noise_covar=np.array([[0.4]]))
# Tracking components
predictor = KalmanPredictor(transition_model=trans_model)
updater = KalmanUpdater(measurement_model=meas_model)
# Prior
cstate = GaussianState(np.ones([2, 1]), np.eye(2), timestamp=start)
track = Track()
for detection in detections:
# Predict
pred = predictor.predict(cstate, timestamp=detection.timestamp)
# form hypothesis
hypothesis = SingleHypothesis(pred, detection)
# Update
cstate = updater.update(hypothesis)
# write to track
track.append(cstate)
smoother = SmootherClass(transition_model=trans_model)
smoothed_track = smoother.smooth(track)
smoothed_state_vectors = [state.state_vector for state in smoothed_track]
# Verify Values
target_smoothed_vectors = [
np.array([[1.688813974839928], [1.267196351952188]]),
np.array([[3.307200214998506], [2.187167840595264]]),
np.array([[6.130402001958210], [3.308896367021604]]),
np.array([[9.821303658438408], [4.119557021638030]]),
np.array([[14.257730973981149], [4.594862462495096]])
]
assert np.allclose(smoothed_state_vectors, target_smoothed_vectors)
# Check that a prediction is smoothable and that no error chucked
# Also remove the transition model and use the one provided by the smoother
track[1] = GaussianStatePrediction(pred.state_vector,
pred.covar,
timestamp=pred.timestamp)
smoothed_track2 = smoother.smooth(track)
assert isinstance(smoothed_track2[1], GaussianStatePrediction)
# Check appropriate error chucked if not GaussianStatePrediction/Update
track[-1] = detections[-1]
with pytest.raises(TypeError):
smoother._prediction(track[-1])
def test_base():
# Define time related variables
timestamp = datetime.datetime.now()
timediff = 2 # 2sec
new_timestamp = timestamp + datetime.timedelta(seconds=timediff)
# Define a static 2d platform and check it does not move
platform_state2d = State(np.array([[2], [2]]), timestamp)
platform = FixedPlatform(states=platform_state2d,
position_mapping=np.array([0, 1]))
platform.move(new_timestamp)
new_statevector = np.array([[2], [2]])
# Ensure 2d platform has not moved
assert (np.array_equal(platform.state.state_vector, new_statevector))
# Test to ensure platform time has updated
assert (platform.state.timestamp == new_timestamp)
assert np.array_equal(platform.velocity, StateVector([0, 0]))
assert platform.ndim == 2
assert not platform.is_moving
# Define a static 3d platform and check it does not move
platform_state3d = State(np.array([[2], [2], [2]]), timestamp)
platform = FixedPlatform(states=platform_state3d,
position_mapping=[0, 1, 2])
platform.move(new_timestamp)
new_statevector = np.array([[2], [2], [2]])
# Ensure 2d platform has not moved
assert np.array_equal(platform.state.state_vector, new_statevector)
assert np.array_equal(platform.velocity, StateVector([0, 0, 0]))
assert platform.ndim == 3
assert not platform.is_moving
# Define zero noise 2d constant velocity transition model
model_1d = ConstantVelocity(0.0)
model_2d = CombinedLinearGaussianTransitionModel([model_1d, model_1d])
# Define a 2d platform with constant velocity motion and test motion
platform_state2d = State(np.array([[2], [1], [2], [1]]), timestamp)
platform = MovingPlatform(states=platform_state2d,
transition_model=model_2d,
position_mapping=[0, 2])
platform.move(new_timestamp)
# Define expected platform location after movement
new_statevector = np.array([[4], [1], [4], [1]])
assert (np.array_equal(platform.state.state_vector, new_statevector))
assert np.array_equal(platform.velocity, StateVector([1, 1]))
assert platform.ndim == 2
assert platform.is_moving
# Define zero noise 3d constant velocity transition model
model_3d = CombinedLinearGaussianTransitionModel(
[model_1d, model_1d, model_1d])
# Define a 3d platform with constant velocity motion and test motion
platform_state = State(np.array([[2], [1], [2], [1], [0], [1]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=model_3d,
position_mapping=[0, 2, 4])
platform.move(new_timestamp)
# Define expected platform location in 3d after movement
new_statevector = np.array([[4], [1], [4], [1], [2], [1]])
assert (np.array_equal(platform.state.state_vector, new_statevector))
assert np.array_equal(platform.velocity, StateVector([1, 1, 1]))
assert platform.ndim == 3
assert platform.is_moving
# We begin our definition of the state-space models by defining the hidden state
# :math:`\mathrm{x}_k`, i.e. the state that we wish to estimate:
#
# .. math::
# \mathrm{x}_k = [x_k, \dot{x}_k, y_k, \dot{y}_k, w_k, h_k]
#
# where :math:`x_k, y_k` denote the pixel coordinates of the top-left corner of the bounding box
# containing an object, with :math:`\dot{x}_k, \dot{y}_k` denoting their respective rate of change,
# while :math:`w_k` and :math:`h_k` denote the width and height of the box, respectively.
#
# We assume that :math:`x_k` and :math:`y_k` move with nearly :class:`~.ConstantVelocity`, while
# :math:`w_k` and :math:`h_k` evolve according to a :class:`~.RandomWalk`.Using these assumptions,
# we proceed to construct our Stone Soup :class:`~.TransitionModel` as follows:
from stonesoup.models.transition.linear import (CombinedLinearGaussianTransitionModel,
ConstantVelocity, RandomWalk)
t_models = [ConstantVelocity(20**2), ConstantVelocity(20**2), RandomWalk(20**2), RandomWalk(20**2)]
transition_model = CombinedLinearGaussianTransitionModel(t_models)
# %%
# Measurement Model
# *****************
# Continuing, we define the measurement state :math:`\mathrm{y}_k`, which follows naturally from
# the form of the detections generated by the :class:`~.TensorFlowBoxObjectDetector` we previously
# discussed:
#
# .. math::
# \mathrm{y}_k = [x_k, y_k, w_k, h_k]
#
# We make use of a 4-dimensional :class:`~.LinearGaussian` model as our :class:`~.MeasurementModel`,
# whereby we can see that the individual indices of :math:`\mathrm{y}_k` map to indices `[0,2,4,5]`
# of the 6-dimensional state :math:`\mathrm{x}_k`:
def test_setting_position():
timestamp = datetime.datetime.now()
platform_state = State(np.array([[2], [2], [0]]), timestamp)
model_1d = ConstantVelocity(0.0)
model_3d = CombinedLinearGaussianTransitionModel(
[model_1d, model_1d, model_1d])
platform = MovingPlatform(states=platform_state,
transition_model=model_3d,
position_mapping=[0, 1, 2])
with pytest.raises(AttributeError):
platform.position = [0, 0, 0]
with pytest.raises(AttributeError):
platform.velocity = [0, 0, 0]
platform_state = State(np.array([[2], [2], [0]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 1, 2])
assert np.array_equal(platform.position, StateVector([2, 2, 0]))
platform.position = StateVector([0, 0, 0])
assert np.array_equal(platform.position, StateVector([0, 0, 0]))
with pytest.raises(AttributeError):
platform.velocity = [0, 0, 0]
platform_state = State(np.array([[2], [2], [0]]), timestamp)
platform = FixedPlatform(states=platform_state, position_mapping=[0, 1, 2])
assert np.array_equal(platform.position, StateVector([2, 2, 0]))
platform.position = StateVector([0, 0, 0])
assert np.array_equal(platform.position, StateVector([0, 0, 0]))
assert np.array_equal(platform.state_vector, StateVector([0, 0, 0]))
with pytest.raises(AttributeError):
platform.velocity = [0, 0, 0]
platform_state = State(np.array([[2], [1], [2], [-1], [2], [0]]),
timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=model_3d,
position_mapping=[0, 2, 4])
with pytest.raises(AttributeError):
platform.position = [0, 0, 0]
platform_state = State(np.array([[2], [1], [2], [-1], [2], [0]]),
timestamp)
platform = FixedPlatform(states=platform_state, position_mapping=[0, 2, 4])
assert np.array_equal(platform.position, StateVector([2, 2, 2]))
platform.position = StateVector([0, 0, 1])
assert np.array_equal(platform.position, StateVector([0, 0, 1]))
assert np.array_equal(platform.state_vector,
StateVector([0, 1, 0, -1, 1, 0]))
with pytest.raises(AttributeError):
platform.velocity = [0, 0, 0]
platform_state = State(np.array([[2], [1], [2], [-1], [2], [0]]),
timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 2, 4])
assert np.array_equal(platform.position, StateVector([2, 2, 2]))
platform.position = StateVector([0, 0, 1])
assert np.array_equal(platform.position, StateVector([0, 0, 1]))
assert np.array_equal(platform.state_vector,
StateVector([0, 1, 0, -1, 1, 0]))
with pytest.raises(AttributeError):
platform.velocity = [0, 0, 0]
#
# .. math::
# F_{k}^{D} &= \begin{bmatrix}
# F_k^{1} & & \mathbf{0} \\
# & \ddots & \\
# \mathbf{0} & & F_k^d \\
# \end{bmatrix}\\
# Q_{k}^{D} &= \begin{bmatrix}
# Q_k^{1} & & \mathbf{0} \\
# & \ddots & \\
# \mathbf{0} & & Q_k^d \\
# \end{bmatrix}
#
# We want a 2d simulation, so we'll do:
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(0.05), ConstantVelocity(0.05)])
# %%
# A 'truth path' is created starting at 0,0 moving to the NE
truth = GroundTruthPath([GroundTruthState([0, 1, 0, 1], timestamp=start_time)])
for k in range(1, 21):
truth.append(
GroundTruthState(transition_model.function(
truth[k - 1], noise=True, time_interval=timedelta(seconds=1)),
timestamp=start_time + timedelta(seconds=k)))
# %%
# Thus the ground truth is generated and we can plot the result
fig = plt.figure(figsize=(10, 6))
ax = fig.add_subplot(1, 1, 1)
from trackers.calc_cross_cov_estimate_error import calc_cross_cov_estimate_error
# load ground truth and the measurements
ground_truth = open_object.open_object(
"../scenarios/scenario2/ground_truth.pk1")
measurements_radar = open_object.open_object(
"../scenarios/scenario2/measurements_radar.pk1")
measurements_ais = open_object.open_object(
"../scenarios/scenario2/measurements_ais.pk1")
# load start_time
start_time = open_object.open_object("../scenarios/scenario2/start_time.pk1")
# same transition models (radar uses same as original)
transition_model_radar = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(0.01), ConstantVelocity(0.01)])
transition_model_ais = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(0.01), ConstantVelocity(0.01)])
# same measurement models as used when generating the measurements
# Specify measurement model for radar
measurement_model_radar = LinearGaussian(
ndim_state=4, # number of state dimensions
mapping=(0, 2), # mapping measurement vector index to state index
noise_covar=np.array([
[1, 0], # covariance matrix for Gaussian PDF
[0, 1]
]))
# Specify measurement model for AIS
measurement_model_ais = LinearGaussian(ndim_state=4,
from stonesoup.hypothesiser.distance import DistanceHypothesiser
from stonesoup.initiator.simple import MultiMeasurementInitiator
from stonesoup.measures import Mahalanobis
from stonesoup.models.transition.linear import (
CombinedLinearGaussianTransitionModel, ConstantVelocity)
from stonesoup.models.measurement.linear import LinearGaussian
from stonesoup.predictor.kalman import KalmanPredictor
from stonesoup.simulator.simple import MultiTargetGroundTruthSimulator, SimpleDetectionSimulator
from stonesoup.tracker.simple import MultiTargetTracker
from stonesoup.types.array import StateVector, CovarianceMatrix
from stonesoup.types.state import GaussianState
from stonesoup.updater.kalman import KalmanUpdater
# Models
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(1), ConstantVelocity(1)])
measurement_model = LinearGaussian(4, [0, 2], np.diag([0.5, 0.5]))
# Simulators
groundtruth_sim = MultiTargetGroundTruthSimulator(
transition_model=transition_model,
initial_state=GaussianState(
StateVector([[0], [0], [0], [0]]),
CovarianceMatrix(np.diag([1000, 10, 1000, 10]))),
timestep=datetime.timedelta(seconds=5),
number_steps=100,
birth_rate=0.2,
death_probability=0.05)
detection_sim = SimpleDetectionSimulator(
groundtruth=groundtruth_sim,
measurement_model=measurement_model,
timestamp=state.timestamp))
# Plot the result (back in cart.)
x, y = pol2cart(np.hstack(state.state_vector[1, 0] for state in measurements),
np.hstack(state.state_vector[0, 0] for state in measurements))
ax.scatter(x + sensor_x, y + sensor_y, color='b')
fig
plt.polar([state.state_vector[0, 0] for state in measurements],
[state.state_vector[1, 0] for state in measurements])
# Create Models and Extended Kalman Filter
from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity
transition_model = CombinedLinearGaussianTransitionModel(
(ConstantVelocity(0.1), ConstantVelocity(0.1)))
from stonesoup.predictor.kalman import ExtendedKalmanPredictor
predictor = ExtendedKalmanPredictor(transition_model)
from stonesoup.models.measurement.nonlinear import CartesianToBearingRange
measurement_model = CartesianToBearingRange(
4, # Number of state dimensions (position and velocity in 2D)
(0, 2), # Mapping measurement vector index to state index
np.diag([np.radians(0.2), 1]), # Covariance matrix for Gaussian PDF
translation_offset=np.array([[sensor_x], [sensor_y]
]) # Location of sensor in cartesian.
)
from stonesoup.updater.kalman import ExtendedKalmanUpdater
updater = ExtendedKalmanUpdater(measurement_model)
from stonesoup.types.state import State
from stonesoup.platform.base import FixedPlatform, MultiTransitionMovingPlatform
from stonesoup.simulator.platform import PlatformDetectionSimulator
from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel,\
ConstantVelocity, ConstantTurn
# Create locations for reference later
# Using Heathrow as origin
*heathrow, utm_zone, _ = utm.from_latlon(51.47, -0.4543)
# Use heathrow utm grid num as reference number for utm conversions later
manchester = utm.from_latlon(53.35, -2.280, utm_zone)
# Create transition models for moving platforms
transition_modelStraight = CombinedLinearGaussianTransitionModel(
(ConstantVelocity(0.01), ConstantVelocity(0.01), ConstantVelocity(0.01)))
transition_modelLeft = CombinedLinearGaussianTransitionModel((ConstantTurn(
(0.01, 0.01), np.radians(3)), ConstantVelocity(0.01)))
transition_modelRight = CombinedLinearGaussianTransitionModel((ConstantTurn(
(0.01, 0.01), np.radians(-3)), ConstantVelocity(0.01)))
# 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)
from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, \
ConstantVelocity
from stonesoup.types.groundtruth import GroundTruthPath, GroundTruthState
from stonesoup.types.detection import Detection
from stonesoup.models.measurement.linear import LinearGaussian
from utils import store_object
start_time = datetime.now()
# specify seed to be able repeat example
np.random.seed(1996)
# combine two 1-D CV models to create a 2-D CV model
transition_model = CombinedLinearGaussianTransitionModel([ConstantVelocity(0.01), ConstantVelocity(0.01)])
# starting at 0,0 and moving NE
truth = GroundTruthPath([GroundTruthState([0, 1, 0, 1], timestamp=start_time)])
# generate truth using transition_model and noise
for k in range(1, 21):
truth.append(GroundTruthState(
transition_model.function(truth[k - 1], noise=True, time_interval=timedelta(seconds=1)),
timestamp=start_time + timedelta(seconds=k)))
# Simulate measurements
# Specify measurement model for radar
measurement_model_radar = LinearGaussian(
ndim_state=4, # number of state dimensions
mapping=(0, 2), # mapping measurement vector index to state index
clutter = []
for n in range(1, 21):
clutter.append(set())
for _ in range(np.random.randint(10)):
x = uniform.rvs(0, 20)
y = uniform.rvs(0, 20)
clutter[-1].add(Clutter(
np.array([[x], [y]]), timestamp=start_time+timedelta(seconds=n)))
# Plot the result
ax.scatter([state.state_vector[0, 0] for clutter_set in clutter for state in clutter_set],
[state.state_vector[1, 0] for clutter_set in clutter for state in clutter_set],
color='y', marker='2')
fig
from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity
transition_model = CombinedLinearGaussianTransitionModel((ConstantVelocity(0.05), ConstantVelocity(0.05)))
from stonesoup.predictor.kalman import KalmanPredictor
predictor = KalmanPredictor(transition_model)
from stonesoup.models.measurement.linear import LinearGaussian
measurement_model = LinearGaussian(
4, # Number of state dimensions (position and velocity in 2D)
(0,2), # Mapping measurement vector index to state index
np.array([[0.25, 0], # Covariance matrix for Gaussian PDF
[0, 0.25]])
)
from stonesoup.updater.kalman import KalmanUpdater
updater = KalmanUpdater(measurement_model)
from stonesoup.hypothesiser.distance import DistanceHypothesiser
# :math:`\mathrm{x}_k`, i.e. the state that we wish to estimate:
#
# .. math::
# \mathrm{x}_k = [x_k, \dot{x}_k, y_k, \dot{y}_k, w_k, h_k]
#
# where :math:`x_k, y_k` denote the pixel coordinates of the top-left corner of the bounding box
# containing an object, with :math:`\dot{x}_k, \dot{y}_k` denoting their respective rate of change,
# while :math:`w_k` and :math:`h_k` denote the width and height of the box, respectively.
#
# We assume that :math:`x_k` and :math:`y_k` move with nearly :class:`~.ConstantVelocity`, while
# :math:`w_k` and :math:`h_k` evolve according to a :class:`~.RandomWalk`.Using these assumptions,
# we proceed to construct our Stone Soup :class:`~.TransitionModel` as follows:
from stonesoup.models.transition.linear import (
CombinedLinearGaussianTransitionModel, ConstantVelocity, RandomWalk)
t_models = [
ConstantVelocity(20**2),
ConstantVelocity(20**2),
RandomWalk(20**2),
RandomWalk(20**2)
]
transition_model = CombinedLinearGaussianTransitionModel(t_models)
# %%
# Measurement Model
# *****************
# Continuing, we define the measurement state :math:`\mathrm{y}_k`, which follows naturally from
# the form of the detections generated by the :class:`~.TensorFlowBoxObjectDetector` we previously
# discussed:
#
# .. math::
# \mathrm{y}_k = [x_k, y_k, w_k, h_k]
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
def _smooth_traj(self, traj, process_noise_std=0.5, measurement_noise_std=1):
# Get detector
detector = self._get_detector(traj)
# Models
if not isinstance(process_noise_std, (list, tuple, np.ndarray)):
process_noise_std = [process_noise_std, process_noise_std]
if not isinstance(measurement_noise_std, (list, tuple, np.ndarray)):
measurement_noise_std = [measurement_noise_std, measurement_noise_std]
transition_model = CombinedLinearGaussianTransitionModel(
[
ConstantVelocity(process_noise_std[0] ** 2),
ConstantVelocity(process_noise_std[1] ** 2),
]
)
measurement_model = LinearGaussian(
ndim_state=4,
mapping=[0, 2],
noise_covar=np.diag(
[measurement_noise_std[0] ** 2, measurement_noise_std[1] ** 2]
),
)
# Predictor and updater
predictor = KalmanPredictor(transition_model)
updater = KalmanUpdater(measurement_model)
# Initiator
state_vector = StateVector([0.0, 0.0, 0.0, 0.0])
covar = CovarianceMatrix(np.diag([0.0, 0.0, 0.0, 0.0]))
prior_state = GaussianStatePrediction(state_vector, covar)
initiator = SimpleMeasurementInitiator(prior_state, measurement_model)
# Filtering
track = None
for i, (timestamp, detections) in enumerate(detector):
if i == 0:
tracks = initiator.initiate(detections, timestamp)
track = tracks.pop()
else:
detection = detections.pop()
prediction = predictor.predict(track.state, timestamp=timestamp)
hypothesis = SingleHypothesis(prediction, detection)
posterior = updater.update(hypothesis)
track.append(posterior)
# Smoothing
smoother = KalmanSmoother(transition_model)
smooth_track = smoother.smooth(track)
# Create new trajectory
if traj.is_latlon:
df = traj.df.to_crs("EPSG:3395")
df.geometry = [
Point(state.state_vector[0], state.state_vector[2])
for state in smooth_track
]
df.to_crs(traj.crs, inplace=True)
else:
df = traj.df.copy()
df.geometry = [
Point(state.state_vector[0], state.state_vector[2])
for state in smooth_track
]
new_traj = Trajectory(df, traj.id)
return new_traj
# * :math:`z` is Gaussian distributed around an altitude of :math:`\mathrm{9}km` with variance of :math:`\mathrm{0.1}km`
# * :math:`\dot{x}` is Gaussian distributed around :math:`\mathrm{100}ms^{-1}` with variance of :math:`\mathrm{50}ms^{-1}`
# * :math:`\dot{y}` is Gaussian distributed around :math:`\mathrm{100}ms^{-1}` with variance of :math:`\mathrm{50}ms^{-1}`
# * :math:`\dot{z}` is Gaussian distributed around :math:`\mathrm{0}ms^{-1}` with variance of :math:`\mathrm{1}ms^{-1}`
#
# We will also configure our simulator to randomly create and delete targets, based on a birth rate and death rate we
# specify. In this example we set the birth rate to be 0.10, i.e. on any given time step there is a 10% chance of a new
# target being initiated. We have set the death rate to 0.01, i.e. on any given time step there is a 1% chance that a
# target will be removed from the simulation.
#
# The above setup will provide a case which loosely approximates an air surveillance radar at an airport.
from stonesoup.simulator.simple import MultiTargetGroundTruthSimulator
# Set a constant velocity transition model for the targets
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(0.5),
ConstantVelocity(0.5),
ConstantVelocity(0.1)])
# Define the Gaussian State from which new targets are sampled on initialisation
initial_target_state = GaussianState(
StateVector([[0], [0], [0], [0], [9000], [0]]),
CovarianceMatrix(np.diag([2000, 50, 2000, 50, 100, 1])))
groundtruth_sim = MultiTargetGroundTruthSimulator(
transition_model=transition_model, # target transition model
initial_state=initial_target_state, # add our initial state for targets
timestep=timedelta(seconds=1), # time between measurements
number_steps=120, # 2 minute
birth_rate=0.10, # 10% chance of a new target being born
death_probability=0.01 # 1% chance of a target being killed
np.diag([0.1, np.radians(0.1)]))
measurements.append(
Detection(np.array([[Bearing(phi)], [rho]]),
timestamp=state.timestamp))
# Plot the result (back in cart.)
x, y = pol2cart(np.hstack(state.state_vector[1, 0] for state in measurements),
np.hstack(state.state_vector[0, 0] for state in measurements))
fig
ax.scatter(x + sensor_x, y + sensor_y, color='b')
# Create Models and Particle Filter
from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity
transition_model = CombinedLinearGaussianTransitionModel(
(ConstantVelocity(0.05), ConstantVelocity(0.05)))
from stonesoup.predictor.particle import ParticlePredictor
predictor = ParticlePredictor(transition_model)
from stonesoup.models.measurement.nonlinear import CartesianToBearingRange
measurement_model = CartesianToBearingRange(
4, # Number of state dimensions (position and velocity in 2D)
(0, 2), # Mapping measurement vector index to state index
np.diag([np.radians(0.1), 0.1]), # Covariance matrix for Gaussian PDF
translation_offset=np.array([[sensor_x], [sensor_y]
]) # Location of sensor in cartesian.
)
from stonesoup.resampler.particle import SystematicResampler
resampler = SystematicResampler()
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,
states=platform_state,
position_mapping=[0, 2],
sensors=None)
assert len(platform.transition_models) == 2
assert len(platform.transition_times) == 2
# Check that platform states length increases as platform moves
assert len(platform) == 1
time = datetime.datetime.now()
time += datetime.timedelta(seconds=1)
platform.move(timestamp=time)
assert len(platform) == 2
time += datetime.timedelta(seconds=1)
platform.move(timestamp=time)
assert len(platform) == 3
time += datetime.timedelta(seconds=1)
platform.move(timestamp=time)
assert len(platform) == 4
px, py = platform.position[0], platform.position[1]
# Starting transition model is index 0
assert platform.transition_index == 0
time += datetime.timedelta(seconds=7)
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
# 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).
#
# First we will create a radar which is capable of measuring bearing (:math:`\phi`), elevation (:math:`\theta`), range
# (:math:`r`) and range-rate (:math:`\dot{r}`) of the target platform.
from stonesoup.hypothesiser.distance import DistanceHypothesiser
from stonesoup.initiator.simple import MultiMeasurementInitiator
from stonesoup.measures import Mahalanobis
from stonesoup.models.transition.linear import (
CombinedLinearGaussianTransitionModel, ConstantVelocity)
from stonesoup.models.measurement.linear import LinearGaussian
from stonesoup.predictor.kalman import KalmanPredictor
from stonesoup.simulator.simple import MultiTargetGroundTruthSimulator, SimpleDetectionSimulator
from stonesoup.tracker.simple import MultiTargetTracker
from stonesoup.types.array import StateVector, CovarianceMatrix
from stonesoup.types.state import GaussianState
from stonesoup.updater.kalman import KalmanUpdater
# Models
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(1), ConstantVelocity(1)], seed=1)
measurement_model = LinearGaussian(4, [0, 2], np.diag([0.5, 0.5]), seed=2)
# Simulators
groundtruth_sim = MultiTargetGroundTruthSimulator(
transition_model=transition_model,
initial_state=GaussianState(
StateVector([[0], [0], [0], [0]]),
CovarianceMatrix(np.diag([1000, 10, 1000, 10]))),
timestep=datetime.timedelta(seconds=5),
number_steps=100,
birth_rate=0.2,
death_probability=0.05,
seed=3
)
from datetime import datetime, timedelta
start_time = datetime.now()
# %%
# Generate ground truth
# ^^^^^^^^^^^^^^^^^^^^^
#
# At the end of the tutorial we will plot the Gaussian mixtures. The ground truth Gaussian
# mixtures are stored in this list where each index refers to an instance in time and holds
# all ground truths at that time step.
truths_by_time = []
# Create transition model
from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity
transition_model = CombinedLinearGaussianTransitionModel(
(ConstantVelocity(0.3), ConstantVelocity(0.3)))
from stonesoup.types.groundtruth import GroundTruthPath, GroundTruthState
start_time = datetime.now()
truths = set() # Truths across all time
current_truths = set() # Truths alive at current time
start_truths = set()
number_steps = 20
death_probability = 0.005
birth_probability = 0.2
# Initialize 3 truths. This can be changed to any number of truths you wish.
truths_by_time.append([])
for i in range(3):
x, y = initial_position = np.random.uniform(
-30, 30, 2) # Range [-30, 30] for x and y
