Bayesian Filtering

Dieter Fox
Probabilistic Robotics
Key idea: Explicit representation of
uncertainty
(using the calculus of probability theory)

Perception = state estimation

Control = utility optimization
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

p( zt | x0:t , z1:t 1, u1:t ) p( zt | xt )

p( xt | x1:t 1, z1:t 1, u1:t ) p( xt | xt 1, ut )
Underlying Assumptions
Static world
Independent noise
Perfect model, no approximation errors
 z = observation
u = action
Bayes Filters 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. P( zt | xt ) P( xt | u1 , z2 , , ut 1 , xt 1 )
P( xt 1 | u1 , z2 , , ut 1 ) dxt 1
Markov 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
Bayes Filters are Familiar!

Bel ( xt ) P( zt | xt ) P( xt | ut , xt 1 ) Bel ( xt 1 ) dxt 1

Kalman filters
Particle filters
Hidden Markov models
Dynamic Bayesian networks
Partially Observable Markov Decision
Processes (POMDPs)
Localization
Using sensory information to locate the robot
in its environment is the most fundamental
problem to providing a mobile robot with
autonomous capabilities. [Cox 91]

Given
Map of the environment.
Sequence of sensor measurements.
Wanted
Estimate of the robots position.
Problem classes
Position tracking
Global localization
Kidnapped robot problem (recovery)
Bayes Filters for
Robot Localization
Probabilistic Kinematics
Odometry information is inherently noisy.

p(x|u,x)

x x
u
u
Proximity Measurement
Measurement can be caused by
a known obstacle.
cross-talk.
an unexpected obstacle (people, furniture, ).
missing all obstacles (total reflection, glass, ).
Noise is due to uncertainty
in measuring distance to known obstacle.
in position of known obstacles.
in position of additional obstacles.
whether obstacle is missed.
Mixture Density

hit
T
Phit ( z | x, m)

unexp Punexp ( z | x, m)
P ( z | x, m )
max Pmax ( z | x, m)

P ( z | x, m )
rand rand

How can we determine the model parameters?

 Raw Sensor Data
Measured distances for expected distance of 300 cm.

Sonar Laser
Approximation Results

Laser

Sonar

300cm 400cm
 Representations for Bayesian
Robot Localization
Kalman filters (late-80s?)
Discrete approaches (95) Gaussians
Topological representation (95) approximately linear models
uncertainty handling (POMDPs) position tracking
occas. global localization, recovery
Grid-based, metric representation (96)
global localization, recovery
Robotics
AI
Particle filters (99) Multi-hypothesis (00)
sample-based representation multiple Kalman filters
global localization, recovery global localization, recovery
Discrete Grid Filters
Piecewise Constant
Representation
Bel ( xt x, y, )
Grid-based Localization
Sonars and
Occupancy Grid Map
Tree-based Representation

Idea: Represent density using a variant of Octrees

Tree-based Representations

Efficient in space and time

Multi-resolution
Particle Filters
 Particle Filters

Represent belief by random samples

Estimation of non-Gaussian, nonlinear processes

Monte Carlo filter, Survival of the fittest,

Condensation, Bootstrap filter, Particle filter

Filtering: [Rubin, 88], [Gordon et al., 93], [Kitagawa 96]

Computer vision: [Isard and Blake 96, 98]
Dynamic Bayesian Networks: [Kanazawa et al., 95]d
Importance Sampling

Weight samples: w = f / g
Particle Filter Algorithm
Bel ( xt ) p( zt | xt ) p( xt | xt 1 , ut 1 ) Bel ( xt 1 ) dxt 1

draw xit1 from Bel(xt1)

draw xit from p(xt | xit1,ut1)

Importance factor for xit:

target distributi on
w
i
t
proposal distributi on
p( zt | xt ) p( xt | xt 1 , ut 1 ) Bel ( xt 1 )

