def generate_target(time, radar):
# Generate true state
state_transition_model = autocommon.DWNAModel(
target_process_noise_covariance)
initial_state = np.array([
initial_position[0], initial_velocity[0], initial_position[1],
initial_velocity[1]
])
true_state_list = []
for k, t in enumerate(time):
if k == 0:
true_state = autocommon.Estimate(t,
initial_state,
np.identity(4),
is_posterior=True)
else:
true_state = state_transition_model.draw_transition(
true_state_list[-1], t, is_posterior=True)
true_state_list.append(true_state)
# Generate measurements
target_originated_measurements = []
for k, t in enumerate(time):
true_pos = true_state_list[k].est_posterior.take((0, 2))
measurement = radar.generate_target_measurements([true_pos], t)
target_originated_measurements.append(measurement)
return target_originated_measurements, true_state_list
def generate_target(time, mmsi, radar):
initial_position = initial_positions[mmsi]
velocity = velocities[mmsi]
# Generate true state
initial_state = np.array([initial_position[0], velocity[0], initial_position[1], velocity[1]])
true_state_list = []
for t in time:
current_state = initial_state+np.array([velocity[0]*t, 0, velocity[1]*t, 0])
true_state = autocommon.Estimate(t, current_state, np.identity(4), is_posterior=True)
true_state_list.append(true_state)
# Generate detectability state and measurements
# Generate measurements
detectability_state = []
target_originated_measurements = []
for t in time:
if np.mod(np.sum(detectability_change_times[mmsi] <= t), 2) == 1:
detectability_state.append(PD_low)
else:
detectability_state.append(PD_high)
for k, t in enumerate(time):
radar.update_detection_probability(detectability_state[k])
true_pos = true_state_list[k].est_posterior.take((0,2))
measurement = radar.generate_target_measurements([true_pos], t)
[true_state_list[k].store_measurement(z) for z in measurement]
target_originated_measurements.append(measurement)
return target_originated_measurements, true_state_list, detectability_state
def generate_target(time, radar):
# Generate true state
state_transition_model = autocommon.DWNAModel(
target_process_noise_covariance)
initial_state = np.array([
initial_position[0], initial_velocity[0], initial_position[1],
initial_velocity[1]
])
true_state_list = []
for k, t in enumerate(time):
if k == 0:
true_state_list.append(
autocommon.Estimate(t,
initial_state,
np.identity(4),
is_posterior=True))
elif t <= termination_time:
true_state = state_transition_model.draw_transition(
true_state_list[-1], t, is_posterior=True)
true_state_list.append(true_state)
# Generate measurements
true_detectability = []
true_existence = []
target_originated_measurements = []
for k, t in enumerate(time):
if t >= detectability_change_time:
radar.update_detection_probability(PD_low)
if k < len(true_state_list):
true_detectability.append(radar.detection_probability)
true_pos = true_state_list[k].est_posterior.take((0, 2))
measurement = radar.generate_target_measurements([true_pos], t)
[true_state_list[k].store_measurement(z) for z in measurement]
target_originated_measurements.append(measurement)
true_existence.append(1)
else:
true_detectability.append(None)
target_originated_measurements.append(set())
true_existence.append(0)
return target_originated_measurements, true_state_list, np.array(
true_detectability), np.array(true_existence)
def add_landmark(measurements_all, landmark_mmsi, ais_data):
landmark_measurements = []
landmark_time = []
for measurements in measurements_all:
t_added = False
for measurement in measurements:
z_pos = measurement.value
if z_pos[0] < 1500 and z_pos[0] > 1200 and z_pos[
1] > 300 and z_pos[1] < 700: # Manually found
landmark_measurements.append(z_pos)
t_added = True
landmark_time.append(measurement.timestamp)
landmark_measurements = np.array(landmark_measurements)
landmark_mean = np.mean(landmark_measurements, axis=0)
landmark_data = []
ais_data[landmark_mmsi] = []
for t in landmark_time:
mark_est = autocommon.Estimate(
t, np.array([landmark_mean[0], 0, landmark_mean[1], 0]),
np.identity(4), True, landmark_mmsi)
ais_data[landmark_mmsi].append(mark_est)
return ais_data
landmark_time = []
for measurements in chosen_dataset.measurements:
t_added = False
for measurement in measurements:
z_pos = measurement.value
if z_pos[0] < 1500 and z_pos[0] > 1200 and z_pos[1] > 300 and z_pos[1] < 700:
landmark_measurements.append(z_pos)
t_added = True
landmark_time.append(measurement.timestamp)
landmark_measurements = np.array(landmark_measurements)
landmark_mean = np.mean(landmark_measurements, axis=0)
landmark_data = []
landmark_mmsi = 123456789
chosen_dataset.ais[landmark_mmsi] = []
for t in landmark_time:
mark_est = autocommon.Estimate(t, np.array([landmark_mean[0], 0, landmark_mean[1], 0]), np.identity(4), True, landmark_mmsi)
chosen_dataset.ais[landmark_mmsi].append(mark_est)
xy_fig = plt.figure(figsize=(7,7))
xy_ax = xy_fig.add_axes((0.15, 0.15, 0.8, 0.8))
for measurements in chosen_dataset.measurements:
for measurement in measurements:
z_line, = xy_ax.plot(measurement.value[1], measurement.value[0], '.', color='#aaaaaa')
ownship_line, = xy_ax.plot(ownship_pose[2,:], ownship_pose[1,:], 'k--')
end_position, = xy_ax.plot(ownship_pose[2,-1], ownship_pose[1,-1], 'ko')
map_labels = ['OSD', 'MH II', 'LM']
for mmsi, label in zip([drone_mmsi, munkholmen_mmsi, landmark_mmsi], map_labels):
position = np.array([estimate.est_posterior[[0,2]] for estimate in chosen_dataset.ais[mmsi]])
target_line, = xy_ax.plot(position[:,1], position[:,0], 'k-')