# & \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)
ax.set_xlabel("$x$")
ax.set_ylabel("$y$")
ax.axis('equal')
ax.plot([state.state_vector[0] for state in truth],
return GaussianStateUpdate(x_post, P_post, hypothesis,
hypothesis.measurement.timestamp)
"""Figur zum Plotten"""
figure = plt.figure(figsize=(16, 9))
ax = figure.add_subplot(1, 1, 1)
ax.set_xlabel("$x$")
ax.set_ylabel("$y$")
"""Erstellen der Groundtruth"""
velocity = 300.0
acceleration = 9.0
omega = acceleration / (2 * velocity)
A = (velocity**2) / acceleration
truth = GroundTruthPath()
for t in range(math.ceil((2 * math.pi) / omega)):
x = A * np.sin(omega * t)
y = A * np.sin(2 * omega * t)
truth.append(GroundTruthState(np.array([[x], [y]]), timestamp=t))
# Plot
ax.plot([state.state_vector[0, 0] for state in truth],
[state.state_vector[1, 0] for state in truth],
linestyle="--",
color="grey")
"""Erstellen der Messungen"""
measurements = []
for state in truth:
from datetime import timedelta
from datetime import datetime
import numpy as np
# Simulate Data
from stonesoup.types.groundtruth import GroundTruthPath, GroundTruthState
# Figure to plot truth (and future data)
from matplotlib import pyplot as plt
fig = plt.figure(figsize=(10, 6))
ax = fig.add_subplot(1, 1, 1)
ax.set_xlabel("$x$")
ax.set_ylabel("$y$")
truth = GroundTruthPath()
start_time = datetime.now()
for n in range(1, 21):
x = n
y = n
varxy = np.array([[0.05,0],[0,0.05]])
xy = np.random.multivariate_normal(np.array([x,y]),varxy)
truth.append(GroundTruthState(np.array([[xy[0]], [xy[1]]]), timestamp=start_time+timedelta(seconds=n)))
#Plot the result
ax.plot([state.state_vector[0, 0] for state in truth],
[state.state_vector[1, 0] for state in truth],
color='g', linestyle="--")
from scipy.stats import multivariate_normal
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")
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")
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
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)
truth = GroundTruthPath([state])
current_truths.add(truth)
truths.add(truth)
start_truths.add(truth)
truths_by_time[0].append(state)
# Simulate the ground truth over time
for k in range(number_steps):
truths_by_time.append([])
# Death
for truth in current_truths.copy():
if np.random.rand() <= death_probability:
current_truths.remove(truth)
# Update truths
for truth in current_truths:
updated_state = GroundTruthState(
truth.append(
GroundTruthState(transition_model.function(
truth[-1], noise=True, time_interval=timedelta(seconds=1)),
timestamp=start_time + timedelta(seconds=k)))
# Birth
for _ in range(np.random.poisson(0.6)): # Birth probability
x, y = initial_position = np.random.rand(2) * [
20, 20
] # Range [0, 20] for x and y
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)
from stonesoup.plotter import Plotter
plotter = Plotter()
plotter.ax.set_ylim(-5, 25)
plotter.plot_ground_truths(truths, [0, 2])
# %%
# Generate Detections and Clutter
# -------------------------------
# Next, generate detections with clutter just as in the previous tutorials, skipping over the truth
# paths that weren't alive at the current time step.
from scipy.stats import uniform