introduction to mobile robotics - george mason universitykosecka/cs685/cs685-ekf-features.pdf ·...
TRANSCRIPT
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
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
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.
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 Σ−=Σ )(
5
The Prediction-Correction-Cycle
⎩⎨⎧
+Σ=Σ
+==
−
−
tTtttt
tttttt RAA
uBAxbel
1
1)(µµ
⎩⎨⎧
+=
+== −
2,
2221)(
tactttt
tttttt a
ubaxbel
σσσ
µµ
Prediction
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
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
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 =
9
First-Order Error Propagation
Approximating f(X) by a first-order Taylor series expansion about the point X = µX
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
11
First-Order Error Propagation
X,Y assumed to be Gaussian
Y = f(X)
Taylor series expansion
Wanted: , (Solution on blackboard)
12
Y = f(X1, X2, ..., Xn)
Taylor series expansion (around mean)
Wanted: , (Solution on blackboard)
First-Order Error Propagation
13
First-Order Error Propagation
Y = f(X1, X2, ..., Xn)
Z = g(X1, X2, ..., Xn)
Wanted:
(Exercise)
14
First-Order Error Propagation
Putting things together...
with
“Is there a compact form?...”
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
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
17
First-Order Error Propagation
Putting things together...
with
“Is there a compact form?...”
18
First-Order Error Propagation
...Yes! Given
• Input covariance matrix CX
• Jacobian matrix FX
the Error Propagation Law
computes the output covariance matrix CY
19
First-Order Error Propagation
Alternative Derivation in Matrix Notation
20
Derivations (2/4)
Definitions
Rules
Result SISO
21
Derivations (3/4)
Result MISO
22
Derivations (4/4)
Result MIMO
23
Feature Extraction: Motivation
Landmarks for:
• Localization
• SLAM
• Scene analysis
Examples:
• Lines, corners, clusters: good for indoor
• Circles, rocks, plants: good for outdoor
Wanted: Parameter Covariance Matrix
Simplified sensor model: all , independence
Result: Gaussians in the model space
24
Example: Line Extraction
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
26
Feature Extraction
Can be subdivided into two subproblems:
• Segmentation: Which points contribute?
• Fitting: How do the points contribute?
Segmentation Fitting
27
Example: Local Map with Lines
Raw range data
Line segments
28
Example: Global Map with Lines
Expo.02 map • 315 m2 • 44 Segments • 8 kbytes • 26 bytes / m2 • Localization accuracy ~1cm
29
Example: Global Map w. Circles
Victoria Park, Sydney • Trees
30
Split and Merge
Picture by J. Tardos
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
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
33
Split and Merge: Improvements
• Merge non-consecutive segments as a post-processing step
34
Line Representation
Choice of the line representation matters!
Intercept-Slope Hessian model
Each model has advantages and drawbacks
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
Problem with “vertical” least squares
• Not rotation-invariant • Fails completely for vertical lines
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)
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)
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)
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
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 )
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
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
!
"#
$
%&
44
Line fitting
Given:
A set of n points in polar coordinates
Wanted:
Line parameters ,
[Arras 1997]
45
LSQ Estimation
Regression, Least Squares-Fitting
Solve the non-linear equation system
Solution (for points in Cartesian coordinates): → Solution on blackboard
46
Can be formulated as a linear regression problem
Circle Extraction
Develop circle equation
Parametrization trick
47
Circle Extraction
Solution via Pseudo-Inverse
Leads to overdetermined equation system
with vector of unknowns
(assuming that A has full rank)
48
Fitting Curves to Points
Attention: Always know the errors that you minimize!
Algebraic versus geometric fit solutions [Gander 1994]
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
50
Example: Line Extraction
Wanted: Parameter Covariance Matrix
Simplified sensor model: all , independence
Result: Gaussians in the parameter space
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
52
Derivations (1/4)
Result: Line Fit Cartesian Coordinates
(only for r, α more complicated...)
EKF Localization
Introduction to Mobile Robotics
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]
Landmark-based Localization
EKF Localization: Basic Cycle
55
Prediction Update
State Prediction Update
Map
Data Association
Odometry or IMU
Sensors
Feature/Landmark Extraction
Measurement Prediction
Landmark-based Localization
EKF Localization: Basic Cycle
56
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
State Prediction (Odometry)
Landmark-based Localization
58
Control uk: wheel displacements sl , sr
Error model: linear growth
Nonlinear process model f :
State Prediction (Odometry)
Landmark-based Localization
59
Control uk: wheel displacements sl , sr
Error model: linear growth
Nonlinear process model f :
Landmark-based Localization
Landmark Extraction (Observation)
60
Extracted lines
Hessian line model
Extracted lines in model space
Raw laser range data
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
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
Landmark-based Localization
Update
• Kalman gain
• State update (robot pose)
• State covariance update
63
Red: posterior estimate
• EKF Localization with Point Features
Landmark-based Localization
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
EKF Prediction Step for different noise levels left previous pose, right predicted pose
BtQBTt BtQBT
t
BtQBTt BtQBT
t
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
EKF Observation Prediction Step left column predictions – white circle ground truth right measurement – predicted and observed
EKF Correction Step left: given predicted and observed measurement, right corrected pose
Estimation Sequence (1) solid line: ground truth, dashed line: odometry without sensing (left), with sensing (right)
Estimation Sequence (2) solid line: ground truth, dashed line: odometry without sensing (left), with sensing (right)
Comparison to GroundTruth solid line: ground truth, dashed line: odometry with sensor with different
• [Arras et al. 98]:
• Laser range-finder and vision
• High precision (<1cm accuracy)
Courtesy of K. Arras
EKF Localization Example
EKF Localization Example
• Line and point landmarks
EKF Localization Example
• Line and point landmarks
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
EKF Localization Example
78
Global EKF Localization
Interpretation tree
79
Global EKF Localization
Env. Dynamics
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 α
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
82
Global EKF Localization
Pygmalion
α = 0.95 , p = 2
83
Global EKF Localization
α = 0.95 , p = 3
Pygmalion
84
Global EKF Localization
α = 0.95 , p = 4 texe: 633 ms PowerPC at
300 MHz
Pygmalion
85
Global EKF Localization
α = 0.95 , p = 5
texe: 633 ms (PowerPC at 300 MHz)
Pygmalion
05.07.02, 17.23 h
Global EKF Localization
α = 0.999
At Expo.02
[Arras et al. 03]
texe = 105 ms
05.07.02, 17.23 h
Global EKF Localization
α = 0.999
At Expo.02
[Arras et al. 03]
05.07.02, 17.32 h
Global EKF Localization
α = 0.999
At Expo.02
[Arras et al. 03]
05.07.02, 17.32 h
Global EKF Localization
α = 0.999 texe = 446 ms
At Expo.02
[Arras et al. 03]
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