Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf ·...
Transcript of Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf ·...
![Page 1: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/1.jpg)
1
Error Propagation, Feature Exatraction, Extended Kalman Filter
Introduction to Mobile Robotics
Some slides adopted from: Wolfram Burgard, Cyrill Stachniss,
Maren Bennewitz, Kai Arras and Probabilistic Robotics Book
![Page 2: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/2.jpg)
2
• Probabilistic robotics is • Representation • Propagation • Reduction of uncertainty
• First-order error propagation is fundamental for: Kalman filter (KF), landmark extraction, KF-based localization and SLAM
Error Propagation: Motivation
![Page 3: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/3.jpg)
3
Discrete Kalman Filter (review)
tttttt uBxAx ε++= −1
tttt xCz δ+=
• Estimates the state x of a discrete-time controlled process that is governed by the linear stochastic difference equation
• with a measurement
tε
Matrix (nxn) that describes how the state evolves from t to t-1 without controls or noise. tA
Matrix (nxl) that describes how the control ut changes the state from t to t-1. tB
Matrix (kxn) that describes how to map the state xt to an observation zt.
tC
tδ
Random variables representing the process and measurement noise that are assumed to be independent
and normally distributed with covariance Rt and Qt respectively.
![Page 4: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/4.jpg)
4
Kalman Filter Algorithm
1. Algorithm Kalman_filter( µt-1, Σt-1, ut, zt):
2. Prediction: 3. 4.
5. Correction: 6. 7. 8.
9. Return µt, Σt
ttttt uBA += −1µµ
tTtttt RAA +Σ=Σ −1
1)( −+ΣΣ= tTttt
Tttt QCCCK
)( tttttt CzK µµµ −+=
tttt CKI Σ−=Σ )(
![Page 5: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/5.jpg)
5
The Prediction-Correction-Cycle
⎩⎨⎧
+Σ=Σ
+==
−
−
tTtttt
tttttt RAA
uBAxbel
1
1)(µµ
⎩⎨⎧
+=
+== −
2,
2221)(
tactttt
tttttt a
ubaxbel
σσσ
µµ
Prediction
![Page 6: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/6.jpg)
6
The Prediction-Correction-Cycle
1)(,)(
)()( −+ΣΣ=
⎩⎨⎧
Σ−=Σ
−+== t
Tttt
Tttt
tttt
ttttttt QCCCK
CKICzK
xbelµµµ
2,
2
2
22 ,)1(
)()(
tobst
tt
ttt
tttttt K
KzK
xbelσσ
σσσ
µµµ
+=
⎩⎨⎧
−=
−+==
Correction
![Page 7: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/7.jpg)
Gaussian Distribution
Why is the Gaussian distribution everywhere? The importance of the normal distribution follows mainly from the Central Limit Theorem: • The mean/sum of a large number of independent RVs, each with finite mean and variance (ergo not e.g. uniformally distributed RVs), will be approximately normally distributed.
• The more RVs the better the approximation.
7
![Page 8: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/8.jpg)
8
Nonlinear Dynamic Systems
• Most realistic robotic problems involve
nonlinear functions
• Extended Kalman filter relaxes linearity assumption
• To be continued
),( 1−= ttt xugx
)( tt xhz =
![Page 9: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/9.jpg)
9
First-Order Error Propagation
Approximating f(X) by a first-order Taylor series expansion about the point X = µX
![Page 10: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/10.jpg)
10
• Second-Order Error Propagation
Rarely used (complex expressions)
• Monte-Carlo
Non-parametric representation of uncertainties
1. Sampling from p(X)
2. Propagation of samples
3. Histogramming
4. Normalization
Other Error Prop. Techniques
![Page 11: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/11.jpg)
11
First-Order Error Propagation
X,Y assumed to be Gaussian
Y = f(X)
Taylor series expansion
Wanted: , (Solution on blackboard)
![Page 12: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/12.jpg)
12
Y = f(X1, X2, ..., Xn)
Taylor series expansion (around mean)
Wanted: , (Solution on blackboard)
First-Order Error Propagation
![Page 13: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/13.jpg)
13
First-Order Error Propagation
Y = f(X1, X2, ..., Xn)
Z = g(X1, X2, ..., Xn)
Wanted:
(Exercise)
![Page 14: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/14.jpg)
14
First-Order Error Propagation
Putting things together...
with
“Is there a compact form?...”
![Page 15: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/15.jpg)
15
Jacobian Matrix
• It’s a non-square matrix in general
• Suppose you have a vector-valued function
• Let the gradient operator be the vector of (first-order) partial derivatives
• Then, the Jacobian matrix is defined as
![Page 16: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/16.jpg)
16
• It’s the orientation of the tangent plane to the vector-valued function at a given point
• Generalizes the gradient of a scalar valued function
• Heavily used for first-order error propagation...
Jacobian Matrix
![Page 17: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/17.jpg)
17
First-Order Error Propagation
Putting things together...
with
“Is there a compact form?...”
![Page 18: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/18.jpg)
18
First-Order Error Propagation
...Yes! Given
• Input covariance matrix CX
• Jacobian matrix FX
the Error Propagation Law
computes the output covariance matrix CY
![Page 19: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/19.jpg)
19
First-Order Error Propagation
Alternative Derivation in Matrix Notation
![Page 20: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/20.jpg)
20
Derivations (2/4)
Definitions
Rules
Result SISO
![Page 21: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/21.jpg)
21
Derivations (3/4)
Result MISO
![Page 22: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/22.jpg)
22
Derivations (4/4)
Result MIMO
![Page 23: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/23.jpg)
23
Feature Extraction: Motivation
Landmarks for:
• Localization
• SLAM
• Scene analysis
Examples:
• Lines, corners, clusters: good for indoor
• Circles, rocks, plants: good for outdoor
![Page 24: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/24.jpg)
Wanted: Parameter Covariance Matrix
Simplified sensor model: all , independence
Result: Gaussians in the model space
24
Example: Line Extraction
![Page 25: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/25.jpg)
Features: Properties
A feature/landmark is a physical object which is • static • perceptible • (at least locally) unique Abstraction from the raw data... • type (range, image, vibration, etc.) • amount (sparse or dense) • origin (different sensors, map) + Compact, efficient, accurate, scales well, semantics − Not general
25
![Page 26: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/26.jpg)
26
Feature Extraction
Can be subdivided into two subproblems:
• Segmentation: Which points contribute?
• Fitting: How do the points contribute?
Segmentation Fitting
![Page 27: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/27.jpg)
27
Example: Local Map with Lines
Raw range data
Line segments
![Page 28: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/28.jpg)
28
Example: Global Map with Lines
Expo.02 map • 315 m2 • 44 Segments • 8 kbytes • 26 bytes / m2 • Localization accuracy ~1cm
![Page 29: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/29.jpg)
29
Example: Global Map w. Circles
Victoria Park, Sydney • Trees
![Page 30: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/30.jpg)
30
Split and Merge
Picture by J. Tardos
![Page 31: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/31.jpg)
31
Split and Merge
Algorithm
Split
• Obtain the line passing by the two extreme points
• Find the most distant point to the line
• If distance > threshold, split and repeat with the left and right point sets
Merge
• If two consecutive segments are close/collinear enough, obtain the common line and find the most distant point
• If distance <= threshold, merge both segments
![Page 32: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/32.jpg)
32
Split and Merge: Improvements
• Residual analysis before split
Split only if the break point provides a "better interpretation" in terms of the error sum
[Castellanos 1998]
: start-, end-, break-point
![Page 33: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/33.jpg)
33
Split and Merge: Improvements
• Merge non-consecutive segments as a post-processing step
![Page 34: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/34.jpg)
34
Line Representation
Choice of the line representation matters!
Intercept-Slope Hessian model
Each model has advantages and drawbacks
![Page 35: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/35.jpg)
Least squares line fitting • Data: (x1, y1), …, (xn, yn) • Line equation: yi = m xi + b • Find (m, b) to minimize
dEdB
= 2XTXB− 2XTY = 0
Y =y1yn
!
"
####
$
%
&&&&
X =x1 1 xn 1
!
"
####
$
%
&&&&
B = mb
!
"#
$
%&
E = Y − XB 2= (Y − XB)T (Y − XB) =Y TY − 2(XB)TY + (XB)T (XB)
Normal equations: least squares solution to XB=Y
E = (yi −mxi − b)2
i=1
n∑
(xi, yi)
y=mx+b
XTXB = XTY
![Page 36: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/36.jpg)
Problem with “vertical” least squares
• Not rotation-invariant • Fails completely for vertical lines
![Page 37: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/37.jpg)
Total least squares • Distance between point (xi, yi) and line ax+by=d (a2+b2=1): |axi + byi – d|
∑= −+=n
i ii dybxaE12)((xi, yi)
ax+by=d Unit normal:
N=(a, b)
![Page 38: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/38.jpg)
Total least squares • Distance between point (xi, yi) and line ax+by=d (a2+b2=1): |axi + byi – d| • Find (a, b, d) to minimize the sum of squared perpendicular distances
∑= −+=n
i ii dybxaE12)((xi, yi)
ax+by=d
E = (axi + byi − d)2
i=1
n∑
Unit normal: N=(a, b)
![Page 39: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/39.jpg)
Total least squares • Distance between point (xi, yi) and line ax+by=d (a2+b2=1): |axi + byi – d| • F ind (a, b, d) to minimize the sum of squared perpendicular distances
∑= −+=n
i ii dybxaE12)((xi, yi)
ax+by=d
E = (axi + byi − d)2
i=1
n∑
Unit normal: N=(a, b)
∂E∂d
= −2(axi + byi − d)i=1
n∑ = 0
d = an
xi +bni=1
n∑ yii=1
n∑ = ax + by
E = (a(xi − x )+ b(yi − y ))2
i=1
n∑ =
x1 − x y1 − y
xn − x yn − y
#
$
%%%%
&
'
((((
ab
#
$%
&
'(
2
= (UN )T (UN )dEdN
= 2(UTU)N = 0
Solution to (UTU)N = 0, subject to ||N||2 = 1: eigenvector of UTU associated with the smallest eigenvalue (least squares solution
to homogeneous linear system UN = 0)
![Page 40: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/40.jpg)
Total least squares
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
−−
−−
=
yyxx
yyxxU
nn
11
UTU =
(xi − x )2
i=1
n
∑ (xi − x )(yi − y )i=1
n
∑
(xi − x )(yi − y )i=1
n
∑ (yi − y )2
i=1
n
∑
#
$
%%%%%
&
'
(((((
second moment matrix
![Page 41: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/41.jpg)
Total least squares
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
−−
−−
=
yyxx
yyxxU
nn
11
UTU =
(xi − x )2
i=1
n
∑ (xi − x )(yi − y )i=1
n
∑
(xi − x )(yi − y )i=1
n
∑ (yi − y )2
i=1
n
∑
#
$
%%%%%
&
'
(((((
),( yx
N = (a, b)
second moment matrix
(xi − x, yi − y )
![Page 42: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/42.jpg)
Least squares as likelihood maximization • Generative model: line points
are sampled independently and corrupted by Gaussian noise in the direction perpendicular to the line
xy
!
"##
$
%&&=
uv
!
"#
$
%&+ε a
b
!
"#
$
%&
(x, y)
ax+by=d
(u, v) ε
point on the
line noise:
sampled from zero-mean
Gaussian with std. dev. σ
normal direction
![Page 43: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/43.jpg)
Least squares as likelihood maximization
P(x1, y1,…, xn, yn | a,b,d) = P(xi, yi | a,b,d)i=1
n
∏ ∝ exp −(axi + byi − d)
2
2σ 2
$
%&
'
()
i=1
n
∏
Likelihood of points given line parameters (a, b, d):
L(x1, y1,…, xn, yn | a,b,d) = −12σ 2 (axi + byi − d)
2
i=1
n
∑Log-likelihood:
(x, y)
ax+by=d
(u, v) ε
• Generative model: line points are sampled independently and corrupted by Gaussian noise in the direction perpendicular to the line
xy
!
"##
$
%&&=
uv
!
"#
$
%&+ε a
b
!
"#
$
%&
![Page 44: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/44.jpg)
44
Line fitting
Given:
A set of n points in polar coordinates
Wanted:
Line parameters ,
[Arras 1997]
![Page 45: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/45.jpg)
45
LSQ Estimation
Regression, Least Squares-Fitting
Solve the non-linear equation system
Solution (for points in Cartesian coordinates): → Solution on blackboard
![Page 46: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/46.jpg)
46
Can be formulated as a linear regression problem
Circle Extraction
Develop circle equation
Parametrization trick
![Page 47: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/47.jpg)
47
Circle Extraction
Solution via Pseudo-Inverse
Leads to overdetermined equation system
with vector of unknowns
(assuming that A has full rank)
![Page 48: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/48.jpg)
48
Fitting Curves to Points
Attention: Always know the errors that you minimize!
Algebraic versus geometric fit solutions [Gander 1994]
![Page 49: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/49.jpg)
49
LSQ Estimation: Uncertainties?
How does the input uncertainty propagate over the fit expressions to the output?
X1, ..., Xn : Gaussian input random variables
A, R : Gaussian output random variables
![Page 50: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/50.jpg)
50
Example: Line Extraction
Wanted: Parameter Covariance Matrix
Simplified sensor model: all , independence
Result: Gaussians in the parameter space
![Page 51: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/51.jpg)
51
Line Extraction in Real Time
• Robot Pygmalion EPFL, Lausanne
• CPU: PowerPC 604e at 300 MHz Sensor: 2 SICK LMS
• Line Extraction Times: ~ 25 ms
![Page 52: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/52.jpg)
52
Derivations (1/4)
Result: Line Fit Cartesian Coordinates
(only for r, α more complicated...)
![Page 53: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/53.jpg)
EKF Localization
Introduction to Mobile Robotics
![Page 54: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/54.jpg)
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]
![Page 55: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/55.jpg)
Landmark-based Localization
EKF Localization: Basic Cycle
55
Prediction Update
![Page 56: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/56.jpg)
State Prediction Update
Map
Data Association
Odometry or IMU
Sensors
Feature/Landmark Extraction
Measurement Prediction
Landmark-based Localization
EKF Localization: Basic Cycle
56
![Page 57: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/57.jpg)
State Prediction Update
Map
Data Association
Odometry or IMU
Sensors
Feature/Landmark Extraction
Measurement Prediction
Landmark-based Localization
EKF Localization: Basic Cycle
57
raw sensory data
landmarks
innovation from matched landmarks
predicted measurements in
sensor coordinates
landmarks in global coordinates
encoder measurements
predicted state
posterior state
![Page 58: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/58.jpg)
State Prediction (Odometry)
Landmark-based Localization
58
Control uk: wheel displacements sl , sr
Error model: linear growth
Nonlinear process model f :
![Page 59: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/59.jpg)
State Prediction (Odometry)
Landmark-based Localization
59
Control uk: wheel displacements sl , sr
Error model: linear growth
Nonlinear process model f :
![Page 60: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/60.jpg)
Landmark-based Localization
Landmark Extraction (Observation)
60
Extracted lines
Hessian line model
Extracted lines in model space
Raw laser range data
![Page 61: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/61.jpg)
Landmark-based Localization
Measurement Prediction
• ...is a coordinate frame transform world-to-sensor
• Given the predicted state (robot pose), predicts the location and location uncertainty of expected observations in sensor coordinates
61
model space
![Page 62: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/62.jpg)
Data Association (Matching)
• Associates predicted measurements with observations
• Innovation and innovation covariance
• Matching on significance level alpha
Landmark-based Localization
62
model space
Green: observation
Magenta: measurement prediction
![Page 63: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/63.jpg)
Landmark-based Localization
Update
• Kalman gain
• State update (robot pose)
• State covariance update
63
Red: posterior estimate
![Page 64: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/64.jpg)
• EKF Localization with Point Features
Landmark-based Localization
![Page 65: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/65.jpg)
1. EKF_localization ( µt-1, Σt-1, ut, zt, m): Prediction:
2.
3.
4.
5.
6.
),( 1−= ttt ug µµ
€
Σt =GtΣt−1GtT + BtQtBt
T
⎟⎟⎟⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜⎜⎜⎜
⎝
⎛
∂
∂
∂
∂
∂
∂∂
∂
∂
∂
∂
∂∂
∂
∂
∂
∂
∂
=∂
∂=
−−−
−−−
−−−
−
−
θ
θ
θ
µθ
µθ
µθ
µµµ
µµµµ
,1,1,1
,1,1,1
,1,1,1
1
1
'''
'''
'''
),(
tytxt
tytxt
tytxt
t
ttt
yyy
xxx
xugG
€
Bt =∂g(ut ,µt−1)
∂ut=
∂x'∂vt
∂x'∂ω t
∂y'∂vt
∂y'∂ω t
∂θ'∂vt
∂θ '∂ω t
&
'
( ( ( ( ( (
)
*
+ + + + + +
€
Qt =α1 | vt |+α2 |ω t |( )2 0
0 α3 | vt |+α4 |ω t |( )2$
% & &
'
( ) )
Motion noise
Jacobian of g w.r.t location
Predicted mean
Predicted covariance
Jacobian of g w.r.t control
![Page 66: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/66.jpg)
EKF Prediction Step for different noise levels left previous pose, right predicted pose
BtQBTt BtQBT
t
BtQBTt BtQBT
t
![Page 67: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/67.jpg)
1. EKF_localization ( µt-1, Σt-1, ut, zt, m): Correction:
2.
3.
4.
5.
6.
7.
8.
)ˆ( ttttt zzK −+= µµ
( ) tttt HKI Σ−=Σ
⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜
⎝
⎛
∂
∂∂
∂
∂
∂∂
∂
∂
∂∂
∂
=∂
∂=
θ
θ
µϕµ
µϕµ
µϕµµ
,
,
,
,
,
,),(
t
t
t
t
yt
t
yt
t
xt
t
xt
t
t
tt
rrr
xmhH
( ) ( )( ) ⎟
⎟
⎠
⎞
⎜⎜
⎝
⎛
−−−
−+−=
θµµµ
µµ
,,,
2,
2,
,2atanˆ
txtxyty
ytyxtxt mm
mmz
€
St = HtΣ tHtT + Rt
1−Σ= tTttt SHK€
Rt =σ r2 00 σ r
2
#
$ %
&
' (
Predicted measurement mean
Innovation covariance
Kalman gain
Updated mean
Updated covariance
Jacobian of h w.r.t location
![Page 68: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/68.jpg)
EKF Observation Prediction Step left column predictions – white circle ground truth right measurement – predicted and observed
![Page 69: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/69.jpg)
EKF Correction Step left: given predicted and observed measurement, right corrected pose
![Page 70: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/70.jpg)
Estimation Sequence (1) solid line: ground truth, dashed line: odometry without sensing (left), with sensing (right)
![Page 71: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/71.jpg)
Estimation Sequence (2) solid line: ground truth, dashed line: odometry without sensing (left), with sensing (right)
![Page 72: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/72.jpg)
Comparison to GroundTruth solid line: ground truth, dashed line: odometry with sensor with different
![Page 73: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/73.jpg)
• [Arras et al. 98]:
• Laser range-finder and vision
• High precision (<1cm accuracy)
Courtesy of K. Arras
EKF Localization Example
![Page 74: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/74.jpg)
EKF Localization Example
• Line and point landmarks
![Page 75: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/75.jpg)
EKF Localization Example
• Line and point landmarks
![Page 76: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/76.jpg)
EKF Localization Example
• Expo.02: Swiss National Exhibition 2002 • Pavilion "Robotics" • 11 fully autonomous robots • tour guides, entertainer, photographer • 12 hours per day • 7 days per week • 5 months
• 3,316 km travel distance • almost 700,000 visitors • 400 visitors per hour
• Localization method: Line-Based EKF
![Page 77: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/77.jpg)
EKF Localization Example
![Page 78: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/78.jpg)
78
Global EKF Localization
Interpretation tree
![Page 79: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/79.jpg)
79
Global EKF Localization
Env. Dynamics
![Page 80: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/80.jpg)
80
Global EKF Localization Geometric constraints we can exploit
Location independent constraints
Unary constraint: intrinsic property of feature
e.g. type, color, size
Binary constraint: relative measure between features
e.g. relative position, angle
Location dependent constraints
Rigidity constraint: "is the feature where I expect it given
my position?"
Visibility constraint: "is the feature visible from my position?"
Extension constraint: "do the features overlap at my position?"
All decisions on a significance level α
![Page 81: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/81.jpg)
81
Global EKF Localization
Interpretation Tree [Grimson 1987], [Drumheller 1987], [Castellanos 1996], [Lim 2000]
Algorithm
• backtracking
• depth-first
• recursive
• uses geometric constraints
• worst-case exponential complexity
![Page 82: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/82.jpg)
82
Global EKF Localization
Pygmalion
α = 0.95 , p = 2
![Page 83: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/83.jpg)
83
Global EKF Localization
α = 0.95 , p = 3
Pygmalion
![Page 84: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/84.jpg)
84
Global EKF Localization
α = 0.95 , p = 4 texe: 633 ms PowerPC at
300 MHz
Pygmalion
![Page 85: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/85.jpg)
85
Global EKF Localization
α = 0.95 , p = 5
texe: 633 ms (PowerPC at 300 MHz)
Pygmalion
![Page 86: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/86.jpg)
05.07.02, 17.23 h
Global EKF Localization
α = 0.999
At Expo.02
[Arras et al. 03]
![Page 87: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/87.jpg)
texe = 105 ms
05.07.02, 17.23 h
Global EKF Localization
α = 0.999
At Expo.02
[Arras et al. 03]
![Page 88: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/88.jpg)
05.07.02, 17.32 h
Global EKF Localization
α = 0.999
At Expo.02
[Arras et al. 03]
![Page 89: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/89.jpg)
05.07.02, 17.32 h
Global EKF Localization
α = 0.999 texe = 446 ms
At Expo.02
[Arras et al. 03]
![Page 90: Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf · 2014-11-19 · Introduction to Mobile Robotics Some slides adopted from: Wolfram Burgard,](https://reader030.fdocuments.us/reader030/viewer/2022040817/5e618fd8ea4b7e5cdb05f20d/html5/thumbnails/90.jpg)
EKF Localization Summary
• EKF localization implements pose tracking
• Very efficient and accurate (positioning error down to subcentimeter)
• Filter divergence can cause lost situations from which the EKF cannot recover
• Industrial applications
• Global EKF localization can be achieved using interpretation tree-based data association
• Worst-case complexity is exponential
• Fast in practice for small maps