truth = GroundTruthPath([GroundTruthState([0, 1, 0, 1], timestamp=start_time)])
# %%
# Create the truth path
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)))
# %%
# Plot the ground truth.
from stonesoup.plotter import Plotter
plotter = Plotter()
plotter.plot_ground_truths(truth, [0, 2])
# %%
# Initialise the bearing, range sensor using the appropriate measurement model.
from stonesoup.models.measurement.nonlinear import CartesianToBearingRange
from stonesoup.types.detection import Detection
sensor_x = 50
sensor_y = 0
measurement_model = CartesianToBearingRange(
ndim_state=4,
mapping=(0, 2),
noise_covar=np.diag([np.radians(0.2), 1]),
translation_offset=np.array([[sensor_x], [sensor_y]]))
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.
#
# Stone Soup has an in-built plotting class which can be used to plot
# ground truths, measurements and tracks in a consistent format. It can be accessed by importing
# the class :class:`Plotter` from Stone Soup as below.
from stonesoup.plotter import Plotter
plotter = Plotter()
plotter.plot_ground_truths(truth, [0, 2])
# %%
# We can check the :math:`F_k` and :math:`Q_k` matrices (generated over a 1s period).
transition_model.matrix(time_interval=timedelta(seconds=1))
# %%
transition_model.covar(time_interval=timedelta(seconds=1))
# %%
# At this point you can play with the various parameters and see how it effects the simulated
# output.
# %%
# Simulate measurements
x_vel, y_vel = (
np.random.rand(2)) * 2 - 1 # Range [-1, 1] for x and y velocity
state = GroundTruthState([x, x_vel, y, y_vel],
timestamp=start_time + timedelta(seconds=k))
# Add to truth set for current and for all timestamps
truth = GroundTruthPath([state])
current_truths.add(truth)
truths.add(truth)
truths_by_time[k].append(state)
# %%
# Plot the ground truth
#
from stonesoup.plotter import Plotter
plotter = Plotter()
plotter.plot_ground_truths(truths, [0, 2])
# %%
# Generate detections with clutter
# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
# Next, generate detections with clutter just as in the previous tutorial. The clutter is
# assumed to be uniformly distributed accross the entire field of view, here assumed to
# be the space where :math:`x \in [-100, 100]` and :math:`y \in [-100, 100]`.
# Make the measurement model
from stonesoup.models.measurement.linear import LinearGaussian
measurement_model = LinearGaussian(ndim_state=4,
mapping=(0, 2),
noise_covar=np.array([[0.75, 0], [0,
0.75]]))
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(0.05), ConstantVelocity(0.05)])
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)))
# %%
# Set-up plot to render ground truth, as before.
from stonesoup.plotter import Plotter
plotter = Plotter()
plotter.plot_ground_truths(truth, [0, 2])
# %%
# Simulate the measurement
# ^^^^^^^^^^^^^^^^^^^^^^^^
#
from stonesoup.models.measurement.nonlinear import CartesianToBearingRange
# Sensor position
sensor_x = 50
sensor_y = 0
# Make noisy measurement (with bearing variance = 0.2 degrees).
measurement_model = CartesianToBearingRange(
ndim_state=4,
mapping=(0, 2),
track2 = Track()
print("Capon detections:")
for timestep, detections in detector1:
for detection in detections:
print(detection)
prediction = predictor.predict(prior, timestamp=detection.timestamp)
hypothesis = SingleHypothesis(
prediction, detection) # Group a prediction and measurement
post = updater.update(hypothesis)
track1.append(post)
prior = track1[-1]
print("RJMCMC detections:")
for timestep, detections in detector2:
for detection in detections:
print(detection)
prediction = predictor.predict(prior, timestamp=detection.timestamp)
hypothesis = SingleHypothesis(
prediction, detection) # Group a prediction and measurement
post = updater.update(hypothesis)
track2.append(post)
prior = track2[-1]
plotter = Plotter()
plotter.plot_tracks(set([track1, track2]), [0, 2], uncertainty=True)
plotter.fig
import matplotlib.pyplot as plt
plt.show()