AI Robotics
1. Sensing & State Estimation
Sensors, State Estimation,
Markov Localization
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
Literature
• Kalman Filter:
– Peter Maybeck Stochastic Models, Estimation, and
Control, Volume 1, Academic Press, Inc.
– Website: http://www.cs.unc.edu/~welch/kalman/
• Markov Localization:
– S. Thrun, W. Burgard, and D. Fox
Probabilistic Robotics. MIT-Press, 2005.
• 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
Dr. A. Kleiner
AI Robotics - State Estimation
2
Contents
•
•
•
•
•
Sensors & Sensor Modeling
Modeling Sensor Noise
State Estimation
Monte Carlo Localization (MCL)
Markov Localization as Observation Filter
– Example: Cooperative Ball Recognition
• Summary
Dr. A. Kleiner
AI Robotics - State Estimation
3
Motivation
Robots need to be aware of their current
state in order to perform meaningful actions
Where am I located in the world?
Where are the victims?
Dr. A. Kleiner
AI Robotics - State Estimation
4
Sensors typically used on Robots
•
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
– Video & Thermo Cameras
•
Others
– GPS, Audio, CO2
Dr. A. Kleiner
AI Robotics - State Estimation
5
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:
Where n is the gear ration, R
the encoder resolution (ticks
per revolution), and D the
diameter of the wheels
Note: The higher the resolution or the gear ration the less critical
errors are in N or D !
Dr. A. Kleiner
AI Robotics - State Estimation
6
Odometry Pose Drift
Dr. A. Kleiner
AI Robotics - State Estimation
7
Computing the Pose from Odometry
(Dead Reckoning)
•
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
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
Sampling from Our Motion
Sampling from a Gaussian Motion Model
Model
Start
Dr. A. Kleiner
AI Robotics - State Estimation
10
Inertial Measurement Unit (IMU)
• A closed system for detecting orientation
and motion of a vehicle or human
• 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
Dr. A. Kleiner
AI Robotics - State Estimation
yaw
Fwd.
roll
pitch
11
Laser Range Finders (LRFs)
• 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]
SICK LMS200
Hokuyo URG
Scan taken on a soccer field
Dr. A. Kleiner
AI Robotics - State Estimation
12
Time of Flight Sensors
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
Laser Scans
Dr. A. Kleiner
AI Robotics - State Estimation
14
Typical Measurement Errors of
an Range Measurements
• Opening angle
• Crosstalk
• Specular reflection
Dr. A. Kleiner
AI Robotics - State Estimation
15
Color Cameras
• Sensor that generates color
images, e.g. with 640x480
pixel resolution @30hz
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
16
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
Dr. A. Kleiner
AI Robotics - State Estimation
17
Modeling Sensor noise
• 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
Univariate
p( x ) ~ N (µ, σ 2 )
p( x ) =
€
1 % x−µ (2
*
σ )
− '
1
2&
e
2 πσ 2
Multivariate
p( x ) ~ N (µ x , Σ2x )
€
p( x ) =
€
1
n
(2 π ) det ( Σ x )
n-dimensional vector
Dr. A. Kleiner
€
e
1
− ( x−µ x )T Σ −1
x ( x−µ x )
2
n x n matrix
AI Robotics - State Estimation
18
!  The Kalman filter and its variants can
only model Gaussian distributions
Gaussians
1D
2
2D
Covariance
Matrix
Download Link:
http://www.informatik.uni-freiburg.de/
~arras/videos/
ErrorEllipsesInAction.mp4
Eigen
values
Correlation
Coefficient
Dr. A. Kleiner
AI Robotics - State Estimation
19
State Estimation
• Continues Integration of Sensor data according
to probability distributions
• Sensor observations are take in different
coordinate frames, e.g., camera, laser
– Transformation of measurements
• State Estimation is the process of integrating
multiple observations to estimate a state
– i.e. robot location, locations of victims
Dr. A. Kleiner
AI Robotics - State Estimation
20
€
€
General Framework: Recursive
Bayesian Filtering
zt : Sensor observation at time t
xt : State at time t
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
AI Robotics - State Estimation
Prior
21
Example of Bayesian State Estimation
! Suppose a robot obtains measurement z
! What is P(open|z)?
Dr. A. Kleiner
AI Robotics - State Estimation
22
Causal vs. Diagnostic Reasoning
•
•
•
•
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:
Dr. A. Kleiner
AI Robotics - State Estimation
23
Example
P(z|¬open) = 0.3
• P(z|open) = 0.6
• P(open) = P(¬open) = 0.5
Marginalization:
compute marginal
probability p(z)
z raises the probability that the door is open
Dr. A. Kleiner
AI Robotics - State Estimation
24
Algorithms for Bayesian Filtering
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
Dr. A. Kleiner
AI Robotics - State Estimation
25
Kalman Filter for Ball tracking
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
26
Limits of the Kalman Filter
Problem of false positives (ghost balls)
?
Player 2 is hallucinating
Dr. A. Kleiner
AI Robotics - State Estimation
27
Better: Using Monte Carlo Localization
(MCL) as Observation Filter
• 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
Dr. A. Kleiner
AI Robotics - State Estimation
28
Markov Localization
Dr. A. Kleiner
AI Robotics - State Estimation
29
Monte Carlo Localization (MCL)
Motivation
Goal:approach
approach
for dealing
with
•!  Goal:
for dealing
with arbitrary
distributions
arbitrary distributions
Dr. A. Kleiner
AI Robotics - State Estimation
30
3
Key Idea: Samples
Key Idea: Samples
!  Use multiple samples to represent
• Use multiple samples to represent arbitrary
arbitrary
distributions
distributions
Samples
Dr. A. Kleiner
samples
AI Robotics - State Estimation
31 4
Particle Set
Particle Set
Particle
Set
!  Set of weighted
samples
Set ofof
weighted
samples
! • Set
weighted
samples
state
hypothesis
State
Hypothesis
state
hypothesis
importance
weight
Importance
Weight
importance
weight
!
The
samples
represent
the posterior
• The samples represent the posterior
!  The samples represent the posterior
Dr. A. Kleiner
AI Robotics - State Estimation
32
5
5
Particles
for Approximation
Particles
for Approximation
Particles
function
approximation
• ! Particles
for for
function
approximation
• ! 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?
33
6
Importance Sampling Principle
Importance
Sampling
Importance
Sampling
PrinciplePrinciple
!  We can use a different distribution g
We can use
a different
g
to ! generate
samples
fromdistribution
f
• We can
a different
distribution
to use
generate
samples
from gf to generate
from
! samples
Account
forf the differences between
!  Account
for
the differences
between
• Account
for
the
“differences
between
g and f 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
77
Dr. A. Kleiner
AI Robotics - State Estimation
34
77
Particle Filter
Recursive Bayes filter
• Non-parametric approach
• Models the distribution by samples
• Prediction: draw from the proposal
• Correction: weighting by the ratio of target
and proposal
The more samples we use, the
better is the estimate!
Dr. A. Kleiner
AI Robotics - State Estimation
35
Particle
FilterFilter
Algorithm
Particle
Algorithm
Particle Filter Algorithm
1.
Sample
the
particles
using
the
1.Sample
the
particles
using
the
proposal
1.  Sample the particles using the
distribution
proposal distribution
proposal distribution
2.
the
2.Compute
the importance
weights weights
2. Compute
Compute
theimportance
importance
weights
3.
Resampling:
Replace
unlikely
3.Resampling:
“Replace
unlikely
samples
by
3.  Resampling: Replace unlikely
moresamples
likely ones”
by more likely ones
samples by more likely ones
Dr. A. Kleiner
AI Robotics - State Estimation
36
10
10
Particle
ParticleFilter
Filter Algorithm
Algorithm
11
Dr. A. Kleiner
AI Robotics - State Estimation
37
Monte
Carlo
Localization
Monte
Carlo
Localization
Monte Carlo Localization
! Each
Each
particle
is
a pose
hypothesis
•!
particle
is
a
pose
hypothesis
Each particle is a pose hypothesis
Proposal
is the
motion
• ! Proposal
is the
motion
model model
!  Proposal is the motion model
! Correction
Correction
via
the observation
•!
via
the
observation
model
Correction via the observation
Dr. A. Kleiner
AI Robotics - State Estimation
model
model
38
12
12
Particle
Filter
for Localization
Particle
Filter
for Localization
Dr. A. Kleiner
AI Robotics - State Estimation
3913
Particle Filter for Localization (Known
Map)
Dr. A. Kleiner
AI Robotics - State Estimation
40
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
41
Resampling
Resampling
wn
Wn-1
wn
w1
w2
Wn-1
w3
w1
w2
w3
• Roulette wheel
•Stochastic
Stochasticuniversal
!
!  Roulette wheel
universal sampling
• Binary search
sampling
Binary
• Low variance
•!  O(n
log search
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
19
Dr. A. Kleiner
AI Robotics - State Estimation
resampling
46
20
Dr. A. Kleiner
motion update
AI Robotics - State Estimation
47
21
Dr. A. Kleiner
measurement
AI Robotics - State Estimation
48
22
Dr. A. Kleiner
weight update
AI Robotics - State Estimation
49
23
Dr. A. Kleiner
AI Robotics - State Estimation
resampling
50
24
Dr. A. Kleiner
motion update
AI Robotics - State Estimation
51
25
Dr. A. Kleiner
measurement
AI Robotics - State Estimation
52
26
Dr. A. Kleiner
weight update
AI Robotics - State Estimation
53
27
Dr. A. Kleiner
AI Robotics - State Estimation
resampling
54
28
Dr. A. Kleiner
motion update
AI Robotics - State Estimation
55
29
Dr. A. Kleiner
measurement
AI Robotics - State Estimation
56
30
Dr. A. Kleiner
weight update
AI Robotics - State Estimation
57
31
Dr. A. Kleiner
AI Robotics - State Estimation
resampling
58
32
Markov Localization as Observation Filter
•  Discretization of the soccer field into a 2D (x,y) grid
•  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’
€
–  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
€
p( z | z') ~ N ( z', diag(σ v2t, σ v2t ))
& Rescue
Robotics
AISearch
Robotics
- State Estimation
59 42
Markov Localization as Observation Filter
•  Update step:
–  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
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)
after 3rd measurement (3)
Consider area with highest peak as possible ball area
Dr. A. Kleiner
AI Robotics - State Estimation
61
Phantom Balls: Development of
Probability Distribution
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.
Dr. A. Kleiner
AI Robotics - State Estimation
62
Summary – Particle Filters
• 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
Dr. A. Kleiner
AI Robotics - State Estimation
63
Summary – PF Localization
• 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
64
ROS Nodes
• AMCL
• humanoid_localization
Dr. A. Kleiner
AI Robotics - State Estimation
65