return g
window = SimulatorWindow(text='Robot simulation')
world = World()
world.add_to_window(window)
robot = SimulatedRobot(world, pos=[1, 1], h=0)
robot.add_to_window(window)
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)
window = SimulatorWindow(text='Robot simulation')
world = World()
world.add_to_window(window)
robot = SimulatedRobot(robot_spec, world, pos=[1.5, 1], h=45)
robot.add_to_window(window)
# Evenly distribute the particles to start
particles = []
for x in range(8 * 10):
for y in range(4 * 10):
p = Particle(robot_spec,
world,
x / (8 * Units.METERS_IN_A_FOOT * 10),
y / (4 * Units.METERS_IN_A_FOOT * 10),
h=random.randint(0, 359))
particles.append(p)
p.add_to_window(window)
# Initial weights.
particle_readings = [p.get_expected_sensor_outputs() for p in particles]
particle_weights = update_particle_weights(robot, particles, particle_readings)
# Draw to screen.
should_quit = False
t = 0
UPDATE_EVERY = 10
update_clock = pygame.time.Clock()