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"
See also:
file: /Techref/method/particlefilters_localization.htm, 3KB, , updated: 2017/7/18 13:32, local time: 2024/4/18 16:32, |
©2024 These pages are served without commercial sponsorship. (No popup ads, etc...).Bandwidth abuse increases hosting cost forcing sponsorship or shutdown. This server aggressively defends against automated copying for any reason including offline viewing, duplication, etc... Please respect this requirement and DO NOT RIP THIS SITE. Questions? <A HREF="http://www.piclist.com/techref/method/particlefilters_localization.htm"> Particle Filter Localization</A> |
Did you find what you needed? |
PICList 2024 contributors:
o List host: MIT, Site host massmind.org, Top posters @none found - Page Editors: James Newton, David Cary, and YOU! * Roman Black of Black Robotics donates from sales of Linistep stepper controller kits. * Ashley Roll of Digital Nemesis donates from sales of RCL-1 RS232 to TTL converters. * Monthly Subscribers: Gregg Rew. on-going support is MOST appreciated! * Contributors: Richard Seriani, Sr. |
Welcome to www.piclist.com! |
.