ax.scatter([state.state_vector[0, 0] for measurements in measurementss for state in measurements],
[state.state_vector[1, 0] for measurements in measurementss for state in measurements],
color='b')
fig
from scipy.stats import uniform
from stonesoup.types.detection import Clutter
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
# Create Models and Kalman Filter
from scipy.stats import uniform
from stonesoup.types.detection import Clutter
clutter = []
for n in range(1, 21):
clutter.append(set())
if np.random.rand() <= probability_detection:
# Generate actual detection from the state
measurement = measurement_model.function(truth_state, noise=True)
measurement_set.add(
TrueDetection(state_vector=measurement,
groundtruth_path=truth,
timestamp=truth_state.timestamp,
measurement_model=measurement_model))
# Generate clutter at this time-step
for _ in range(np.random.poisson(clutter_rate)):
x = uniform.rvs(-100, 200)
y = uniform.rvs(-100, 200)
measurement_set.add(
Clutter(np.array([[x], [y]]),
timestamp=timestamp,
measurement_model=measurement_model))
all_measurements.append(measurement_set)
# Plot true detections and clutter.
plotter.plot_measurements(all_measurements, [0, 2], color='g')
plotter.fig
# %%
# Create the Predicter and Updater
# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
#
# The updater is a :class:`~.PHDUpdater`, and since it uses the mixed Gaussian paths, it is a
# GM-PHD updater. For each individual track we use a :class:`~.KalmanUpdater`. Here we assume
# that the measurement range and clutter spatial density are known to the filter. We
# Generate actual detection from the state with a 10% chance that no detection is received.
if np.random.rand() <= prob_detect:
measurement = measurement_model.function(truth[k], noise=True)
measurement_set.add(
TrueDetection(state_vector=measurement,
groundtruth_path=truth,
timestamp=truth[k].timestamp))
# Generate clutter at this time-step
truth_x = truth[k].state_vector[0]
truth_y = truth[k].state_vector[2]
for _ in range(np.random.randint(10)):
x = uniform.rvs(truth_x - 10, 20)
y = uniform.rvs(truth_y - 10, 20)
measurement_set.add(
Clutter(np.array([[x], [y]]), timestamp=truth[k].timestamp))
all_measurements.append(measurement_set)
# Plot measurements.
for set_ in all_measurements:
# Plot actual detections.
axm.scatter([
state.state_vector[0]
for state in set_ if isinstance(state, TrueDetection)
], [
state.state_vector[1]
for state in set_ if isinstance(state, TrueDetection)
],
color='g')
# Plot clutter.
axm.scatter([
for truth in truths:
# Generate actual detection from the state with a 10% chance that no detection is received.
if np.random.rand() <= prob_detect:
measurement = measurement_model.function(truth[k], noise=True)
measurement_set.add(TrueDetection(state_vector=measurement,
groundtruth_path=truth,
timestamp=truth[k].timestamp,
measurement_model=measurement_model))
# Generate clutter at this time-step
truth_x = truth[k].state_vector[0]
truth_y = truth[k].state_vector[2]
for _ in range(np.random.randint(10)):
x = uniform.rvs(truth_x - 10, 20)
y = uniform.rvs(truth_y - 10, 20)
measurement_set.add(Clutter(np.array([[x], [y]]), timestamp=truth[k].timestamp,
measurement_model=measurement_model))
all_measurements.append(measurement_set)
# Plot true detections and clutter.
plotter.plot_measurements(all_measurements, [0, 2], color='g')
# %%
from stonesoup.predictor.kalman import KalmanPredictor
predictor = KalmanPredictor(transition_model)
# %%
from stonesoup.updater.kalman import KalmanUpdater
updater = KalmanUpdater(measurement_model)
# %%
# Initial hypotheses are calculated (per track) in the same manner as the PDA.
# Generate actual detection from the state with a 1-p_d chance that no detection is received.
if np.random.rand() <= prob_det:
measurement = measurement_model.function(state, noise=True)
measurement_set.add(
TrueDetection(state_vector=measurement,
groundtruth_path=truth,
timestamp=state.timestamp))
# Generate clutter at this time-step
truth_x = state.state_vector[0]
truth_y = state.state_vector[2]
for _ in range(np.random.randint(10)):
x = uniform.rvs(truth_x - 10, 20)
y = uniform.rvs(truth_y - 10, 20)
measurement_set.add(
Clutter(np.array([[x], [y]]), timestamp=state.timestamp))
all_measurements.append(measurement_set)
# %%
# Plot the ground truth and measurements with clutter.
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$")
ax.set_ylim(0, 25)
ax.set_xlim(0, 25)
ax.plot(
[state.state_vector[0] for state in truth],