def plot_clusters_with_track_init_hyp(self, ax):
H1_true_tracks = dict()
H0_true_tracks = dict()
for cluster_id, cluster in self.clusters.items():
track = cluster.get_track()
measurements = cluster.get_measurements()
color = 'g' if self.cluster_H1_true[cluster_id] else 'r'
ms = 12 if self.cluster_H1_true[cluster_id] else 14
for z in measurements:
ax.plot(z.value[1],
z.value[0],
'o',
markeredgecolor=color,
markerfacecolor='none',
markersize=ms)
ax.text(z.value[1], z.value[0], str(z.timestamp))
if self.cluster_H1_true[cluster_id]:
H1_true_tracks[cluster_id] = track
else:
H0_true_tracks[cluster_id] = track
autovis.plot_track_pos(H1_true_tracks, ax, color='g')
autovis.plot_track_pos(H0_true_tracks, ax, color='r')
ax.set_xlim(-500, 500)
ax.set_ylim(-500, 500)
ax.set_aspect('equal')
def plot_particles(ax, particles, track_file, measurements):
autovis.plot_measurements(measurements, ax)
particle_states = np.array([particle.state for particle in particles])
ax.plot(particle_states[:,2], particle_states[:,0], '.', ms=1, color='b')
autovis.plot_track_pos(track_file, ax, color='r')
ax.set_xlim(-setup.radar_range, setup.radar_range)
ax.set_ylim(-setup.radar_range, setup.radar_range)
ax.set_aspect('equal')
def plot_particles(ax, particles, track_file, measurements):
autovis.plot_measurements(measurements, ax)
particle_states = np.array([particle.state for particle in particles])
ax.plot(particle_states[:,2], particle_states[:,0], '.', ms=1, color='b')
autovis.plot_track_pos(track_file, ax, color='r')
ax.set_xlim(-setup.radar_range, setup.radar_range)
ax.set_ylim(-setup.radar_range, setup.radar_range)
ax.set_aspect('equal')
def plot_clusters_with_track_init_hyp(self, ax):
H1_true_tracks = dict()
H0_true_tracks = dict()
for cluster_id, cluster in self.clusters.items():
track = cluster.get_track()
measurements = cluster.get_measurements()
color = 'g' if self.cluster_H1_true[cluster_id] else 'r'
ms = 12 if self.cluster_H1_true[cluster_id] else 14
for z in measurements:
ax.plot(z.value[1], z.value[0], 'o', markeredgecolor=color,markerfacecolor='none', markersize=ms)
ax.text(z.value[1], z.value[0], str(z.timestamp))
if self.cluster_H1_true[cluster_id]:
H1_true_tracks[cluster_id] = track
else:
H0_true_tracks[cluster_id] = track
autovis.plot_track_pos(H1_true_tracks, ax, color='g')
autovis.plot_track_pos(H0_true_tracks, ax, color='r')
ax.set_xlim(-500, 500)
ax.set_ylim(-500, 500)
ax.set_aspect('equal')
def dual_plot_sim(measurements_all, num_ships, track_file, x_true):
print('Starting dual_plot_sim')
f, (ax2, ax1) = plt.subplots(1, 2)
fig, ax1 = visualization.plot_measurements(measurements_all, ax1)
# fig, ax = visualization.setup_plot(None)
for ship in range(num_ships):
# ax.plot(x_true[ship, 2, 0:100], x_true[ship, 0, 0:100], 'k', label='True trajectory '+str(ship+1))
ax1.plot(x_true[ship, 2, :],
x_true[ship, 0, :],
'k',
label='True trajectory ' + str(ship + 1))
ax1.plot(x_true[ship, 2, 0], x_true[ship, 0, 0], 'ko')
visualization.plot_track_pos(track_file, ax1, 'r')
ax1.set_xlim(-250, 250)
ax1.set_ylim(-250, 250)
ax1.set_xlabel('East[m]')
ax1.set_ylabel('North[m]')
ax1.set_title('Track position with sample rate: 1/s')
ax1.legend(loc="upper left")
fig, ax2 = visualization.plot_measurements(measurements_all, ax2)
# fig, ax = visualization.setup_plot(None)
for ship in range(num_ships):
# ax.plot(x_true[ship, 2, 0:100], x_true[ship, 0, 0:100], 'k', label='True trajectory '+str(ship+1))
ax2.plot(x_true[ship, 2, :],
x_true[ship, 0, :],
'k',
label='True trajectory ' + str(ship + 1))
ax2.plot(x_true[ship, 2, 0], x_true[ship, 0, 0], 'ko')
# visualization.plot_track_pos(track_manager.track_file, ax, 'r')
ax2.set_xlim(-250, 250)
ax2.set_ylim(-250, 250)
ax2.set_xlabel('East[m]')
ax2.set_ylabel('North[m]')
ax2.set_title('Track position with sample rate: 1/s')
ax2.legend(loc="upper left")
print "Unknown tracker {}, skipping".format(title)
continue
track_manager = automanagers.Manager(tracker, init, term)
current_managers[title] = track_manager
return current_managers
if __name__ == '__main__':
import autoseapy.visualization as autovis
import matplotlib.pyplot as plt
true_targets, true_detectability_mode, true_existence_mode, measurements_clutter, measurements_all, time = generate_scenario()
target_fig = plt.figure(figsize=(7,7))
target_ax = target_fig.add_axes((0.15, 0.15, 0.8, 0.8))
autovis.plot_track_pos(true_targets, target_ax)
target_ax.set_xlim(-radar_range, radar_range)
target_ax.set_ylim(-radar_range, radar_range)
target_ax.set_aspect('equal')
target_ax.set_xlabel('East [m]')
target_ax.set_ylabel('North [m]')
ext_fig = plt.figure(figsize=(7,4))
ext_ax = ext_fig.add_axes((0.1, 0.15, 0.85, 0.8))
for true_ext in true_existence_mode.values():
ext_ax.step(time, true_ext, color='k',where='mid', lw=2)
ext_ax.set_ylim(0, 1.1)
ext_ax.set_xlim(0, t_max)
ext_ax.set_xlabel('Time [s]')
ext_ax.set_ylabel('Target posterior probability')
ext_ax.grid()
det_fig = plt.figure(figsize=(7,4))
movie_writer = autovis.TrackMovie('movies/pf_tracker_{}'.format(n_mc), enable=False)
true_targets, true_detectability, true_existence, measurements_all, measurement_timestamps = setup.generate_scenario()
existence_probability = np.zeros_like(measurement_timestamps)
pf_manager.reset()
previous_ext_prob = 1-pf_tracker.initial_empty_weight
for n_time, timestamp in enumerate(measurement_timestamps):
measurements = measurements_all[n_time]
debug_data = pf_manager.step(measurements, timestamp)
movie_writer.grab_frame(plot_particles, {'particles' : debug_data['current_particles'], 'track_file' : pf_manager.track_file, 'measurements' : measurements_all[:n_time+1]})
existence_probability[n_time] = 1-pf_tracker.empty_weight
movie_writer.save_movie()
# Plot true and estimated position
fig, ax = plt.subplots(figsize=(8,8))
autovis.plot_measurements(measurements_all, ax)
autovis.plot_track_pos(true_targets, ax, color='k')
autovis.plot_track_pos(pf_manager.track_file, ax, color='r')
ax.set_xlim(-setup.radar_range, setup.radar_range)
ax.set_ylim(-setup.radar_range, setup.radar_range)
ax.set_aspect('equal')
ax.set_title("Extracted track - n_mc={}".format(n_mc))
# Plot posterior target probability and misdetections
ext_fig, ext_ax = plt.subplots()
ext_ax.plot(measurement_timestamps, existence_probability, 'k--')
for track_id, est_list in pf_manager.track_file.items():
timestamps = [est.timestamp for est in est_list]
ext_prob = [est.existence_probability for est in est_list]
ext_ax.plot(timestamps, ext_prob, 'k-')
for true_state in true_targets[setup.munkholmen_mmsi]:
if len(true_state.measurements) == 0:
if __name__ == '__main__':
import autoseapy.visualization as autovis
import matplotlib.pyplot as plt
true_targets, ownship_state, measurements_all, measurement_timestamps = generate_scenario(
)
fig = plt.figure(figsize=(7, 7))
ax = fig.add_axes((0.15, 0.15, 0.8, 0.8))
autovis.plot_measurements_dots(measurements_all,
ax,
color='grey',
label='Measurements')
autovis.plot_track_pos(true_targets,
ax,
color='k',
title='Targets',
end_title='End position',
lw=1)
autovis.plot_track_pos({-1: ownship_state},
ax,
color='k',
ls='--',
title='Ownship',
lw=1)
ax.legend(numpoints=1)
ax.set_ylim(-1000, 5000)
ax.set_xlim(-1000, 5000)
ax.set_xlabel('East [m]')
ax.set_ylabel('North [m]')
ax.set_aspect('equal')
target_strings = {
markevery=10)
num_false_ax.legend(loc='best')
num_false_ax.set_ylabel('Number of false tracks')
num_false_ax.set_xlabel('Time')
num_false_ax.set_ylim(0, 3.5)
num_false_ax.grid()
num_false_fig.savefig('real_data_num_false.pdf')
# Plot the tracking results
for title, manager in current_managers.items():
fig, ax = plt.subplots()
autovis.plot_measurements_dots(measurements_all,
ax,
color='gray',
label='Measurements')
autovis.plot_track_pos(true_targets, ax, lw=2, title='Targets')
autovis.plot_track_pos(manager.track_file,
ax,
color=colors[title],
lw=2,
title='Tracks',
end_title='Track end pos.')
small_ax = fig.add_axes([0.22, 0.62, 0.25, 0.25])
autovis.plot_measurements_dots(measurements_all,
small_ax,
color='gray')
autovis.plot_track_pos(true_targets, small_ax, lw=2)
autovis.plot_track_pos(manager.track_file,
small_ax,
color=colors[title],
lw=2)
print "Unknown tracker {}, skipping".format(title)
continue
track_manager = automanagers.Manager(tracker, init, term)
current_managers[title] = track_manager
return current_managers
if __name__ == '__main__':
import autoseapy.visualization as autovis
import matplotlib.pyplot as plt
true_targets, true_detectability_mode, true_existence_mode, measurements_clutter, measurements_all, time = generate_scenario(
)
target_fig = plt.figure(figsize=(7, 7))
target_ax = target_fig.add_axes((0.15, 0.15, 0.8, 0.8))
autovis.plot_track_pos(true_targets, target_ax)
target_ax.set_xlim(-radar_range, radar_range)
target_ax.set_ylim(-radar_range, radar_range)
target_ax.set_aspect('equal')
target_ax.set_xlabel('East [m]')
target_ax.set_ylabel('North [m]')
ext_fig = plt.figure(figsize=(7, 4))
ext_ax = ext_fig.add_axes((0.1, 0.15, 0.85, 0.8))
for true_ext in true_existence_mode.values():
ext_ax.step(time, true_ext, color='k', where='mid', lw=2)
ext_ax.set_ylim(0, 1.1)
ext_ax.set_xlim(0, t_max)
ext_ax.set_xlabel('Time [s]')
ext_ax.set_ylabel('Target posterior probability')
ext_ax.grid()
det_fig = plt.figure(figsize=(7, 4))
clutter_model=clutter_model)
tracker = autotrack.IPDAFTracker(target_model, measurement_model,
track_gate, survival_probability)
track_initiation = autoinit.IPDAInitiator(tracker, init_prob,
conf_threshold, term_threshold)
track_termination = autotrack.IPDATerminator(term_threshold)
ipda_track_manager = automanagers.Manager(tracker, track_initiation,
track_termination)
return ipda_track_manager
if __name__ == '__main__':
import autoseapy.visualization as autovis
import matplotlib.pyplot as plt
true_targets, measurements_all, time = generate_scenario()
manager = setup_ipda_manager()
for measurements, timestamp in zip(measurements_all, time):
manager.step(measurements, timestamp)
target_fig = plt.figure(figsize=(7, 7))
target_ax = target_fig.add_axes((0.15, 0.15, 0.8, 0.8))
autovis.plot_measurements(measurements_all, target_ax)
autovis.plot_track_pos({1: true_targets}, target_ax)
autovis.plot_track_pos(manager.track_file, target_ax, color='r')
target_ax.set_xlim(-radar_range, radar_range)
target_ax.set_ylim(-radar_range, radar_range)
target_ax.set_aspect('equal')
target_ax.set_xlabel('East [m]')
target_ax.set_ylabel('North [m]')
plt.show()
movie_writer = autovis.TrackMovie('movies/pf_tracker_{}'.format(n_mc), enable=False)
true_targets, true_detectability, true_existence, measurements_all, measurement_timestamps = setup.generate_scenario()
existence_probability = np.zeros_like(measurement_timestamps)
pf_manager.reset()
previous_ext_prob = 1-pf_tracker.initial_empty_weight
for n_time, timestamp in enumerate(measurement_timestamps):
measurements = measurements_all[n_time]
debug_data = pf_manager.step(measurements, timestamp)
movie_writer.grab_frame(plot_particles, {'particles' : debug_data['current_particles'], 'track_file' : pf_manager.track_file, 'measurements' : measurements_all[:n_time+1]})
existence_probability[n_time] = 1-pf_tracker.empty_weight
movie_writer.save_movie()
# Plot true and estimated position
fig, ax = plt.subplots(figsize=(8,8))
autovis.plot_measurements(measurements_all, ax)
autovis.plot_track_pos(true_targets, ax, color='k')
autovis.plot_track_pos(pf_manager.track_file, ax, color='r')
ax.set_xlim(-setup.radar_range, setup.radar_range)
ax.set_ylim(-setup.radar_range, setup.radar_range)
ax.set_aspect('equal')
ax.set_title("Extracted track - n_mc={}".format(n_mc))
# Plot posterior target probability and misdetections
ext_fig, ext_ax = plt.subplots()
ext_ax.plot(measurement_timestamps, existence_probability, 'k--')
for track_id, est_list in pf_manager.track_file.items():
timestamps = [est.timestamp for est in est_list]
ext_prob = [est.existence_probability for est in est_list]
ext_ax.plot(timestamps, ext_prob, 'k-')
for true_state in true_targets[setup.munkholmen_mmsi]:
if len(true_state.measurements) == 0:
for track in new_tracks:
if track[-1].track_index in preliminary_tracks[title].keys():
preliminary_tracks[title][track[-1].track_index].append(
track[-1])
for estimate in current_estimates:
if estimate.track_index not in confirmed_tracks[title].keys():
confirmed_tracks[title][estimate.track_index] = []
confirmed_tracks[title][estimate.track_index].append(estimate)
if len(new_tracks) > 0:
print "Track confirmed in {}".format(title)
pos_fig, pos_ax = plt.subplots()
autovis.plot_measurements(measurements_all, pos_ax)
for title, manager in current_managers.items():
autovis.plot_track_pos(confirmed_tracks[title],
pos_ax,
color=colors[title],
title=title)
autovis.plot_track_pos(preliminary_tracks[title],
pos_ax,
color=colors[title],
title=title,
ls='--')
pos_ax.set_aspect('equal')
pos_ax.legend()
ext_fig, ext_ax = plt.subplots()
for title, manager in current_managers.items():
for track_id, est_list in confirmed_tracks[title].items():
t = np.array([est.timestamp for est in est_list])
p = np.array([est.get_existence_probability() for est in est_list])
ext_ax.plot(t, p, color=colors[title], ls='-', label=title)
print "max existence probability={} for {} confirmed track".format(
