def update_particle_weights(self, robot_spec, robot_sensor_readings, world):
particle_readings = [Particle.get_expected_sensor_outputs(robot_spec, world, self.particle_pos[:, i,np.newaxis], self.particle_h[i]) for
i in range(self.N_PARTICLES)]
self.particle_weights = np.zeros(self.N_PARTICLES)
for i in range(self.N_PARTICLES):
if Particle.is_position_valid(world, self.particle_pos[:, i]):
self.particle_weights[i] = Particle.get_sensor_reading_probabilities(robot_spec, robot_sensor_readings,
particle_readings[i])
self.particle_weights = self.particle_weights / np.sum(self.particle_weights)
particles = []
for x in range(8 * 10):
for y in range(4 * 10):
p = Particle(world,
x / (8 * Units.METERS_IN_A_FOOT * 10),
y / (4 * Units.METERS_IN_A_FOOT * 10),
h=robot.h)
p.add_to_window(window)
particles.append(p)
# Update particle weights.
robot_sensor_readings = robot.get_expected_sensor_outputs()
particle_weights = np.zeros(len(particles))
invalid_particle_indices = []
for i, p in enumerate(particles):
if p.is_position_valid():
particle_sensor_readings = p.get_expected_sensor_outputs()
particle_weights[i] = np.prod([
w_gauss(r_s, p_s, s) for r_s, p_s, s in zip(
robot_sensor_readings, particle_sensor_readings,
sensor_reading_sigmas)
])
particle_weights /= np.sum(particle_weights)
max_weight = np.max(particle_weights)
for i, p in enumerate(particles):
p.w = particle_weights[i] / max_weight
# Draw to screen.
window.draw()
block_until_window_closed()