p( xt | xt 1 , ut 1 ) Bel ( xt 1 )
p( zt | xt )
Particle Filters
Sensor Information: Importance Sampling
Bel ( x) p( z | x) Bel ( x)
p( z | x) Bel ( x)
w
p( z | x)
Bel ( x)
Robot Motion
Bel ( x) p( x | u x' ) Bel ( x' )
, d x'
Sensor Information: Importance Sampling
Bel ( x) p( z | x) Bel ( x)
p( z | x) Bel ( x)
w
p( z | x)
Bel ( x)
Robot Motion
Bel ( x) p( x | u x' ) Bel ( x' )
, d x'
Sample-based Localization (sonar)
Using Ceiling Maps for Localization

[Dellaert et al. 99]

Vision-based Localization

P(z|x)
z
h(x)
 Under a Light
Measurement z: P(z|x):
 Next to a Light
Measurement z: P(z|x):
 Elsewhere
Measurement z: P(z|x):
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:
3
1 2 k 1 2 2
n (k 1,1 d ) 1 z1d
2e 2e 9(k 1) 9(k 1)
Example Run Sonar
Example Run Laser
Kalman Filters
Bayes Filter Reminder
Prediction
bel ( xt ) p( xt | ut , xt 1 ) bel ( xt 1 ) dxt 1

Correction
bel ( xt ) p( zt | xt ) bel ( xt )
Gaussians
p( x) ~ N ( , 2 ) :

1 ( x ) 2
1
p( x) e 2 2
2
Univariate -

p (x) ~ (,) :

1
1 ( x ) t 1 ( x )
p ( x) e 2

(2 )
1/ 2
d /2

Multivariate
Gaussians and Linear Functions
Kalman Filter Updates in 1D
t t t K
a t
( ztt tb) t ut t2
bel((xtx)t )
2 Kt 2
bel t 1 with
t 2 (1 2Kt )2 t t obs
2 2

t a 2
t t act ,t
,t

t t Kt ( zt Ct t )
bel ( xt ) t At t 1 Bt ut with Kt t Ct (Ct t CtT Qt ) 1
bel ( xt ) t ( I Kt Ct )T t
t At t 1 At Rt
Kalman Filter Algorithm
1. Algorithm Kalman_filter( t-1, t-1, ut, zt):

2. Prediction:
3. t At t 1 Bt ut
4. t At t 1 AtT Rt

5. Correction:
6. Kt t Ct (Ct t CtT Qt )1
7. t t Kt ( zt Ct t )
8. t ( I Kt Ct )t
9. Return t, t
Nonlinear Dynamic Systems
Most realistic robotic problems involve
nonlinear functions

xt g (ut , xt 1 )

zt h( xt )
Linearity Assumption Revisited
Non-linear Function
EKF Linearization (1)
EKF Linearization (2)
EKF Linearization (3)
Particle Filter Projection
Density Extraction
Sampling Variance
EKF Algorithm
1. Extended_Kalman_filter( t-1, t-1, ut, zt):

2. Prediction:
3. t g (ut , t 1 ) t At t 1 Bt ut
4. t Gt t 1GtT Rt t At t 1 AtT Rt

5. Correction:
6. Kt t Ht ( H t t H tT Qt ) 1 Kt t Ct (Ct t CtT Qt )1
7. t t Kt ( zt h(t )) t t Kt ( zt Ct t )
8. t ( I Kt H t )t t ( I Kt Ct )t
9. Return t, t h( t ) g (ut , t 1 )
Ht Gt
xt xt 1
Landmark-based Localization
EKF Prediction Step
EKF Observation Prediction Step
EKF Correction Step
Estimation Sequence (1)
Estimation Sequence (2)
Comparison to GroundTruth
EKF Summary

Highly efficient: Polynomial in

measurement dimensionality k and
state dimensionality n:
O(k2.376 + n2)

Not optimal!
Can diverge if nonlinearities are large!
Works surprisingly well even when all
assumptions are violated!
Linearization via Unscented
Transform

EKF UKF
UKF Sigma-Point Estimate (2)

EKF UKF
UKF Sigma-Point Estimate (3)

EKF UKF
 Unscented Transform
Sigma points Weights


0
w
0
w
0
(1 2 )
n n
m c

i ( n )
i
wmi wci
1
2(n )
for i 1,...,2n

