Transcript Probabilistic Robotics - Interactive Computing Lab

Recursive Bayes Filtering
Advanced AI
Wolfram Burgard
RoboCup
Physical Agents are Inherently
Uncertain

Uncertainty arises from four major
factors:
Environment stochastic, unpredictable
 Robot stochastic
 Sensor limited, noisy
 Models inaccurate

Nature of Sensor Data
Odometry Data
Range Data
Probabilistic Techniques for
Physical Agents
Key idea: Explicit representation of
uncertainty using the calculus of
probability theory
Perception = state estimation
Action = utility optimization
Advantages of Probabilistic
Paradigm
Can accommodate inaccurate models
 Can accommodate imperfect sensors
 Robust in real-world applications
 Best known approach to many hard
robotics problems

Pitfalls
Computationally demanding
 False assumptions
 Approximate

Simple Example of State Estimation


Suppose a robot obtains measurement z
What is P(open|z)?
Causal vs. Diagnostic Reasoning
P(open|z) is diagnostic.
 P(z|open) is causal.
 Often causal knowledge is easier to
obtain.
count frequencies!
 Bayes rule allows us to use causal
knowledge:

P( z | open) P(open)
P(open | z ) 
P( z )
Example


P(z|open) = 0.6
P(z|open) = 0.3
P(open) = P(open) = 0.5
P( z | open) P(open)
P(open | z ) 
P( z | open) p(open)  P( z | open) p(open)
0.6  0.5
2
P(open | z ) 
  0.67
0.6  0.5  0.3  0.5 3
• z raises the probability that the door is open.
Combining Evidence

Suppose our robot obtains another
observation z2.

How can we integrate this new
information?

More generally, how can we estimate
P(x| z1...zn )?
Recursive Bayesian Updating
P( zn | x, z1,, zn  1) P( x | z1,, zn  1)
P( x | z1,, zn) 
P( zn | z1,, zn  1)
Markov assumption: zn is independent of z1,...,zn-1 if
we know x.
P( zn | x) P( x | z1, , zn  1)
P( x | z1,, zn) 
P( zn | z1,, zn  1)
  P( zn | x) P( x | z1,, zn  1)
 1... n
 P( z | x) P( x)
i
i 1... n
Example: Second Measurement

P(z2|open) = 0.5

P(open|z1)=2/3
P(z2|open) = 0.6
P( z2 | open) P(open | z1 )
P(open | z2 , z1 ) 
P( z2 | open) P(open | z1 )  P( z2 | open) P(open | z1 )
1 2

5
2 3


 0.625
1 2 3 1
8
  
2 3 5 3
• z2 lowers the probability that the door is open.
A Typical Pitfall
Two possible locations x1 and x2
 P(x1)= 1-P(x2)= 0.99
 P(z|x2)=0.09 P(z|x1)=0.07

1
p(x2 | d)
p(x1 | d)
0.9
0.8
0.7
p( x | d)
0.6
0.5
0.4
0.3
0.2
0.1
0
5
10
15
20
25
30
Number of integrations
35
40
45
50
Actions

Often the world is dynamic since
actions carried out by the robot,
 actions carried out by other agents,
 or just the time passing by

change the world.

How can we incorporate such
actions?
Typical Actions





The robot turns its wheels to move
The robot uses its manipulator to grasp
an object
Plants grow over time…
Actions are never carried out with
absolute certainty.
In contrast to measurements, actions
generally increase the uncertainty.
Modeling Actions

To incorporate the outcome of an
action u into the current “belief”, we
use the conditional pdf
P(x|u,x’)

This term specifies the pdf that
executing u changes the state
from x’ to x.
Example: Closing the door
State Transitions
P(x|u,x’) for u = “close door”:
0.9
0.1
open
closed
1
0
If the door is open, the action “close
door” succeeds in 90% of all cases.
Integrating the Outcome of Actions
Continuous case:
P( x | u )   P( x | u, x' ) P( x' )dx'
Discrete case:
P( x | u)   P( x | u, x' ) P( x' )
Example: The Resulting Belief
P(closed | u )   P(closed | u , x' ) P( x' )
 P (closed | u , open) P(open)
 P (closed | u , closed ) P(closed )
