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))
if PLOT:
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')
if(visible_measurements.size != 0):
plot_connections(ax[0], vehicle.position, visible_measurements + vehicle.position[:2])
plot_particles_weight(ax[0], particles)
if(visible_measurements.size != 0):
plot_measurement(ax[0], vehicle.position[:2], visible_measurements, color="red")
ax[1].clear()
ax[1].set_xlim([-5, 20])
ax[1].set_ylim([-5, 20])
best = np.argmax(FlatParticle.w(particles))
plot_landmarks(ax[1], landmarks, color="green")
plot_landmarks(ax[1], FlatParticle.get_landmarks(particles, best), color="orange")
covariances = FlatParticle.get_covariances(particles, best)
for i, landmark in enumerate(FlatParticle.get_landmarks(particles, best)):
plot_confidence_ellipse(ax[1], landmark, covariances[i], n_std=3)
landmark_indices.append(i)
z_real = np.array(z_real)
visible_landmarks = np.array(visible_landmarks, dtype=np.float)
plot_measurement(ax, ss[:2], z_real, color="red")
# print(z_real + ss[:2])
update(particles, landmark_indices, z_real, sigmas)
plt.pause(0.01)
slam_history.append(get_mean(particles))
ax.clear()
ax.set_xlim([0, 17])
ax.set_ylim([0, 17])
plot_landmarks(ax, landmarks)
plot_history(ax, ss_history, color='green')
plot_history(ax, slam_history, color='orange')
plot_connections(ax, ss, z_real[landmark_indices, :] + ss[:2])
plot_particles_weight(ax, particles)
plot_measurement(ax, ss[:2], z_real, color="red")
if neff(particles) < N / 2:
print("resample", neff(particles))
weights = [p.w for p in particles]
indexes = systematic_resample(weights)
# print(indexes)
particles = resample_from_index(particles, indexes)
# print(particles)
# plt.show()
visible_landmarks.append(landmark)
landmark_indices.append(i)
z_real = np.array(z_real)
visible_landmarks = np.array(visible_landmarks, dtype=np.float)
# plot_measurement(ax, real_position[:2], z_real, color="red")
start = time.time()
update(particles, z_real[landmark_indices], measurement_variance)
print("took: ", (time.time() - start))
# plt.pause(2)
predicted_position_history.append(
Particle.get_mean_position(particles))
if PLOT:
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_connections(ax, real_position,
z_real[landmark_indices, :] + real_position[:2])
plot_particles_weight(ax, particles)
plot_measurement(ax, real_position[:2], z_real, color="red")
if neff(particles) < N / 2:
print("resample", neff(particles))
particles = resample_particles(particles)
z_real.append(z)
z_real = np.array(z_real)
plot_measurement(ax, ss[:2], z_real, color="red")
# print(z_real + ss[:2])
update(particles, landmarks, z_real, sigmas)
plt.pause(0.01)
slam_history.append(get_mean(particles))
ax.clear()
ax.set_xlim([0, 17])
ax.set_ylim([0, 17])
plot_landmarks(ax, landmarks)
plot_history(ax, ss_history, color='green')
plot_history(ax, slam_history, color='orange')
plot_connections(ax, ss, z_real + ss[:2])
plot_particles_weight(ax, particles)
plot_measurement(ax, ss[:2], z_real, color="red")
if neff(particles) < N/2:
print("resample", neff(particles))
weights = [p.w for p in particles]
indexes = systematic_resample(weights)
# print(indexes)
particles = resample_from_index(particles, indexes)
# print(particles)
# plt.show()