cuda_cov = cuda.mem_alloc(4 * 4)
# plt.pause(5)
for i in range(u.shape[0]):
print(i)
if PLOT:
plt.pause(0.05)
ax[0].clear()
ax[0].set_xlim([-5, 20])
ax[0].set_ylim([-5, 20])
plot_landmarks(ax[0], landmarks)
plot_history(ax[0], real_position_history, color='green')
plot_history(ax[0], predicted_position_history, color='orange')
plot_particles_grey(ax[0], particles)
vehicle.move_noisy(u[i])
real_position_history.append(vehicle.position)
FlatParticle.predict(particles, u[i], sigmas=movement_variance, dt=1)
visible_measurements = sensor.get_noisy_measurements(vehicle.position[:2])
particles = update(
particles, THREADS, BLOCK_SIZE, visible_measurements, measurement_variance,
cuda_particles, cuda_measurements, cuda_cov, THRESHOLD
)
predicted_position_history.append(FlatParticle.get_mean_position(particles))
movement_variance = [0.03, 0.05]
measurement_variance = [0.1, 0.1]
for i in range(u.shape[0]):
print(i)
if PLOT:
plt.pause(0.05)
ax.clear()
ax.set_xlim([0, 15])
ax.set_ylim([0, 15])
plot_landmarks(ax, landmarks)
plot_history(ax, real_position_history, color='green')
plot_history(ax, predicted_position_history, color='orange')
plot_particles_grey(ax, particles)
real_position = move_vehicle_stochastic(real_position,
u[i],
dt=1,
sigmas=movement_variance)
real_position_history.append(real_position)
particles = predict(particles, u[i], sigmas=movement_variance, dt=1)
z_real = []
visible_landmarks = []
landmark_indices = []
for i, landmark in enumerate(landmarks):
z = get_measurement_stochastic(real_position[:2], landmark,
measurement_variance)