9 5 1 3 15
    
10 8 1 8 16
P(open | u )   P(open | u , x' ) P( x' )
 P (open | u, open) P(open)
 P (open | u , closed ) P(closed )
1 5 0 3 1
    
10 8 1 8 16
 1  P(closed | u )
Bayes Filters: Framework

Given:
 Stream of observations z and action data u:
dt  {u1 , z2 , ut 1 , zt }




Sensor model P(z|x).
Action model P(x|u,x’).
Prior probability of the system state P(x).
Wanted:
 Estimate of the state X of a dynamical system.
 The posterior of the state is also called Belief:
Bel ( xt )  P( xt | u1 , z2 , ut 1 , zt )
Markov Assumption
Zt-1
Xt-1
Zt+1
Zt
ut-1
Xt
ut
Xt+1
p(dt , dt 1 ,..., d0 | xt , dt 1 , dt 2 , ...)  p(dt , dt 1 ,..., d0 | xt )
p(dt , dt 1 ,... | xt , d1 , d 2 ,..., dt 1 )  p(dt , dt 1 ,... | xt )
p( xt | ut 1 , xt 1 , dt 2 ,..., d 0 )  p( xt | ut 1 , xt 1 )
Underlying Assumptions
 Static world
 Independent noise
 Perfect model, no approximation errors
Bayes Filters
z = observation
u = action
x = state
Bel ( xt )  P( xt | u1 , z2 , ut 1 , zt )
Bayes
  P( zt | xt , u1 , z2 , , ut 1 ) P( xt | u1 , z2 , , ut 1 )
Markov
  P( zt | xt ) P( xt | u1 , z2 , , ut 1 )
Total prob.
Markov
  P( zt | xt )  P( xt | u1 , z2 , , ut 1 , xt 1 )
P( xt 1 | u1 , z2 , , ut 1 ) dxt 1
  P( zt | xt )  P( xt | ut 1 , xt 1 ) P( xt 1 | u1 , z 2 , , ut 1 ) dxt 1
  P( zt | xt )  P( xt | ut 1 , xt 1 ) Bel ( xt 1 ) dxt 1
Bel ( xt )   Filter
P( zt | xt )  Algorithm
P( xt | ut 1 , xt 1 ) Bel ( xt 1 ) dxt 1
Bayes
2.
Algorithm Bayes_filter( Bel(x),d ):
0
3.
if d is a perceptual data item z then
1.
4.
5.
6.
7.
8.
9.
For all x do
Bel ' ( x)  P( z | x) Bel ( x)
    Bel ' ( x)
