# Resample based on reciprocal of maximum particle weight drops below threshold
particle_filter_mwr = ParticleFilterMWR(
number_of_particles,
pf_state_limits,
process_noise,
measurement_noise,
resampling_algorithm,
reciprocal_max_weight_resampling_threshold)
particle_filter_mwr.set_particles(particle_filter_sir.particles)
for i in range(n_time_steps):
# Move the simulated robot
robot.move(robot_setpoint_motion_forward,
robot_setpoint_motion_turn,
world)
# Simulate measurement
measurements = robot.measure(world)
# Update SIR particle filter (in this case: propagate + weight update + resample)
res = particle_filter_sir.update(robot_setpoint_motion_forward,
robot_setpoint_motion_turn,
measurements,
world.landmarks)
if res:
cnt_sir += 1
# Update NEPR particle filter (in this case: propagate + weight update, resample if needed)
res = particle_filter_nepr.update(robot_setpoint_motion_forward,
max_number_of_particles)
adaptive_particle_filter_kld.set_particles(copy.deepcopy(particle_filter_sir.particles))
##
# Start simulation
##
errors_kld = []
npar_kld = []
errors_sir = []
npar_sir = []
for i in range(num_time_steps):
# Simulate robot motion (required motion will not exactly be achieved)
robot.move(desired_distance=robot_setpoint_motion_forward,
desired_rotation=robot_setpoint_motion_turn,
world=world)
# Simulate measurement
measurements = robot.measure(world)
# Real robot pose
robot_pose = np.array([robot.x, robot.y, robot.theta])
# Update adaptive particle filter KLD
adaptive_particle_filter_kld.update(robot_forward_motion=robot_setpoint_motion_forward,
robot_angular_motion=robot_setpoint_motion_turn,
measurements=measurements,
landmarks=world.landmarks)
errors_kld.append(robot_pose - np.asarray(adaptive_particle_filter_kld.get_average_state()))
npar_kld.append(len(adaptive_particle_filter_kld.particles))
