count = 0
for m in measurements:
count += 1
if not pf.initialized:
# Initialize the particle set using GNSS measurement.
pf.initialize(m['gnss_x'], m['gnss_y'], m['gnss_theta'], *pos_std)
else:
# Prediction step
pf.predict(delta_t, m['previous_velocity'],
m['previous_yawrate'], *pos_std)
# Feed the particle with observations to update weights.
observations = [{'x': x, 'y': y} for (x, y) in \
zip (m['measurement_x'], m['measurement_y'])]
pf.update_weights(sensor_range, *landmark_std, observations, Map)
# Resample particles to capture posterior belief distribution.
pf.resample()
# Select the best (highest weight) particle;
# we will assume that this represents the vehicle's position.
particle = pf.get_best_particle()
# Print for debugging purposes.
print("[%d] %f, %f" % (count, particle['x'], particle['y']))
# Collect data for post-mortem plotting.
graph.append({
'position': (particle['x'], particle['y']),
'particles': [(p['x'], p['y']) for p in pf.particles],
'landmarks': [(Map[l]['x'], Map[l]['y']) \
for l in particle['assoc']],
})
# Go plot the results.
plot_2D(graph)