For all x do
Bel ' ( x)   1Bel ' ( x)
else if d is an action data item u then
10.
11.
For all x do
12.
return Bel’(x)
Bel ' ( x)   P( x | u, x' ) Bel ( x' ) dx'
Bayes Filters are Familiar!
Bel ( xt )   P( zt | xt )  P( xt | ut 1 , xt 1 ) Bel ( xt 1 ) dxt 1





Kalman filters
Particle filters
Hidden Markov models
Dynamic Bayes networks
Partially Observable Markov Decision
Processes (POMDPs)
Representations for Bayesian
Robot Localization
Kalman filters (late-80s?)
Discrete approaches (’95)
• Topological representation (’95)
• uncertainty handling (POMDPs)
• occas. global localization, recovery
• Grid-based, metric representation (’96)
• global localization, recovery
Particle filters (’99)
• sample-based representation
• global localization, recovery
AI
• Gaussians
• approximately linear models
• position tracking
Robotics
Multi-hypothesis (’00)
• multiple Kalman filters
• global localization, recovery
What is the Right Representation?

Kalman filters

Multi-hypothesis tracking

Grid-based representations

Topological approaches

Particle filters
Gaussians
p ( x) ~ N (  ,  2 ) :

1 ( x )

1
p ( x) 
e 2
2 
2
2
Univariate
-

p (x) ~ Ν (μ,Σ) :
p ( x) 
1
(2 )
d /2
Σ
1/ 2
e
Multivariate
1
 ( x μ ) t Σ 1 ( x μ )
2

Kalman Filters
Estimate the state of processes that are
governed by the following linear
stochastic difference equation.
xt 1  Axt  Bu t  vt
zt  Cxt  wt
The random variables vt and wt represent
the process measurement noise and are
assumed to be independent, white and
with normal probability distributions.
59
Kalman Filters
[Schiele et al. 94], [Weiß et al. 94],
[Borenstein 96],
[Gutmann et al. 96, 98], [Arras 98]
Kalman Filter Algorithm
1.
Algorithm Kalman_filter( <,S>, d ):
2.
3.
4.
5.
If d is a perceptual data item z then
6.
Else if d is an action data item u then
7.
8.
9.

K  SC CSC  S obs
    K ( z  C )
T
T

1
S  ( I  KC )S
  A  Bu
S  ASAT  S act
Return <,S>
Non-linear Systems

Very strong assumptions:
Linear state dynamics
 Observations linear in state


What can we do if system is not linear?
Linearize it: EKF
 Compute the Jacobians of the dynamics
and observations at the current state.
 Extended Kalman filter works surprisingly
well even for highly non-linear systems.

Kalman Filter-based Systems (1)

[Gutmann et al. 96, 98]:

Match LRF scans against map

Highly successful in RoboCup mid-size league
Courtesy of S. Gutmann
Kalman Filter-based Systems (2)
Courtesy of S. Gutmann
Kalman Filter-based Systems (3)

[Arras et al. 98]:

Laser range-finder and vision

High precision (<1cm accuracy)
Courtesy of K. Arras
Multihypothesis
Tracking
[Cox 92], [Jensfelt, Kristensen 99]
Localization With MHT

Belief is represented by multiple hypotheses

Each hypothesis is tracked by a Kalman filter

Additional problems:

Data association: Which observation
corresponds to which hypothesis?

Hypothesis management: When to add / delete
hypotheses?

Huge body of literature on target tracking, motion
correspondence etc.
See e.g. [Cox 93]
MHT: Implemented System (1)

[Jensfelt and Kristensen 99,01]

Hypotheses are extracted from LRF scans

Each hypothesis has probability of being the
correct one:
H i  { xˆ i, Si , P ( H i )}

Hypothesis probability is computed using Bayes’
rule
P( s | H i ) P( H i )
P( H i | s ) 
P( s )

Hypotheses with low probability are deleted

New candidates are extracted from LRF scans
C j  {z j , R j }
MHT: Implemented System (2)
Courtesy of P. Jensfelt and S. Kristensen
MHT: Implemented System (3)
Example run
# hypotheses
P(Hbest)
Map and trajectory
Courtesy of P. Jensfelt and S. Kristensen
Hypotheses vs. time
Piecewise
Constant
[Burgard et al. 96,98], [Fox et al. 99],
[Konolige et al. 99]
Piecewise Constant
Representation
bel ( xt  x, y, >)
Grid-based Localization
Tree-based Representations (1)
Idea: Represent density using a variant of Octrees
Xavier:
Localization in a Topological Map
[Simmons and Koenig 96]
Particle Filters


Represent density by random samples

Monte Carlo filter, Survival of the fittest,
Condensation, Bootstrap filter, Particle filter



Filtering: [Rubin, 88], [Gordon et al., 93], [Kitagawa 96]
Estimation of non-Gaussian, nonlinear processes
Computer vision: [Isard and Blake 96, 98]
Dynamic Bayesian Networks: [Kanazawa et al., 95]
MCL: Global Localization
MCL: Sensor Update
Bel ( x)   p( z | x) Bel  ( x)
 p( z | x) Bel  ( x)
w

  p( z | x)

Bel ( x)
MCL: Robot Motion
Bel  ( x) 
 p( x | u x' ) Bel ( x' )
,
d x'
MCL: Sensor Update
Bel ( x)   p( z | x) Bel  ( x)
 p( z | x) Bel  ( x)
w

  p( z | x)

Bel ( x)
MCL: Robot Motion
Bel  ( x) 
 p( x | u x' ) Bel ( x' )
,
d x'
Particle Filter Algorithm
1. Algorithm particle_filter( St-1, ut-1 zt):
2. St  ,
 0
3. For i  1n
Generate new samples
4.
Sample index j(i) from the discrete distribution given by wt-1
5.
Sample xti from p( xt | xt 1 , ut 1 ) using xtj(1i ) and ut 1
6.
wti  p( zt | xti )
Compute importance weight
7.
    wti
Update normalization factor
8.
St  St  { xti , wti >}
Insert
9. For i  1n
10.
wti  wti / 
Normalize weights
Resampling

Given: Set S of weighted samples.

Wanted : Random sample, where the
probability of drawing xi is given by wi.

Typically done n times with replacement to
generate new sample set S’.
Resampling
wn
Wn-1
wn
w1
w2
Wn-1
w3
• Roulette wheel
• Binary search, log n
w1
w2
w3
• Stochastic universal sampling
• Systematic resampling
• Linear time complexity
• Easy to implement, low variance
Motion Model p(xt | at-1, xt-1)
Model odometry error as Gaussian noise on , b, and d
Motion Model p(xt | at-1, xt-1)
Start
Model for Proximity Sensors
The sensor is reflected either by a known or by an
unknown obstacle:
Laser sensor
Sonar sensor
MCL: Global Localization (Sonar)
[Fox et al., 99]
Using Ceiling Maps for Localization
[Dellaert et al. 99]
Vision-based Localization
P(s|x)
s
h(x)
MCL: Global Localization Using Vision
Localization for AIBO robots
Adaptive Sampling
KLD-sampling
• Idea:
• Assume we know the true belief.
• Represent this belief as a multinomial distribution.
• Determine number of samples such that we can guarantee
that, with probability (1- d), the KL-distance between the true
posterior and the sample-based approximation is less than e.
• Observation:
• For fixed d and e, number of samples only depends on
number k of bins with support:

1 2
k 1 
2
2
n
 (k  1,1  d ) 

z1d 
1 
2e
2e  9(k  1)
9(k  1)

3
MCL: Adaptive Sampling (Sonar)
Particle Filters for Robot
Localization (Summary)

Approximate Bayes Estimation/Filtering

Full posterior estimation

Converges in O(1/#samples) [Tanner’93]


Robust: multiple hypotheses with degree of
belief
Efficient in low-dimensional spaces: focuses
computation where needed

Any-time: by varying number of samples

Easy to implement
Localization Algorithms - Comparison
Kalman
filter
Multihypothesis
tracking
Topological
maps
(fixed/variable)
Sensors
Gaussian
Gaussian
Features
Non-Gaussian
NonGaussian
Posterior
Gaussian
Multi-modal
Piecewise
constant
Piecewise
constant
Samples
Efficiency (memory)
++
++
++
-/+
+/++
Efficiency (time)
++
++
++
o/+
+/++
Implementation
+
o
+
+/o
++
++
++
-
+/++
++
Robustness
-
+
+
++
+/++
Global
localization
No
Yes
Yes
Yes
Yes
Accuracy
Grid-based
Particle
filter
Bayes Filtering: Lessons
Learned
General algorithm for recursively
estimating the state of dynamic
systems.
 Variants:

Hidden Markov Models
 (Extended) Kalman Filters
 Discrete Filters
 Particle Filters
