Upload
clarissa-welch
View
244
Download
2
Tags:
Embed Size (px)
Citation preview
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?