1Lecture 16:Particle FiltersCS 344R/393R: RoboticsBenjamin KuipersMarkov Localization• The integral is evaluated over all xt-1.– It computes the probability of reaching xt fromany location xt-1, using the action ut-1.• The equation is evaluated for every xt.– It computes the posterior probabilitydistribution of xt.• Computational efficiency is a problem.– o(k2) if there are k poses xt.– k reflects resolution of position and orientation.! Bel(xt) ="P(zt| xt) P(xt| ut#1, xt#1)$Bel(xt#1) dxt#12Action and Sensor Models• Action model: P(xt | ut-1, xt-1)• Sensor model: P(zt | xt)• Distributions over possible values of xt or ztgiven specific values of other variables.• We discussed these last time.Monte Carlo Simulation• Given a probability distribution over inputs,computing the distribution over outcomescan be hard.– Simulating a concrete instance is easy.• Sample concrete instances (“particles”)from the input distribution.• Collect the outcomes.– The distribution of sample outcomesapproximates the desired distribution.• This has been called “particle filtering.”3Actions Disperse the Distribution• N particlesapproximate aprobabilitydistribution.• The distributiondisperses underactionsMonte Carlo Localization• A concrete instance is a particular pose.– A pose is position plus orientation.• A probability distribution is represented bya collection of N poses.– Each pose has an importance factor.– The importance factors sum to 1.• Initialize with– N uniformly distributed poses.– Equal importance factors of N-1.4Localization Movie (known map)Representing a Distribution• The distribution Bel(xt) is represented by aset St of N weighted samples: where• A particle filter is a Bayes filter that usesthis sample representation. ! St= xt(i),wt(i)| i =1,L N{ }! wt(i)i=1N"= 15Importance Sampling• Sample from a proposal distribution.– Correct to approximate a target distribution.Simple Example• Uniform distribution• Weighting by sensormodel• Prediction by actionmodel• Weighting by sensormodel6The Basic Particle Filter Algorithm• Input: ut-1, zt,– St := ∅, i := 1, α := 0• while i ≤ N do– sample j from the discrete distribution given bythe weights in St-1– sample xt(i) from p(xt | ut-1, xt-1) given xt-1(j) and ut-1.– wt(i) := p(zt | xt(i))–α := α + wt(i); i := i + 1– St := St ∪ {〈xt(i), wt(i)〉}• for i := 1 to N do wt(i) := wt(i)/ α• return St ! St"1= xt"1(i),wt"1(i)| i =1,L N{ }Sampling from aWeighted Set of Particles• Given• Draw α from a uniform distributionover [0,1].• Find the minimum k such that• Return ! St= xt(i),wt(i)| i =1,L N{ }! wt(i)i=1k">#! xt(k )01w(4)w(1)w(2)w(3)w(10)w(11)7KLD Sampling• The number N of samples needed can be adapteddynamically, based on the discrete χ2 (chi-squared)statistic.• At each iteration of the particle filter, determine thenumber of samples such that, with probability 1−δ,the error between the true posterior and the sample-based approximation is less than ε.• See the handout [Fox, IJRR, 2003].Kullbach-Liebler Distance• Consider an unknown distribution p(x) thatwe approximate with the distribution q(x).– How much extra information is required?• KL distance is non-negative, and zero onlywhen q(x)=p(x), but it’s not symmetric.– So it’s not a metric.! KL( p || q) = " p(x)logq(x) " " p(x)log p(x)x#$ % & ' ( ) x#= p(x)logx#p(x)q(x)8KLD Sampling• Let p(x) be the true distribution over k bins.• Let q(x) be the maximum likelihoodestimate of p(x) given n samples.• We can guarantee P(KL(p||q) ≤ ε) = 1−δ bychoosing the number of samples naccording to the Chi-square distributionwith k−1 degrees of freedom.! n =12"#k$1,1$%2=k $12"1$29(k $1)+29(k $1)z1$%& ' ( ) * + Chi-Square Distribution• If Xi ∼ N(0,1) are k independent randomvariables, then the random variable is distributed according to the Chi-squaredistribution with k degrees of freedom: Q ∼ χk2! Q = Xi2i=1k"9Localization Movie (known map)MCL Algorithm• Repeat to collect N samples.– Draw a sample xt-1 from the distribution Bel(xt-1),with likelihood given by its importance factor.– Given an action ut-1 and the action modeldistribution P(xt | ut-1, xt-1), sample state xt.– Assign the importance factor P(zt | xt) to xt.• Normalize the importance factors.• Repeat for each time-step.! Bel(xt) ="P(zt| xt) P(xt| ut#1, xt#1)$Bel(xt#1) dxt#110MCL works quite well• N=1000 seems to work OK.• Straight MCL works best for sensors thatare not highly accurate.– For very accurate sensors, P(zt | xt) is verynarrow and highly peaked.– Poses xt that are nearly (but not exactly) correctcan get low importance values.– They may be under-represented in the nextgeneration.An Alternative:Mixture Proposal Distribution• In Monte Carlo, the proposal distribution isthe distribution for selecting the concretehypotheses (“particles”).• For MCL, the proposal distribution for• Instead, we can use a mixture of severaldifferent proposal distributions.! xt, xt"1is P(xt| ut"1, xt"1) # Bel(xt"1)11Dual Proposal Distribution• Draw sample particles xt based on the sensormodel distribution P(zt | xt).– This is not straight-forward.• Draw sample particles xt-1 based on Bel(xt-1).– The proposal distribution for• Each pair gets importance factor P(xt | ut-1, xt-1)• Normalize to sum to 1.! Bel(xt) ="P(zt| xt) P(xt| ut#1, xt#1)$Bel(xt#1) dxt#1! xt, xt"1is P(zt| xt) # Bel(xt"1)Mixture Proposal• Some particles are proposed based on priorposition and the action model.– Vulnerable to problems with highly accuratesensors!• Some particles are proposed based on priorposition and the sensor model.– Vulnerable to problems due to sensor noise.• A mixture does better than either.– Good results with as few as N = 50 particles!– Use k M1 + (1-k) M2 for 0 < k < 1.12A mixture proposal distributionfor the mapping assignment• Use a mixture of– 90% MCL proposal distribution based on thesensor and action models given;– 10% broader Gaussian distribution, spreadingparticles around in case you have a major error.• The dual proposal distribution is too hard toimplement.Make a Good Graphical Display• Show the evolving occupancy grid map at each step.• Show the distribution of particles during localization.– The display can only show (x, y), but– The particle is really (x, y, θ)• Start at the origin of a big array (memory is cheap!).–
View Full Document