Bayesian Filtering Dieter Fox Probabilistic Robotics Key idea: Explicit representation of...

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:

• 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:

),,,|()( 121 tttt zuzuxPxBel

},,,{ 121 ttt zuzud

Markov Assumption

Underlying Assumptions• Static world• Independent noise• Perfect model, no approximation errors

),|(),,|( 1:11:11:1 ttttttt uxxpuzxxp )|(),,|( :11:1:0 tttttt xzpuzxzp

Bayes Filters

),,,|(),,,,|( 121121 ttttt uzuxPuzuxzP Bayes

z = observationu = actionx = state

),,,|()( 121 tttt zuzuxPxBel

Markov ),,,|()|( 121 tttt uzuxPxzP

1111 )(),|()|( ttttttt dxxBelxuxPxzP

Markov1121111 ),,,|(),|()|( tttttttt dxuzuxPxuxPxzP

11211

1121

),,,|(

),,,,|()|(

ttt

ttttt

dxuzuxP

xuzuxPxzP

Total prob.

Bayes Filters are Familiar!

• Kalman filters

• Particle filters

• Hidden Markov models

• Dynamic Bayesian networks

• Partially Observable Markov Decision Processes (POMDPs)

111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel

Localization

• Given • Map of the environment.• Sequence of sensor measurements.

• Wanted• Estimate of the robot’s position.

• Problem classes• Position tracking• Global localization• Kidnapped robot problem (recovery)

“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]

Bayes Filters for Robot Localization

Probabilistic Kinematics

• Odometry information is inherently noisy.

x’u

p(x|u,x’)

u

x’

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

),|(

),|(

),|(

),|(

),|(

rand

max

unexp

hit

rand

max

unexp

hit

mxzP

mxzP

mxzP

mxzP

mxzP

T

How can we determine the model parameters?

Raw Sensor DataMeasured distances for expected distance of 300 cm.

Sonar Laser

Approximation Results

Sonar

Laser

300cm 400cm

Representations for Bayesian Robot Localization

Discrete approaches (’95)• Topological representation (’95)

• uncertainty handling (POMDPs)• occas. global localization, recovery

• Grid-based, metric representation (’96)• global localization, recovery

Multi-hypothesis (’00)• multiple Kalman filters• global localization, recovery

Particle filters (’99)• sample-based representation• global localization, recovery

Kalman filters (late-80s?)• Gaussians• approximately linear models• position tracking

AI

Robotics

Discrete Grid Filters

Piecewise Constant Representation

),,( yxxBel t

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

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

Particle Filters

Weight samples: w = f / g

Importance Sampling

draw xit1 from Bel(xt1)

draw xit from p(xt | xi

t1,ut1)

Importance factor for xit:

)|()(),|(

)(),|()|(ondistributi proposal

ondistributitarget

111

111

tt

tttt

tttttt

it

xzpxBeluxxp

xBeluxxpxzp

w

1111 )(),|()|()( tttttttt dxxBeluxxpxzpxBel

Particle Filter Algorithm

Particle Filters

)|()(

)()|()()|()(

xzpxBel

xBelxzpw

xBelxzpxBel

Sensor Information: Importance Sampling

'd)'()'|()( , xxBelxuxpxBel

Robot Motion

)|()(

)()|()()|()(

xzpxBel

xBelxzpw

xBelxzpxBel

Sensor Information: Importance Sampling

Robot Motion

'd)'()'|()( , xxBelxuxpxBel

Sample-based Localization (sonar)

Using Ceiling Maps for Localization

[Dellaert et al. 99]

Vision-based Localization

P(z|x)

h(x)z

Under a Light

Measurement z: P(z|x):

Next to a Light


Elsewhere


Global Localization Using Vision

Localization for AIBO robots

Adaptive 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- ), the KL-distance between the true posterior and the sample-based approximation is less than .

• Observation: • For fixed and , number of samples only depends on

number k of bins with support:

KLD-sampling

3