Pass sigma points through nonlinear function

i g( i )
Recover mean and covariance
2n
' wmi i
i 0
2n
' wci ( i )( i )T
i 0
UKF Prediction Step
UKF Observation Prediction Step
UKF Correction Step
EKF Correction Step
Estimation Sequence

EKF PF UKF
Estimation Sequence

EKF UKF
Prediction Quality

EKF UKF
UKF Summary

Highly efficient: Same complexity as

EKF, with a constant factor slower in
typical practical applications
Better linearization than EKF:
Accurate in first two terms of Taylor
expansion (EKF only first term)
Derivative-free: No Jacobians needed
Still not optimal!
 SLAM: Simultaneous
Localization and Mapping
Mapping with Raw Odometry
SLAM:
Simultaneous Localization and Mapping

Full SLAM:
p( x1:t , m | z1:t , u1:t )

Online SLAM:
p( xt , m | z1:t , u1:t ) p( x1:t , m | z1:t , u1:t ) dx1 ,dx2 ,..., dxt 1

Integrations typically done one at a time

SLAM: Mapping with Kalman Filters
Map with N landmarks:(2N+3)-dimensional
Gaussian
l1 l21 l1l2 l1lN l1x l1 y l1

l2 l1l2 l22 l2lN l2 x l2 y l2


Bel ( xt , mt ) lN , l1lN l2l N l2N lN x lN y lN
x
l1x l2 x lN x x xy x
2

y l y l2 y lN y xy y2 y
1
l l2 lN x y 2
1

Can handle hundreds of dimensions

SLAM: Mapping with Kalman Filters
SLAM: Mapping with Kalman Filters
SLAM: Mapping with Kalman Filters

Map Correlation matrix

Graph-SLAM

Full SLAM technique

Generates probabilistic links

Computes map only occasionally

Based on Information Filter form

Graph-SLAM Idea
 Robot Poses and Scans [Lu and Milios
1997]

Successive robot
poses connected by
odometry
Dij Dij Qij
Sensor readings yield
constraints between
poses
Constraints
represented by
Gaussians
Dij Dij Qij

Globally optimal

estimate arg max P( Dij | Dij )
X

Loop Closure
Use scan patches to detect loop closure
Add new position constraints
Deform the network based on covariances of
matches

Before loop closure After loop closure

Efficient Map Recovery

Minimize constraint function JGraphSLAM

using standard optimization
techniques (gradient descent, Levenberg
Marquardt, conjugate gradient)
Mapping the Allen Center
Rao-Blackwellised
Particle Filters
 Rao-Blackwellized Mapping
Compute a posterior over the map and possible
trajectories of the robot :
map and trajectory

measurements
p( x1:t , m | z1:t , u0:t 1 )
p(m | x1:t , z1:t , u0:t 1 ) p( x1:t | z1:t , u0:t 1 )

map robot motion trajectory

 FastSLAM

Robot Pose 2 x 2 Kalman Filters

Particle
#1
x, y, Landmark 1 Landmark 2 Landmark N

Particle
#2
x, y, Landmark 1 Landmark 2 Landmark N

Particle
#3
x, y, Landmark 1 Landmark 2 Landmark N

Particle
M
x, y, Landmark 1 Landmark 2 Landmark N

[Begin courtesy of Mike Montemerlo]

 FastSLAM Simulation

Up to 100,000
landmarks

100 particles
103 times fewer
parameters than
EKF SLAM

Blue line = true robot path

Red line = estimated robot path
Black dashed line = odometry
Victoria Park Results

4 km traverse
100 particles
Uses negative
evidence to remove
spurious landmarks

Blue path = odometry

Red path = estimated path

[End courtesy of Mike Montemerlo]

Motion Model for Scan Matching

measured pose '

Raw Odometry d'
initial pose d
Scan Matching final pose
'

path
Rao-Blackwellized Mapping with
Scan-Matching

Map: Intel Research Lab Seattle

Loop Closure
Rao-Blackwellized Mapping with
Scan-Matching

Map: Intel Research Lab Seattle

