def particle_filter(ps, control, scan): global weights v, w = control v_dt = v*dt w_dt = w*dt sigma = sqrt(v*v + w*w)/6.0 * dt 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() ''' p_prime = [motion_update(p) if not mcl_tools.map_hit(p[0], p[1]) else mcl_tools.random_particle() for p in ps] ''' new_weights = np.array([particle_weight(p, scan) for p in ps]) weights *= new_weights weights /= weights.sum() wvar = 1./sum([w*w for w in weights]) if wvar < random.gauss(PAR_COUNT*.81, 60): ps = mcl_tools.random_sample(ps, PAR_COUNT - NOISE, weights) + [mcl_tools.random_particle() for ii in xrange(NOISE)] weights = [1.0 for ii in xrange(PAR_COUNT)] else: pass return [motion_update(p) for p in ps]
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
def particle_filter(ps, control, scan): """ :param ps: particle list :param control: tuple of linear and angular velocity :param scan: laser scan msg :return: updated list of particles """ global W_SLOW, W_FAST, COUNT, PRIOR_TIME # need this to set initial time if PRIOR_TIME is None: PRIOR_TIME = rp.get_rostime() # <editor-fold desc="motion update"> # adjust speed based on simulation time_dif = (rp.get_rostime() - PRIOR_TIME).to_sec() con = [time_dif * i for i in control] #con = np.multiply(time_dif, control) #numpy should be used for large data sets rp.loginfo("control" + str(con)) #moving each particle based on adjusted velocity for p in xrange(PARTICLE_COUNT): ps[p].move(con) # </editor-fold> # <editor-fold desc="Weight update"> weight_p = [] w_avg = 0.0 w_sum = 0.0 max_w = (0, 0.0) # id, weight for p in xrange(PARTICLE_COUNT): w_sum += ps[p].measurement_prob(scan) weight = ps[p].get_weight() # individual weight weight_p.append(weight) w_avg += 1.0/PARTICLE_COUNT * weight if weight > max_w[1]: max_w = p, weight #print weight_p mcl_tools.show_best_pose(ps[max_w[0]].get_pose()) # </editor-fold> # <editor-fold desc="random sampling with probability"> sample_p = [] # do actual resampling # 0 <= alpha_slow << alpha_fast alpha_slow = 0.001 # decay rate for long-term avg alpha_fast = 0.08 # decay rate for short-term avg W_SLOW += alpha_slow * (w_avg - W_SLOW) W_FAST += alpha_fast * (w_avg - W_FAST) #print 'long term avg: ', W_SLOW #print 'short term avg: ', W_FAST # if short-term likelihood is worse long-term, samples added in proportion to quotient # sudden decrease in likelihood increases number of random samples random_prob = max(0.0, 1.0 - W_FAST/W_SLOW) # create an array of random indexes (1 to particle count-1) rand_array = np.random.randint(low=1, high=PARTICLE_COUNT-1, size=int(PARTICLE_COUNT * random_prob)) # array of indexes to randomly move index = int(np.random.random() * PARTICLE_COUNT-len(rand_array)) beta = 0.0 for k in xrange(PARTICLE_COUNT-len(rand_array)): #print "gets here: ", k # from thrun's udacity class beta += np.random.random() * 2.0 * (max_w[1]/w_sum) #print beta, ps[index].get_weight() count = 0 while beta > ps[index].get_weight(): count += 1 #print "stuck here?", count beta -= ps[index].get_weight() index = (index + 1) % PARTICLE_COUNT sample_p.append(ps[index]) #print "sample_p len ", len(sample_p) #weight # create random particles while len(sample_p) < PARTICLE_COUNT: r = ROBOT() r.set_weight(w_avg) # give it a weight so it shows up COUNT += 1 sample_p.append(r) #print '\ncreated ', COUNT, ' new particles' #print 'total num particles:', len(sample_p) ps = sample_p # </editor-fold> weight_p = np.multiply(weight_p, 1.0/np.sum(weight_p)) # normalize the weights # reset data COUNT = 0 PRIOR_TIME = rp.get_rostime() return mcl_tools.random_sample(ps, PARTICLE_COUNT, weight_p)