Bayesian Filteringfor Robot Localization

CSE-473

Probabilistic Robotics

Key idea: Explicit representation of uncertainty

(using the calculus of probability theory)

•Perception = state estimation

•Control = utility optimization

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

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

Graphical Model of Localization

m

x

z

u

x

z

u

2

2

x

z

u

... t

t

x 1

1

0

10 t-1

Markov Assumption

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

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

Two Time Slice Representation of Dynamic Bayes Net

x

z

u

t-1

t-1

x

z

u

t

t

x0

t-1 t

Belief Update

xt-1 x

z

u

t

t

t

111

111

11

)d(),|()|(

)d()|(),|(

d),,,(),|(

ttttttt

ttttttt

tttttttt

xxpxuxpxzp

xxpxzpxuxp

xxzuxpzuxp

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.

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

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

Sample-based Density Representation

Weight samples: w = f / g

Importance Sampling

Particle Filters

)|()(

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

xzpxBel

xBelxzpw

xBelxzpxBel

Sensor Information: Importance Sampling

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

Robot Motion

)|()(

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

xzpxBel

xBelxzpw

xBelxzpxBel

Sensor Information: Importance Sampling

Robot Motion

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

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

Start

Motion Model Reminder

Proximity Sensor Model Reminder

Laser sensor Sonar sensor

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

Measurement z: P(z|x):

Elsewhere

Measurement z: P(z|x):

Global Localization Using Vision

Localization for AIBO robots

Adaptive Sampling

Example Run Sonar

Example Run Laser

Example Run

Tracking with a Moving Robot

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.

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

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

Ball-Environment Interaction

GPS Receivers We Used

Nokia 6600 Java Cell Phone with Bluetooth

GPS unit

GeoStats wearable

GPS logger

Geographic Information Systems

Bus routes and bus stopsData source: Metro GIS

Street mapData source: Census 2000

Tiger/line data

Adding Mode of Transportation

GPS readingzk-1 zk

Edge, velocity, positionxk-1 xk

k-1 k Data (edge) association

Time k-1 Time k

mk-1 mk Transportation mode

Infer Location and Transportation

Measurements

Projections

Bus mode

Car mode

Foot mode

Green

Red

Blue

Hierarchical Model

GPS readingzk-1 zk

Edge, velocity, positionxk-1 xk

k-1 k Data (edge) association

Time k-1 Time k

mk-1 mk Transportation mode

tk-1 tk Trip segment

gk-1 gk Goal

Predict Goal and Path

Predicted goal

Predicted path

Application: Opportunity Knocks

Detect User Errors

Untrained Trained Instantiated

Application: Opportunity Knocks

Particle Filter Algorithm

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’.

w2

w3

w1wn

Wn-1

Resampling

w2

w3

w1wn

Wn-1

• Roulette wheel

• Binary search, log n

• Stochastic universal sampling

• Systematic resampling

• Linear time complexity

• Easy to implement, low variance

Resampling Algorithm

Probabilistic Kinematics

22 )'()'( yyxxtrans

)','(atan21 xxyyrot

12 ' rotrot

• Robot moves from to .

• Odometry information .

,, yx ',',' yx

transrotrotu ,, 21

trans1rot

2rot

,, yx

',',' yx

Noise Model for Motion

•The measured motion is given by the true motion corrupted with noise.

•To predict sample position, just sample from “noisy version” of measured motion.

||||11 211

ˆtransrotrotrot

||||22 221

ˆtransrotrotrot

|||| 2143

ˆrotrottranstranstrans

Gaussian Noise Model

2

2

22

1

22

1)(

x

ex

Normal distribution

Examples (odometry based)

Motion Model with Map

)',|( xuxP )',|()|(),',|( xuxPmxPmxuxP

• When does this approximation fail?