Loop Closure
Rao-Blackwellized Mapping with
Scan-Matching

Map: Intel Research Lab Seattle

Example (Intel Lab)
15 particles
four times faster
than real-time
P4, 2.8GHz
5cm resolution
during scan
matching
1cm resolution in
final map

joint work with Giorgio Grisetti

Outdoor Campus Map
30 particles
250x250m2
1.75
1.088km
miles
(odometry)
20cm resolution
during scan
matching
30cm resolution
in final map

joint work with Giorgio Grisetti

 DP-SLAM [Eliazar & Parr]

scale: 3cm

Runs at real-time speed on 2.4GHz Pentium 4 at 10cm/s

Consistency
Results obtained with
DP-SLAM 2.0 (offline)

Eliazar & Parr, 04

Close up

End courtesy of Eliazar & Parr

Fast-SLAM Summary
Full and online version of SLAM
Factorizes posterior into robot trajectories
(particles) and map (EKFs).

Landmark locations are independent!

More efficient proposal distribution through
Kalman filter prediction

Data association per particle

Ball Tracking
in RoboCup

Extremely noisy (nonlinear) motion of

observer
Inaccurate sensing, limited processing
power
Interactions between target and
environment
Goal: Unified framework for modeling the ball
Interactionsand its interactions.
between robot(s) and target
Tracking Techniques

Kalman Filter
Highly efficient, robust (even for nonlinear)
Uni-modal, limited handling of nonlinearities
Particle Filter
Less efficient, highly robust
Multi-modal, nonlinear, non-Gaussian
Rao-Blackwellised Particle Filter, MHT
Combines PF with KF
Multi-modal, highly efficient
Dynamic Bayes Network for Ball
Tracking

Robot localization
l l
z k-2 zk-1 z lk Landmark detection

r k-2 r k-1 r k Map and robot location

u k-2 u k-1 Robot control
mk-2 mk-1 mk Ball motion mode

Ball tracking
bk-2 bk-1 bk Ball location and velocity

b b
z k-2 zk-1 z bk Ball observation
Robot Location

Robot localization
l l
z k-2 zk-1 z lk Landmark detection

r k-2 r k-1 r k Map and robot location

u k-2 u k-1 Robot control
mk-2 mk-1 mk Ball motion mode

Ball tracking
bk-2 bk-1 bk Ball location and velocity

b b
z k-2 zk-1 z bk Ball observation
Robot and Ball Location (and
velocity)

Robot localization
l l
z k-2 zk-1 z lk Landmark detection

r k-2 r k-1 r k Map and robot location

u k-2 u k-1 Robot control
mk-2 mk-1 mk Ball motion mode

Ball tracking
bk-2 bk-1 bk Ball location and velocity

b b
z k-2 zk-1 z bk Ball observation
Ball-Environment Interactions

None Grabbed

Bounced Deflected
Kicked
Ball-Environment Interactions

(0.8)

(residual prob .) Robot loses grab

(0.2)

None Within grab range

Grabbed
and robot grabs
(prob. from model )

Robot kicks
ball (0.9)
Kick fails (0.1)

Bounced Deflected
Kicked
Integrating Discrete Ball
Motion Mode

Robot localization
l l
z k-2 zk-1 z lk Landmark detection

r k-2 r k-1 r k Map and robot location

u k-2 u k-1 Robot control
mk-2 mk-1 mk Ball motion mode

Ball tracking
bk-2 bk-1 bk Ball location and velocity

b b
z k-2 zk-1 z bk Ball observation
Grab Example (1)

Robot localization
l l
z k-2 zk-1 z lk Landmark detection

r k-2 r k-1 r k Map and robot location

u k-2 u k-1 Robot control
mk-2 mk-1 mk Ball motion mode

Ball tracking
bk-2 bk-1 bk Ball location and velocity

b b
z k-2 zk-1 z bk Ball observation
Grab Example (2)

Robot localization
l l
z k-2 zk-1 z lk Landmark detection

r k-2 r k-1 r k Map and robot location

u k-2 u k-1 Robot control
mk-2 mk-1 mk Ball motion mode

