Literature
AI Robotics
• Kalman Filter:
– Peter Maybeck Stochastic Models, Estimation, and
Control, Volume 1, Academic Press, Inc.
1. Sensing & State Estimation
– Website: http://www.cs.unc.edu/~welch/kalman/
• Markov Localization:
– S. Thrun, W. Burgard, and D. Fox
Probabilistic Robotics. MIT-Press, 2005.
Sensors, State Estimation,
Markov Localization
• State Estimation:
– M. Dietl, J.-S. Gutmann and B. Nebel, Cooperative Sensing
in Dynamic Environments, in: Proceedings of the IEEE/RSJ
International Conference on Intelligent Robots and Systems
(IROS'01), Maui, Hawaii, 2001
Alexander Kleiner
Some slides (mainly all slides on MCL) are based on material from the group “Autonome Intelligenge
Systeme (AIS)” at the University of Freiburg, Germany (Prof. Burgard & Dr. Cyrill Stachniss).
Dr. A. Kleiner
AI Robotics - State Estimation
1
Dr. A. Kleiner
AI Robotics - State Estimation
2
Motivation
Contents
Robots need to be aware of their current
state in order to perform meaningful actions
•
•
•
•
•
Sensors & Sensor Modeling
Modeling Sensor Noise
State Estimation
Monte Carlo Localization (MCL)
Markov Localization as Observation Filter
– Example: Cooperative Ball Recognition
• Summary
Where am I located in the world?
Where are the victims?
Dr. A. Kleiner
AI Robotics - State Estimation
3
Dr. A. Kleiner
AI Robotics - State Estimation
4
Sensors typically used on Robots
•
Wheel Odometry
• Typical setup consists of an incremental
encoder & gear
– Hardware that counts the number of
wheel revolution per second
– Counting of “ticks” via shaft encoders
mounted directly on the wheel axis
– Can be used for estimating the robot
pose (x,y,θ)
• Distance d traveled by the wheel:
Tactile Sensors:
– Analog touch sensors & Bumpers
•
Internal Sensors
– Accelerometers (spring-mounted masses)
– Gyroscopes (spinning mass, laser light)
– Compasses, inclinometers (earth magnetic field, gravity)
•
Proximity Sensors
– Sonar (time of flight)
– Laser Range-Finders (time of flight)
– Infrared (intensity)
•
Visual Sensors
•
Others
Where n is the gear ration, R
the encoder resolution (ticks
per revolution), and D the
diameter of the wheels
– Video & Thermo Cameras
Note: The higher the resolution or the gear ration the less critical
errors are in N or D !
– GPS, Audio, CO2
Dr. A. Kleiner
AI Robotics - State Estimation
5
AI Robotics - State Estimation
AI Robotics - State Estimation
6
Computing the Pose from Odometry
(Dead Reckoning)
Odometry Pose Drift
Dr. A. Kleiner
Dr. A. Kleiner
7
•
Depends on the wheel configuration, e.g.,
Ackerman steering, Mecanum Wheels,
Differential Drive
•
When Δt is very small, distance Δd and angle
Δφ can be computed after a simplified model:
•
Computing the pose update:
Dr. A. Kleiner
AI Robotics - State Estimation
8
Sampling from Our Motion
Sampling from a Gaussian Motion Model
Model
Modeling Noise in Robot Motion
• The Motion Model is represented by a
probabilistic distribution p(xt+1|xt,ut)
• Answers the question: What is the probability
for ending up in state xt+1 when issuing
command ut in state xt?
• Example: Robot receives command to go 1
meter straight ahead and end up at every 10th
time at 1.01m
• Typically represented by a Gaussian
Dr. A. Kleiner
AI Robotics - State Estimation
9
Start
Dr. A. Kleiner
AI Robotics - State Estimation
Inertial Measurement Unit (IMU)
Laser Range Finders (LRFs)
• A closed system for detecting orientation
and motion of a vehicle or human
• Found on many robots
• Highly accurate, high data rate
• Measures distances and angles to
surrounding objects
• Returns distances di and angles
αi, with i ∈ [0…FOV/resolution]
• Typically consists of 3 accelerometer, 3
gyroscopes, and 3 magnetometers
– Data rate @100 Hz
• Gyro reliable only within some time period
(temperature drift)
• Magnetometer data can locally be wrong
Backward
(magnetic perturbation)
• Data from all 3 is fused by a Kalman Filter
inside the IMU sensor
yaw
Fwd.
10
SICK LMS200
Hokuyo URG
roll
pitch
Scan taken on a soccer field
Dr. A. Kleiner
AI Robotics - State Estimation
11
Dr. A. Kleiner
AI Robotics - State Estimation
12
Time of Flight Sensors
Laser Scans
Object
Object
Turning Mirror
v: speed of the signal
t: time elapsed between broadcast of signal and
reception of the echo.
Dr. A. Kleiner
AI Robotics - State Estimation
13
Typical Measurement Errors of
an Range Measurements
Dr. A. Kleiner
AI Robotics - State Estimation
14
Color Cameras
• Sensor that generates color
images, e.g. with 640x480
pixel resolution @30hz
• Opening angle
• Crosstalk
• Specular reflection
Logitech Quickcam
4000 Pro
Sony DFW-500
Image coordinates
World coordinates
• Can be used for object
detection, e.g. soccer ball
– Color thresholding, e.g.
separation of ball colors from
background
– Determination of relative
object location by camera
calibration or interpolation
Dr. A. Kleiner
AI Robotics - State Estimation
15
Dr. A. Kleiner
AI Robotics - State Estimation
16
Modeling Sensor noise
Modeling Sensor noise
• Sensors are represented by a probabilistic
sensor model p(z|x)
• Answers the question: What is the probability
for measuring z when given I am located in
state x?
• Example: Laser Scanner located 1m from the
wall returns in average every 9th time 1.01m
• Typically represented by a Gaussian
• Data from sensors is noisy, .e.g., the distance
measurement of a LRF at one meter can be 1m ± 1cm
• Sensor noise is typically modeled by a normal distribution
• Fully described by mean µ and variance σ2
p( x ) =
€
1 % x−µ (2
*
σ )
− '
1
e 2&
2 πσ 2
p( x ) =
€
AI Robotics - State Estimation
Multivariate
p( x ) ~ N (µ x , Σ2x )
€
Dr. A. Kleiner
Univariate
p( x ) ~ N (µ, σ 2 )
1
n
(2 π ) det ( Σ x )
n-dimensional vector
Dr. A. Kleiner
17
e
1
− ( x−µ x )T Σ −1
x ( x−µ x )
2
n x n matrix
AI Robotics - State Estimation
18
€
Gaussian Filters
!  The Kalman filter and its variants can
only model Gaussian distributions
Gaussians
State Estimation
• Continues Integration of Sensor data according
to probability distributions
• Sensor observations are take in different
coordinate frames, e.g., camera, laser
1D
– Transformation of measurements
• State Estimation is the process of integrating
multiple observations to estimate a state
2
2D
Covariance
Matrix
Download Link:
http://www.informatik.uni-freiburg.de/
~arras/videos/
ErrorEllipsesInAction.mp4
– i.e. robot location, locations of victims
Eigen
values
Correlation
Coefficient
Dr. A. Kleiner
AI Robotics - State Estimation
19
Dr. A. Kleiner
AI Robotics - State Estimation
20
General Framework: Recursive
Bayesian Filtering
Example of Bayesian State Estimation
zt : Sensor observation at time t
xt : State at time t
! Suppose a robot obtains measurement z
! What is P(open|z)?
Bel(xt ) = η P(zt | xt )
€
€
€
Likelihood
(sensor model)
Dr. A. Kleiner
∫ P(xt | ut , xt−1 ) Bel(xt−1 ) dxt−1
Transition
(or motion)
model
Prior
AI Robotics - State Estimation
21
Dr. A. Kleiner
AI Robotics - State Estimation
Causal vs. Diagnostic Reasoning
Example
•
•
•
•
P(z|¬open) = 0.3
• P(z|open) = 0.6
• P(open) = P(¬open) = 0.5
P(open|z) is diagnostic
P(z|open) is causal, i.e., the sensor model
Often causal knowledge is easier to obtain
Bayes rule allows us to use causal knowledge:
22
Marginalization:
compute marginal
probability p(z)
z raises the probability that the door is open
Dr. A. Kleiner
AI Robotics - State Estimation
23
Dr. A. Kleiner
AI Robotics - State Estimation
24
Algorithms for Bayesian Filtering
Kalman Filter for Ball tracking
Kalman Filter: optimal for linear systems and
normal distributions, very efficient, uni-modal
Monte Carlo Localization (Particle Filter):
good for any distribution, can be computationally
expensive, multi-modal
Effect of triangulation
Kalman filtering
Kalman Filtering compared
to Simple Averaging:
Highly Confident Estimates
are more Strongly Weighted
Simple averaging
Dr. A. Kleiner
AI Robotics - State Estimation
25
Dr. A. Kleiner
AI Robotics - State Estimation
26
Better: Using Monte Carlo Localization
(MCL) as Observation Filter
Limits of the Kalman Filter
Problem of false positives (ghost balls)
• The Kalman-Filter can only handle a single
hypotheses
– However, color thresholding on a soccer field
might confuse for example “red t-shirts” with
the ball
– Consequently, Kalman filtering yields poor
results
?
• MCL: Simultaneous tracking of multiple
hypotheses
– Can be used to filter-out hypotheses weakly
supported by observations over time
Player 2 is hallucinating
Dr. A. Kleiner
AI Robotics - State Estimation
27
Dr. A. Kleiner
AI Robotics - State Estimation
28
Monte Carlo Localization (MCL)
Motivation
Markov Localization
Goal:approach
approach
for dealing
with
•!  Goal:
for dealing
with arbitrary
distributions
arbitrary distributions
Dr. A. Kleiner
AI Robotics - State Estimation
29
Dr. A. Kleiner
AI Robotics - State Estimation
30
3
Particle Set
Key Idea: Samples
Key Idea: Samples
Particle Set
Particle
Set
!  Set of weighted
samples
!  Use multiple samples to represent
• Use multiple samples to represent arbitrary
arbitrary
distributions
distributions
Set ofof
weighted
samples
! • Set
weighted
samples
state
hypothesis
State
Hypothesis
state
hypothesis
importance
weight
Importance
Weight
importance
weight
Thesamples
samples
represent
the posterior
•!  The
represent
the posterior
!  The samples represent the posterior
Samples
Dr. A. Kleiner
samples
AI Robotics - State Estimation
31 4
Dr. A. Kleiner
AI Robotics - State Estimation
32
5
5
Importance Sampling Principle
Importance
Sampling
Importance
Sampling
PrinciplePrinciple
Particles
for Approximation
Particles
for Approximation
!  We can use a different distribution g
We can use
a different
g
to ! generate
samples
fromdistribution
f
Particles
function
approximation
• ! Particles
for for
function
approximation
• We can
a different
distribution
to use
generate
samples
from gf to generate
from
! samples
Account
forf the differences between
!
Account
the differences
between
• Account
thefor
“differences
between
g and ffor using
a weight
w = f / gg and f ”
using ga and
weight
= f / ag weight w = f / g
f w
using
!
target
f
• target
f
!  target
f
• ! proposal
g g
proposal
!  proposal
g
• Pre-condition:
!  Pre-condition:
!  Pre-condition:
f(x)>0
! g(x)>0
f(x)>0
! g(x)>0
• ! The
more
particles
fall into
interval,
the
The
more
particles
fallaninto
an interval,
higher its probability density
the higher its probability density
How to obtain such samples?
Dr. A. Kleiner
How to obtain
such
AI Robotics - State
Estimation samples?
77
33
Dr. A. Kleiner
AI Robotics - State Estimation
77
34
6
Particle
FilterFilter
Algorithm
Particle
Algorithm
Particle Filter
Particle Filter Algorithm
1.
the
1.Sample
the particles
using the using
proposal
1. Sample
Sample
theparticles
particles
usingthe
the
distribution
proposal distribution
Recursive Bayes filter
• Non-parametric approach
• Models the distribution by samples
• Prediction: draw from the proposal
• Correction: weighting by the ratio of target
proposal distribution
2.
the
2.Compute
the importance
weights weights
2. Compute
Compute
theimportance
importance
weights
and proposal
3.
unlikely
3.Resampling:
“Replace Replace
unlikely
samples
by
3. Resampling:
Resampling:
Replace
unlikely
moresamples
likely ones”
by more likely ones
The more samples we use, the
better is the estimate!
Dr. A. Kleiner
AI Robotics - State Estimation
samples by more likely ones
35
Dr. A. Kleiner
AI Robotics - State Estimation
36
10
10
Particle
ParticleFilter
Filter Algorithm
Algorithm
Monte
Carlo
Localization
Monte
Carlo
Localization
Monte Carlo Localization
! Each
Each particle
is
a pose
hypothesis
•!
is a is
pose
hypothesis
Eachparticle
particle
a pose
hypothesis
Proposal
is the
motion
• ! Proposal
is the
motion
model model
!  Proposal is the motion model
! Correction
Correction
via
the observation
•!
via via
the observation
model
Correction
the observation
model
model
11
Dr. A. Kleiner
AI Robotics - State Estimation
37
Dr. A. Kleiner
AI Robotics - State Estimation
38
12
12
Particle Filter for Localization (Known
Map)
Particle
Filter
for Localization
Particle
Filter
for Localization
Dr. A. Kleiner
AI Robotics - State Estimation
3913
Dr. A. Kleiner
AI Robotics - State Estimation
40
Resampling
Resampling
Resampling
• Survival of the fittest: “Replace unlikely
samples by more likely ones”
• “Trick” to avoid that many samples cover
unlikely states
• Needed as we have a limited number of
samples
Dr. A. Kleiner
AI Robotics - State Estimation
wn
Wn-1
wn
w1
w2
Wn-1
w3
41
w1
w2
w3
• Roulette wheel
Stochasticuniversal
!  •Stochastic
!  Roulette wheel
universal sampling
• Binary search
sampling
!
Binary
search
•
Low variance
• O(n log n)
!  Low variance
•
O(n)
!  O(n log n)
Dr. A. Kleiner
AI Robotics - State Estimation
42
!  O(n)
16
LowLow
Variance
Resampling
Variance
Resampling
17
Dr. A. Kleiner
AI Robotics - State Estimation
43
Dr. A. Kleiner
AI Robotics - State Estimation
initialization
44
18
Dr. A. Kleiner
AI Robotics - State Estimation
observation
45
Dr. A. Kleiner
AI Robotics - State Estimation
resampling
46
19
Dr. A. Kleiner
motion update
AI Robotics - State Estimation
47
20
Dr. A. Kleiner
21
measurement
AI Robotics - State Estimation
48
22
Dr. A. Kleiner
weight update
AI Robotics - State Estimation
49
Dr. A. Kleiner
AI Robotics - State Estimation
resampling
50
24
23
Dr. A. Kleiner
motion update
AI Robotics - State Estimation
51
Dr. A. Kleiner
25
measurement
AI Robotics - State Estimation
52
26
Dr. A. Kleiner
weight update
AI Robotics - State Estimation
53
Dr. A. Kleiner
AI Robotics - State Estimation
resampling
54
27
Dr. A. Kleiner
motion update
AI Robotics - State Estimation
55
28
Dr. A. Kleiner
29
measurement
AI Robotics - State Estimation
56
30
weight update
Dr. A. Kleiner
AI Robotics - State Estimation
57
Dr. A. Kleiner
AI Robotics - State Estimation
resampling
58
31
Markov Localization as Observation Filter
Markov Localization as Observation Filter
•  Discretization of the soccer field into a 2D (x,y) grid
•  Update step:
•  each cell z is associated with a probability p(z) that the
ball is in this cell
•  Uniform initialization of the grid before any observation is
processed, i.e. p(z)=1/#cells
•  Prediction step:
–  Simple model of ball motion
p( z) ← ∑ p( z | z') p( z') where
p( z | z') denotes the probability that the ball moves to cell z given
it was at z’
€
32
–  When assuming that all kind of motion directions are equally
possible, and velocities
are normally distributed with zero mean
2€
and covariance σ v , p( z | z') can be modeled by a time-depended
Gaussian around z’:
Dr.
Dr.A.A.Kleiner
Kleiner
–  Fusion of new ball observation zb into the grid according
to Bayes’ law:
p( z | z) p( z')
p( z ) ←
b
∑ z ' p(zb | z') p(z')
Normalization: Ensuring
that probabilities sum up
to 1.0
–  The sensor model p( zb | z) determines the likelihood
observing zb given the ball is at position z
•  e.g. less confidence as more far from the camera
€ Markov grid can be used for outlier
•  Finally, the
rejection €
–  The ball is assumed to be located at the highest peak
p( z | z') ~ N ( z', diag(σ v2t, σ v2t ))
€
& Rescue
Robotics
AISearch
Robotics
- State Estimation
€
59 42
Dr. A. Kleiner
Dr. A. Kleiner
AI Robotics - State Estimation
Search & Rescue Robotics
60
43
Phantom Balls: Development of
Probability Distribution
after 1st measurement (1)
after 2nd measurement (2)
Phantom Balls: Development of
Probability Distribution
after 3rd measurement (3)
Consider area with highest peak as possible ball area
Dr. A. Kleiner
AI Robotics - State Estimation
after 4th measurement (1)
after 5th measurement (2)
after 6th measurement (3)
At RoboCup 2000, 938 out of 118388 (0.8%) ball observations were ignored because of
the Markov localization filter.
61
Dr. A. Kleiner
AI Robotics - State Estimation
62
Summary – Particle Filters
Summary – PF Localization
• Particle filters are non-parametric, recursive
Bayes filters
• Posterior is represented by a set of weighted
samples
• Not limited to Gaussians
• Proposal to draw new samples
• Weight to account for the differences between
the proposal and the target
• Work well in low-dimensional spaces
• Particles are propagated according to the
motion model
• They are weighted according to the likelihood
of the observation
• Called: Monte-Carlo localization (MCL)
• MCL is the gold standard for mobile robot
localization today
Dr. A. Kleiner
AI Robotics - State Estimation
63
Dr. A. Kleiner
AI Robotics - State Estimation
64
ROS Nodes
• AMCL
• humanoid_localization
Dr. A. Kleiner
AI Robotics - State Estimation
65