def motion_update(p):
x, y, t = p
new_p = (x + random.gauss(v_dt * cos(t), sigma),
y + random.gauss(v_dt * sin(t), sigma),
t + random.gauss(w_dt, sigma))
if not mcl_tools.map_hit(new_p[0], new_p[1]):
return new_p
else:
return mcl_tools.random_particle()
def motion_update(p):
x, y, t = p
new_p = (x + random.gauss(v_dt*cos(t), sigma),
y + random.gauss(v_dt*sin(t), sigma),
t + random.gauss(w_dt, sigma))
if not mcl_tools.map_hit(new_p[0], new_p[1]):
return new_p
else:
return mcl_tools.random_particle()
def reset(self): # would like a better way to do
x = np.random.random() * MAP_WIDTH
y = np.random.random() * MAP_HEIGHT
while mcl_tools.map_hit(x, y): # outside of map?
x = np.random.random() * MAP_WIDTH
y = np.random.random() * MAP_HEIGHT
self.x = x
self.y = y
self.theta = np.random.random() * 2.0 * np.pi
self.weight = 0.1
self.forward_noise = 0.01
self.turn_noise = 0.01
def move(self, control):
# print con
# turn, and add randomness to the turning command
forward, turn = control # unpack linear/angular velocities
theta = self.theta + float(turn) + np.random.normal(0.0, self.turn_noise)
theta %= 2 * np.pi # make sure between 0 and Tau
dist = float(forward) + np.random.normal(0.0, self.forward_noise)
x = self.x + (np.cos(theta) * dist)
y = self.y + (np.sin(theta) * dist)
x %= MAP_WIDTH # cyclic truncate
y %= MAP_HEIGHT # make sure still inside world range
self.set_pose(x, y, theta) # adjust pose
while mcl_tools.map_hit(self.x, self.y): # movement now outside of map?
#print "kill bad particle"
self.reset()
return self # new object with updated values
def particle_filter(ps, control, scan):
global last_time, PAR_COUNT, MID_LIFE, MID_UNCERT, MID_COUNT, \
LONG_LIFE, LONG_UNCERT, LONG_COUNT
if last_time is None:
last_time = rp.get_rostime()
# probabilistically move all the particles
new_pos = partial(integrate_control_to_distance, control, (rp.get_rostime() - last_time).to_sec())
last_time = rp.get_rostime()
new_ps = []
for part in ps:
# part[0] = x
# part[1] = y
if not mcl_tools.map_hit(part[0], part[1]):
# print "hit"
new_ps.append(new_pos(part))
else:
new_ps.append(mcl_tools.random_particle())
ps = new_ps # update our particle set
# update weights of each particle
weights = []
for part in ps:
weight = particle_weight(scan, part)
weights.append(weight)
# normalize the weights
weights = np.multiply(weights, 1.0 / np.sum(weights))
'''
############ RESAMPLING! ##############
'''
correction = False
# mid level corrections
if MID_COUNT > MID_LIFE: # crisis
correction = True
# resample 20%
ps = mcl_tools.random_sample(ps, PAR_COUNT - MID_UNCERT, weights)
rand_ps = []
for x in range(MID_UNCERT):
rand_ps.append(mcl_tools.random_particle())
ps.extend(rand_ps)
MID_COUNT = 0
print "mid"
return ps
else:
MID_COUNT += 1
# big corrections
if LONG_COUNT > LONG_LIFE:
correction = True
# resample 50%
ps = mcl_tools.random_sample(ps, PAR_COUNT - LONG_UNCERT, weights)
rand_ps = []
for x in range(LONG_UNCERT):
rand_ps.append(mcl_tools.random_particle())
ps.extend(rand_ps)
LONG_COUNT = 0
print "long"
return ps
else:
LONG_COUNT += 1
# no corrections (normal resample all particles)
if not correction:
ps = mcl_tools.random_sample(ps, PAR_COUNT, weights)
return ps