Ball tracking
bk-2 bk-1 bk Ball location and velocity

b b
z k-2 zk-1 z bk Ball observation
Inference: Posterior
Estimation

Robot localization
l l
z k-2 zk-1 z lk Landmark detection

r k-2 r k-1 r k Map and robot location

u k-2 u k-1 Robot control
mk-2 mk-1 mk Ball motion mode

Ball tracking
bk-2 bk-1 bk Ball location and velocity

b b
z k-2 zk-1 z bk Ball observation

b l
p(bk , mk , rk | z , z , u1:k 1 )
1:k 1:k
Rao-Blackwellised PF for
Inference
Represent posterior by random samples
Each sample
si ri , mi , bi x, y, i , mi , , i

contains robot location, ball mode, ball Kalman

filter
Generate individual components of a particle
stepwise using the factorization
p(bk , m1:k , r1:k | z1:k , u1:k 1 )
p(bk | m1:k , r1:k , z1:k , u1:k 1 ) p(m1:k | r1:k , z1:k , u1:k 1 ) p(r1:k | z1:k , u1:k 1 )
Rao-Blackwellised Particle Filter for
Inference

Robot localization
r k-1 r k Map and robot location

mk-1 mk Ball motion mode

Ball tracking
bk-1 bk Ball location and velocity

Draw a sample from the previous sample set:

(i ) (i ) (i )
r , m ,b
k 1 k 1 k 1
Generate Robot Location

Robot localization
z lk Landmark detection

r k-1 r k Map and robot location

u k-1 Robot control
mk-1 mk Ball motion mode

Ball tracking
bk-1 bk Ball location and velocity

rk(i ) ~ p(rk | rk(i 1) , mk(i)1 , bk(i)1 , zk , uk 1 ) rk(i ) , _, _

Generate Ball Motion Model

Robot localization
z lk Landmark detection

r k-1 r k Map and robot location

u k-1 Robot control
mk-1 mk Ball motion mode

Ball tracking
bk-1 bk Ball location and velocity

mk(i ) ~ p(mk | rk(i ) , mk(i)1 , bk(i)1 , zk , uk 1 ) rk(i ) , mk(i ) , _

Update Ball Location and
Velocity

Robot localization
z lk Landmark detection

r k-1 r k Map and robot location

u k-1 Robot control
mk-1 mk Ball motion mode

Ball tracking
bk-1 bk Ball location and velocity

z bk

bk(i ) ~ p(bk | rk(i ) , mk(i ) , bk(i)1 , zk ) rk(i ) , mk(i ) , bk(i )

Importance Resampling
Weight sample by
wk(i ) p( zkl | rk(i ) )
if observation is landmark detection and by
wk(i ) p( zkb | M k , rk(i ) , bk(i)1 ) p(M k | rk(i ) , mk(i)1, bk(i)1, uk 1 )
Mk

if observation is ball detection.

Resample
Ball-Environment Interaction
Ball-Environment Interaction
Tracking and Finding the Ball

Cluster ball samples by discretizing

pan / tilt angles
Uses negative information
Experiment: Real Robot

Robot kicks ball 100 times, tries to find it

afterwards
Finds ball0.5in 1.5 seconds on average
With Map
Without Map
0.45
Percentage of ball lost

0.4
0.35
0.3
0.25
0.2
0.15
0.1
0.05
0
0 5 10 15 20 25 30 35 40 45 50
Number of ball samples
Simulation Runs
Reference
* Observations
Comparison to KF* (optimized for straight
motion)

RBPF
KF*
Reference
* Observations
Comparison to KF (inflated prediction noise)

RBPF
KF
Reference
* Observations
Orientation Errors

180
RBPF
KF*
160 KF'
Orientation Error [degrees]

140
120
100
80
60
40
20
0
2 3 4 5 6 7 8 9 10 11
Time [sec]
Conclusions

Bayesian filters are the most successful

technique in robotics (vision?)
Many instances (Kalman, particle, grid,
MHT, RBPF, )
Special case of dynamic Bayesian
networks
Recently: hierarchical models