12

)1(9

2

)1(9

21

2

1)1,1(

2

1

zkk

kkn

Example Run Sonar

Example Run Laser

Kalman Filters

•Prediction

•Correction

Bayes Filter Reminder

111 )(),|()( tttttt dxxbelxuxpxbel

)()|()( tttt xbelxzpxbel

Gaussians

2

2)(

2

1

2

2

1)(

:),(~)(

x

exp

Nxp

-

Univariate

)()(2

1

2/12/

1

)2(

1)(

:)(~)(

μxΣμx

Σx

Σμx

t

ep

,Νp

d

Multivariate

Gaussians and Linear Functions

Kalman Filter Updates in 1D

1)(with )(

)()(

tTtttttt

tttt

ttttttt QCCCK

CKI

CzKxbel

2,

2

2

22 with )1(

)()(

tobst

tt

ttt

tttttt K

K

zKxbel

tTtttt

tttttt RAA

uBAxbel

1

1)(

2

,2221)(

tactttt

tttttt a

ubaxbel

Kalman Filter Algorithm

1. Algorithm Kalman_filter( t-1, t-1, ut, zt):

2. Prediction:3. 4.

5. Correction:6. 7. 8.

9. Return t, t

ttttt uBA 1

tTtttt RAA 1

1)( tTtttttt QCCCK

)( tttttt CzK

tttt CKI )(

Nonlinear Dynamic Systems

•Most realistic robotic problems involve nonlinear functions

),( 1 ttt xugx

)( tt xhz

Linearity Assumption Revisited

Non-linear Function

EKF Linearization (1)

Particle Filter Projection

Density Extraction

Sampling Variance

EKF Algorithm

1. Extended_Kalman_filter( t-1, t-1, ut, zt):

2. Prediction:3. 4.

5. Correction:6. 7. 8.

9. Return t, t

),( 1 ttt ug

tTtttt RGG 1

1)( tTtttttt QHHHK

))(( ttttt hzK

tttt HKI )(

1

1),(

t

ttt x

ugG

t

tt x

hH

)(

ttttt uBA 1

tTtttt RAA 1

1)( tTtttttt QCCCK

)( tttttt CzK

tttt CKI )(

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

nin

wwn

nw

nw

ic

imi

i

cm

2,...,1for )(2

1 )(

)1( 2000

Sigma points Weights

)( ii g

n

i

Tiiic

n

i

iim

w

w

2

0

2

0

))(('

'

Pass sigma points through nonlinear function

Recover mean and covariance

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:

•Online SLAM:

Integrations typically done one at a time

121:1:1:1:1:1 ,...,,),|,(),|,( ttttttt dxdxdxuzmxpuzmxp

),|,( :1:1:1 ttt uzmxp

• Map with N landmarks:(2N+3)-dimensional Gaussian

• Can handle hundreds of dimensions

2

2

2

2

2

2

2

1

21

21

21

21

2222221

1111211

,),(

yxlll

yyxyylylyl

xxyxxlxlxl

lylxllllll

lylxllllll

lylxllllll

Ntt

N

N

N

NNNNNN

N

N

y

x

l

l

l

mxBel

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

• Sensor readings yield constraints between poses

• Constraints represented by Gaussians

• Globally optimal estimate

ijijij QDD

ijijij QDD

Loop Closure

Before loop closure After loop closure

• Use scan patches to detect loop closure

• Add new position constraints

• Deform the network based on covariances of matches

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

),|,( 1:0:1:1 ttt uzmxp

),|(),,|( 1:0:1:11:0:1:1 tttttt uzxpuzxmp

Compute a posterior over the map and possible trajectories of the robot :

robot motionmap trajectory

map and trajectory

measurements

FastSLAM

Robot Pose 2 x 2 Kalman Filters

Landmark 1 Landmark 2 Landmark N…x, y,

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



ParticleM

…

[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 pathRed line = estimated robot pathBlack dashed line = odometry

Victoria Park Results

• 4 km traverse

• 100 particles

• Uses negative evidence to remove spurious landmarks

Blue path = odometryRed path = estimated path

[End courtesy of Mike Montemerlo]

Motion Model for Scan Matching

'

'

d'

final pose

d

measured pose

initial pose

path

Raw OdometryScan Matching

Rao-Blackwellized Mapping with Scan-Matching

Map:

Inte

l R

ese

arc

h L

ab

Seatt

le

Loop Closure

Rao-Blackwellized Mapping with Scan-Matching

Map:

Inte

l R

ese

arc

h L

ab

Seatt

le

Example (Intel Lab)

15 particles

four times faster than real-timeP4, 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 km (odometry)



joint work with Giorgio Grisetti

30 particles

250x250m2

1.088 miles (odometry)



DP-SLAM [Eliazar & Parr]

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

scale: 3cm

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

Interactions between robot(s) and targetGoal: Unified framework for modeling

the ball and its interactions.

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

k-2b k-1b kb

r k-2 r k-1 r k

z k-2 z k-1 z k

u k-2 u k-1

z k-1 z kz k-2

kmk-1mk-2m

Ball observation

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l l l

b b b

Robot Location

k-2b k-1b kb

r k-2 r k-1 r k

z k-2 z k-1 z k

u k-2 u k-1

z k-1 z kz k-2

kmk-1mk-2m

Ball observation


Ball motion mode


Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l l l

b b b

Robot and Ball Location (and velocity)

k-2b k-1b kb

r k-2 r k-1 r k

z k-2 z k-1 z k

u k-2 u k-1

z k-1 z kz k-2

kmk-1mk-2m

Ball observation


Ball motion mode


Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l l l

b b b

Ball-Environment Interactions

None Grabbed

BouncedKicked

Deflected

Ball-Environment Interactions

None Grabbed

BouncedKicked

(0.8)

Robot loses grab (0.2)

(1.0)

Ro

bo

t kicks b

all (0.9)

(1.0

)

Within grab range and robot grabs

(prob. from model )

(0.1

)

Kick fails (0.1)

Deflected

(residual prob .) (1

.0)

Col

lisio

n w

ith

obje

cts

on m

ap (1

.0)

Integrating Discrete Ball Motion Mode

k-2b k-1b kb

r k-2 r k-1 r k

z k-2 z k-1 z k

u k-2 u k-1

z k-1 z kz k-2

kmk-1mk-2m

Ball observation


Ball motion mode


Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l l l

b b b

Grab Example (1)

k-2b k-1b kb

r k-2 r k-1 r k

z k-2 z k-1 z k

u k-2 u k-1

z k-1 z kz k-2

kmk-1mk-2m

Ball observation


Ball motion mode


Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l l l

b b b

Grab Example (2)

k-2b k-1b kb

r k-2 r k-1 r k

z k-2 z k-1 z k

u k-2 u k-1

z k-1 z kz k-2

kmk-1mk-2m

Ball observation


Ball motion mode


Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l l l

b b b

Inference: Posterior Estimation

k-2b k-1b kb

r k-2 r k-1 r k

z k-2 z k-1 z k

u k-2 u k-1

z k-1 z kz k-2

kmk-1mk-2m

Ball observation


Ball motion mode


Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l l l

b b b

),,|,,( 1:1:1:1 klk

bkkkk uzzrmbp

Rao-Blackwellised PF for Inference Represent posterior by random samples

Each sample

contains robot location, ball mode, ball Kalman filter

Generate individual components of a particle stepwise using the factorization

iiiiiii myxbmrs ,,,,,,,

),|(),,|(),,,|(

),|,,(

1:1:1:11:1:1:1:11:1:1:1:1

1:1:1:1:1

kkkkkkkkkkkk

kkkkk

uzrpuzrmpuzrmbp

uzrmbp

Rao-Blackwellised Particle Filter for Inference

k-1b kb

r k-1 r k

kmk-1m


Ball motion mode


Bal

l tra

ckin

gR

obot

loca

liza

tion

)(1

)(1

)(1 ,, i

ki

ki

k bmr

Draw a sample from the previous sample set:

Generate Robot Location

r k-1 r k

u k-1

z k


Robot control

Landmark detection

Rob

ot lo

cali

zati

on l

k-1b

k-1m


Ball motion mode

Bal

l tra

ckin

g

kb

km

__,,),,,,|(~ )(1

)(1

)(1

)(1

)( ikkk

ik

ik

ikk

ik ruzbmrrpr

Generate Ball Motion Model

k-1b

r k-1 r k

u k-1

z k

kmk-1m


Ball motion mode


Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l

kb

_,,),,,,|(~ )()(1

)(1

)(1

)()( ik

ikkk

ik

ik

ikk

ik mruzbmrmpm

Update Ball Location and Velocity

k-1b

r k-1 r k

u k-1

z k

kmk-1m


Ball motion mode


Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l

kb

z k b

)()()()(1

)()()( ,,),,,|(~ ik

ik

ikk

ik

ik

ikk

ik bmrzbmrbpb

Importance Resampling

Weight sample by

if observation is landmark detection and by

if observation is ball detection.

Resample

)|( )()( ik

lk

ik rzpw

kM

ki

ki

ki

kki

ki

kkbk

ik ubmrMpbrMzpw ),,,|(),,|( 1

)(1

)(1

)()(1

)()(

Ball-Environment Interaction

Tracking and Finding the Ball

Cluster ball samples by discretizing pan / tilt angles

Uses negative information

Experiment: Real Robot

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

0.5

0 5 10 15 20 25 30 35 40 45 50

Pe

rce

nta

ge

of

ba

ll lo

st

Number of ball samples

With MapWithout Map

Robot kicks ball 100 times, tries to find it afterwards

Finds ball in 1.5 seconds on average

Reference* Observations


Simulation Runs

Comparison to KF* (optimized for straight motion)

RBPFKF*


RBPFKF*


RBPFKF’


RBPFKF’


Comparison to KF’ (inflated prediction noise)

Orientation Errors

0

20

40

60

80

100

120

140

160

180

2 3 4 5 6 7 8 9 10 11

Orie

nta

tion

Err

or [

deg

rees

]

Time [sec]

RBPFKF*KF'

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

Bayesian Filtering Dieter Fox Probabilistic Robotics Key idea: Explicit representation of...

Documents

Transcript of Bayesian Filtering Dieter Fox Probabilistic Robotics Key idea: Explicit representation of...