Particle Filter Localization

Continuous multi-modal tracking which scales well and is simple to implement. It maintains a set of many vectors containing position and orientation data. Each of these represents a guess as to position. Some measurement, e.g. the distance to each of several landmarks, compared to the position guess of each particle, determines that particles likelihood of being the accurate position. Particles with a low probability of being correct are deleted, and replaced with new particles which are copies of the high probability particles, in a process called "resampling".

We can normalize the measurement errors as weights to find the probabilities which which each particle should be treated by summing all the weights and dividing each weight by that total. During resampling, we copy the the existing particles into a new list according to the probability that they are correct. Particles with low probability should be less likely (but still possible!) to get copied, and those with high probability are likely to be copied multiple times.

To select higher weighted items with greater frequency, start a new, empty list. Initialize a sum at 0 and an index into the particle list. Add the weight of that particle to the sum, and Iterate through the list from that starting point on, until the sum overflows the maximum. Add that particle to the new list.

Between resampling, move each particle a little bit in generally it's current orientation then measure and compute a new fitness weight. This ensures that particles which are in the correct location, but which are oriented incorrectly, will move out of favor.

Efficiency can be improved by skipping the normalization step, and intead finding the maximum fitness weight (easily done while calculating each weight), then picking the item which causes the sum to exceed that maximum.

#Given P list of particles with W list weights for each P
#copy random particles to new list biased by weight
Pn = [] #make a new list
idx=0 #int(random.random()*len(P)) #no need to start at random idx
mw = max(W) #find largest weight.
for i in range(len(P))
	s += random.random() * mw
	while s > W[idx]
		s -= W[idx]
		idx = (idx + 1) % N
	Pn.append(P[idx])

#alternate code with normalize weights
s = 0.0
for i in range(len(W)):
    s = s + W[i]
for i in range(len(W)):
    W[i] = W[i]/s

Pn = [] #new list
c=0.0
N = len(P)
while len(Pn) < N:
    c = c + W[i]
    if c >= 1: 
        Pn.append(P[i])
        print "found #",i," with weight",W[i]
        c = 0.0
    i = i + 1
    if i >= N:
        i=0
        print "looped"