Post on 13-Jun-2020
transcript
1
CSE-571Probabilistic Robotics
Particle filters for tracking
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 target
Goal: Unified framework for modeling the ball and its interactions.
Dieter Fox 2CSE-571: Probabilistic Robotics
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
Dieter Fox 3CSE-571: Probabilistic Robotics
Dynamic Bayes Network for Ball Tracking
k-2b k-1b kb
r k-2 r k-1 r k
zk-2 zk-1 z k
u k-2 u k-1
zk-1 zkzk-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tr
ack
ing
Ro
bo
t lo
cali
zati
on
l l l
b b b
Dieter Fox 4CSE-571: Probabilistic Robotics
2
Robot Location
k-2b k-1b kb
r k-2 r k-1 r k
zk-2 zk-1 z k
u k-2 u k-1
zk-1 zkzk-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tr
ack
ing
Ro
bo
t lo
cali
zati
on
l l l
b b b
Dieter Fox 5CSE-571: Probabilistic Robotics
Robot and Ball Location (and velocity)
k-2b k-1b kb
r k-2 r k-1 r k
zk-2 zk-1 z k
u k-2 u k-1
zk-1 zkzk-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tr
ack
ing
Ro
bo
t lo
cali
zati
on
l l l
b b b
Dieter Fox 6CSE-571: Probabilistic Robotics
Ball-Environment Interactions
None Grabbed
BouncedKicked
Deflected
Dieter Fox 7CSE-571: Probabilistic Robotics
Ball-Environment Interactions
None Grabbed
BouncedKicked
(0.8)
Robot loses grab
(0.2)
Ro
bo
tkic
ks
ball
(0.9
)
Within grab range
and robot grabs
(prob. from model )
Kick fails (0.1)
Deflected
(residual prob .)
Dieter Fox 8CSE-571: Probabilistic Robotics
3
Integrating Discrete Ball Motion Mode
k-2b k-1b kb
r k-2 r k-1 r k
zk-2 zk-1 z k
u k-2 u k-1
zk-1 zkzk-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tr
ack
ing
Ro
bo
t lo
cali
zati
on
l l l
b b b
Dieter Fox 9CSE-571: Probabilistic Robotics
Grab Example (1)
k-2b k-1b kb
r k-2 r k-1 r k
zk-2 zk-1 z k
u k-2 u k-1
zk-1 zkzk-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tr
ack
ing
Ro
bo
t lo
cali
zati
on
l l l
b b b
Dieter Fox 10CSE-571: Probabilistic Robotics
Grab Example (2)
k-2b k-1b kb
r k-2 r k-1 r k
zk-2 zk-1 z k
u k-2 u k-1
zk-1 zkzk-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tr
ack
ing
Ro
bo
t lo
cali
zati
on
l l l
b b b
Dieter Fox 11CSE-571: Probabilistic Robotics
Inference: Posterior Estimation
k-2b k-1b kb
r k-2 r k-1 r k
zk-2 zk-1 z k
u k-2 u k-1
zk-1 zkzk-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tr
ack
ing
Ro
bo
t lo
cali
zati
on
l l l
b b b
),,|,,(1:1:1:1 k
l
k
b
kkkkuzzrmbp
Dieter Fox 12CSE-571: Probabilistic Robotics
4
Particle Filter for Robot Localization
Dieter Fox 13CSE-571: Probabilistic Robotics
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
iiiiiiimyxbmrs ,,,,,,,
),|(),,|(),,,|(
),|,,(
1:1:1:11:1:1:1:11:1:1:1:1
1:1:1:1:1
kkkkkkkkkkkk
kkkkk
uzrpuzrmpuzrmbp
uzrmbp
Dieter Fox 14CSE-571: Probabilistic Robotics
Rao-Blackwellised Particle Filter for Inference
k-1b kb
r k-1 r k
kmk-1m
Ball location and velocity
Ball motion mode
Map and robot location
Bal
l tr
ack
ing
Ro
bo
t lo
cali
zati
on
)(
1
)(
1
)(
1,,
i
k
i
k
i
kbmr
Draw a sample from the previous sample set:
Dieter Fox 15CSE-571: Probabilistic Robotics
Generate Robot Location
r k-1 r k
u k-1
zk
Map and robot location
Robot control
Landmark detection
Ro
bo
t lo
cali
zati
on
l
k-1b
k-1m
Ball location and velocity
Ball motion mode
Bal
l tr
ack
ing
kb
km
__,,),,,,|(~)(
1
)(
1
)(
1
)(
1
)( i
kkk
i
k
i
k
i
kk
i
kruzbmrrpr
Dieter Fox 16CSE-571: Probabilistic Robotics
5
Generate Ball Motion Model
k-1b
r k-1 r k
u k-1
zk
kmk-1m
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tr
ack
ing
Ro
bo
t lo
cali
zati
on
l
kb
_,,),,,,|(~)()(
1
)(
1
)(
1
)()( i
k
i
kkk
i
k
i
k
i
kk
i
kmruzbmrmpm
Dieter Fox 17CSE-571: Probabilistic Robotics
Update Ball Location and Velocity
k-1b
r k-1 r k
u k-1
zk
kmk-1m
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tr
ack
ing
Ro
bo
t lo
cali
zati
on
l
kb
z kb
)()()()(
1
)()()(,,),,,|(~
i
k
i
k
i
kk
i
k
i
k
i
kk
i
kbmrzbmrbpb
Dieter Fox 18CSE-571: Probabilistic Robotics
Importance Resampling
Weight sample by
if observation is landmark detection and by
if observation is ball detection.
Resample
)|()()( i
k
l
k
i
krzpw
k
i
k
i
k
i
k
i
k
i
k
i
k
i
k
b
k
i
k
i
k
i
k
b
k
i
k
bbrmbpbrmzp
brmzpw
d),,|(),,|(
),,|(
)(
1
)()()()()()(
)(
1
)()()(
Dieter Fox 19CSE-571: Probabilistic Robotics
Ball-Environment Interaction
Dieter Fox 20CSE-571: Probabilistic Robotics
6
Ball-Environment Interaction
Dieter Fox 21CSE-571: Probabilistic Robotics
Tracking and Finding the Ball
Cluster ball samples by discretizing pan / tilt angles
Uses negative information
Dieter Fox 22CSE-571: Probabilistic Robotics
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 b
all
lost
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
Dieter Fox 23CSE-571: Probabilistic Robotics
ReferenceObservations
Reference* Observations
Simulation Runs
Dieter Fox 24CSE-571: Probabilistic Robotics
7
Comparison to KF* (optimized for straight
motion)
RBPFKF*
ReferenceObservations
RBPFKF*
Reference* Observations
Dieter Fox 25CSE-571: Probabilistic Robotics
RBPFKF’
ReferenceObservations
RBPFKF’
Reference* Observations
Comparison to KF’ (inflated prediction noise)
Dieter Fox 26CSE-571: Probabilistic Robotics
Error vs. Prediction Time
0
10
20
30
40
50
60
70
80
0 0.5 1 1.5 2
RM
S E
rror
[cm
]
Prediction time [sec]
RBPFKF'KF*
Dieter Fox 27CSE-571: Probabilistic Robotics
Orientation Errors
0
20
40
60
80
100
120
140
160
180
2 3 4 5 6 7 8 9 10 11
Orienta
tion E
rror
[degre
es]
Time [sec]
RBPFKF*KF'
Dieter Fox 28CSE-571: Probabilistic Robotics
8
STREET MAPSource: Tiger / Line data
BUS ROUTES / STOPSSource: Metro GIS
Dieter Fox CSE-571: Probabilistic Robotics 29
RESTAURANTS / STORESSource: MS MapPoint Given data stream from a wearable GPS unit
Infer the user’s location and mode of transportation (foot, car, bus, bike, ...)
Predict where user will go
Detect novel behavior / user errors
Dieter Fox CSE-571: Probabilistic Robotics 30
[Liao-Fox-Kautz: AAAI-04, AIJ-07]
Dead and semi-dead zones near buildings, trees, etc.
Sparse measurements inside vehicles, especially bus
Multi-path propagation
Inaccurate street map
…
Dieter Fox CSE-571: Probabilistic Robotics 31
Map is directed graph
Location: Edge e Distance d from start of edge
Prediction: Move along edges according to velocity model
Correction: Update estimate based on GPS reading
Dieter Fox CSE-571: Probabilistic Robotics 32
9
Problem: Predicted location is multi-modal
Dieter Fox CSE-571: Probabilistic Robotics 33
e1
e2
e3
k-1x
e0
Problem: GPS reading is not on the graph
Dieter Fox CSE-571: Probabilistic Robotics 34
e1
e2
e3
zk
kx
Probabilistically “snap” GPS reading to the graph
Perform A* search to compute innovation
Dieter Fox CSE-571: Probabilistic Robotics 35
e1
e2
e3
zk
kx
if =e1
Probabilistically “snap” GPS reading to the graph
Perform A* search to compute innovation
Dieter Fox CSE-571: Probabilistic Robotics 36
e1
e2
e3
zkkx
if =e2
10
Rao-Blackwellised particle filter represents posterior by sets of weighted particles:
Each particle contains Kalman filter for location:
Dieter Fox CSE-571: Probabilistic Robotics 37
},...,1,,{)()(
niwsSii
k
),(,,,2)()()()()( iiiii
Nves
Edge transitions, velocities, edge
associations
Gaussian for position
Dieter Fox CSE-571: Probabilistic Robotics 38
GPS measurements
Particles (Kalman filters)
Encode prior knowledge into the model
Modes have different velocity distributions
Buses run on bus routes
Get on/off the bus near bus stops
Switch to car near car location
Dieter Fox CSE-571: Probabilistic Robotics 39 Dieter Fox CSE-571: Probabilistic Robotics 40
GPS readingzk-1 zk
Edge, velocity, positionxk-1 xk
Time k-1 Time k
mk-1 mk Transportation mode
( ) ( ) ( ) ( )( ) ( ) 2, , , , ( , )
i i i ii is e v NmParticles:
11
Dieter Fox CSE-571: Probabilistic Robotics 41
MeasurementsProjections
Bus modeCar modeFoot mode
GreenRedBlue
Goal (destination):
workplace (home, friends, restaurant, ...)
Trip segments: <start, end, transportation>
Home to Bus stop A on Foot
Bus stop A to Bus stop B on Bus
Bus stop B to workplace on Foot
Dieter Fox CSE-571: Probabilistic Robotics 42
BA Workplace
Dieter Fox CSE-571: Probabilistic Robotics 43
GPS readingzk-1 zk
Edge, velocity, positionxk-1 xk
Time k-1 Time k
mk-1 mk Transportation mode
tk-1 tk Trip segment
gk-1 gk Goal
(( ) ( ) ( ) ( ) () ) ( ) 2, , ,, , , ( , )
i i i i i ii
s m eg v NtParticles:
Key to goal / path prediction and error detection Customized model for each user Unsupervised model learning
Learn variable domains (goals, trip segments)
Learn transition parameters (goals, trips, edges)
Training data
30 days GPS readings of one user, logged every second (when outdoors)
Dieter Fox CSE-571: Probabilistic Robotics 44
12
Dieter Fox CSE-571: Probabilistic Robotics 45
Predicted goal
Predicted path
GOING TO THE WORKPLACE GOING HOME
Dieter Fox CSE-571: Probabilistic Robotics 46
High probability transitions: bus car foot
Dieter Fox CSE-571: Probabilistic Robotics 47
GPS readingzk-1 zk
Edge, velocity, positionxk-1 xk
Time k-1 Time k
mk-1 mk Transportation mode
tk-1 tk Trip segment
gk-1 gk Goal
bk-1 bk Behavior modenormal / unknown / error
[Patterson-Liao-etAl: Ubicomp-04]
Dieter Fox CSE-571: Probabilistic Robotics 48
13
Dieter Fox CSE-571: Probabilistic Robotics 49 Dieter Fox CSE-571: Probabilistic Robotics 50
Discussion
Particle filters are intuitive and simple
Support point-wise thinking (reduced uncertainty)
It’s an art to make them work
Good for test implementation if system behavior is not well known
Inefficient compared to Kalman filter
Rao-Blackwellization
Only sample discrete / highly non-linear parts of state space
Solve remaining part analytically (KF,discrete)Dieter Fox 51CSE-571: Probabilistic Robotics