def load_elements_by_random(self):
map = Map((int(self.cfg['MAP']['grid_row_dimension']), int(self.cfg['MAP']['grid_column_dimension'])))
count = 0
while count <= int(self.cfg['MAP']['random_init_blocks_num']):
x = random.randint(0, int(self.cfg['MAP']['grid_row_dimension']) - 1)
y = random.randint(0, int(self.cfg['MAP']['grid_column_dimension']) - 1)
if (x, y) not in map.blocks:
count += 1
map.grid[x][y] = BLOCK_AREA
map.blocks.append((x, y))
count = 0
while count <= int(self.cfg['MAP']['random_init_robots_num']):
x = random.randint(0, int(self.cfg['MAP']['grid_row_dimension']) - 1)
y = random.randint(0, int(self.cfg['MAP']['grid_column_dimension']) - 1)
if (x, y) not in map.blocks:
count += 1
map.grid[x][y] = ROBOT_AREA
map.robots.append(Robot(x, y)) # add robots
return map
def load_elements_from_file(self, robot_init_filename,
block_init_filename):
map = Map((int(self.cfg['MAP']['grid_row_dimension']),
int(self.cfg['MAP']['grid_column_dimension'])))
for x, y in map.get_blocks_init_place(block_init_filename):
if not map.is_location_in_environment(
x, y, (int(self.cfg['MAP']['grid_row_dimension']),
int(self.cfg['MAP']['grid_column_dimension']))):
raise OutsideBoundryError('init block (x, y) not in map!')
map.grid[x][y] = BLOCK_AREA
map.blocks.append((x, y))
for x, y in map.get_robot_init_place(robot_init_filename):
if not map.is_location_in_environment(
x, y, (int(self.cfg['MAP']['grid_row_dimension']),
int(self.cfg['MAP']['grid_column_dimension']))):
raise OutsideBoundryError('init robot (x, y) not in map!')
map.grid[x][y] = ROBOT_AREA
map.robots.append(Robot(x, y)) # add robots
return map
2) The second particle filter resamples if the approximate number of effective particles drops below a pre-defined
threshold (NEPR)
3) The third particle filter resamples in case the reciprocal of the maximum weight drops below a pre-defined
threshold (MWR)
"""
print("Compare three resampling schemes.")
# Define simulated world
world = World(world_size_x, world_size_y, landmark_positions)
# Initialize simulated robot
robot = Robot(x=world.x_max * 0.75,
y=world.y_max / 5.0,
theta=3.14 / 2.0,
std_forward=true_robot_motion_forward_std,
std_turn=true_robot_motion_turn_std,
std_meas_distance=true_robot_meas_noise_distance_std,
std_meas_angle=true_robot_meas_noise_angle_std)
# Set resampling algorithm used (same for all filters)
resampling_algorithm = ResamplingAlgorithms.MULTINOMIAL
# Number of particles in the particle filters
number_of_particles = 1000
# Resampling thresholds
number_of_effective_particles_threshold = number_of_particles / 4.0
reciprocal_max_weight_resampling_threshold = 1.0 / 0.005
##
##
# Simulated world
world = World(world_size_x, world_size_y, landmark_positions)
# Initialize visualization
show_particle_pose = False # only set to true for low #particles (very slow)
visualizer = Visualizer(show_particle_pose)
# Number of simulated time steps
n_time_steps = 15
# Simulated robot
robot = Robot(robot_initial_x_position, robot_initial_y_position,
robot_initial_heading, true_robot_motion_forward_std,
true_robot_motion_turn_std,
true_robot_meas_noise_distance_std,
true_robot_meas_noise_angle_std)
# Number of particles
number_of_particles = 500
# Set resampling algorithm used
resampling_algorithm = ResamplingAlgorithms.MULTINOMIAL
# Initialize the particle filter
# Initialize SIR particle filter
particle_filter_sir = ParticleFilterSIR(number_of_particles,
pf_state_limits, process_noise,
measurement_noise,