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] 
Example #2
0
 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()
Example #3
0
 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()
Example #4
0
def particle_filter(ps, control, scan):
    # FIXME: Should really particle filter.
    return [mcl_tools.random_particle() for ii in range(PAR_COUNT)]
Example #5
0
import rospy
rospy.init_node('with_weights')
import mcl_tools

from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry

import time
import random

PAR_COUNT = 1500
cmd_vel = None

# State
parset = [mcl_tools.random_particle() for ii in range(PAR_COUNT)]


def particle_weight(particle, scan):
    # FIXME: You should assign weight.
    return 1.0


def particle_filter(ps, control, scan):
    # FIXME: Should really particle filter.
    return [mcl_tools.random_particle() for ii in range(PAR_COUNT)]


def got_scan(msg):
    global parset
    global cmd_vel
Example #6
0
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
Example #7
0
LONG_LIFE = 80  # How many time steps before adding random particles into array
LONG_COUNT = 0

odometry = None

STD_DEV_HIT = 1.0
MAX_DIST = mcl_tools.LASER_MAX
Z_HIT = 0.6
Z_RAND = 0.3
Z_MAX = 0.1

last_time = None
angles = None

# Initially uncertain
parset = [mcl_tools.random_particle() for ii in range(PAR_COUNT)]


# get the weight of a single particle given a laser scan
def particle_weight(scan, particle):
    scan_min = scan.angle_min
    # scan_inc = scan.angle_increment * 50
    scan_inc = scan.angle_increment

    prob = 1.0
    for i in xrange(len(scan.ranges)):
        if i % 50 is 0:
            # print "scan: " + str(i)
            sensed = scan.ranges[i]
            val = scan_min + (i * scan_inc)
            traced = mcl_tools.map_range(particle, val)