Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf ·...

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


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)


13


Y = f(X1, X2, ..., Xn)

Z = g(X1, X2, ..., Xn)

Wanted:

(Exercise)

14


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


Putting things together...

with

“Is there a compact form?...”

18


...Yes! Given

•  Input covariance matrix CX

•  Jacobian matrix FX

the Error Propagation Law

computes the output covariance matrix CY

19


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


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


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



56

State Prediction Update

Map

Data Association

Odometry or IMU

Sensors

Feature/Landmark Extraction




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)


58

Control uk: wheel displacements sl , sr

Error model: linear growth

Nonlinear process model f :

State Prediction (Odometry)


59

Control uk: wheel displacements sl , sr

Error model: linear growth

Nonlinear process model f :


Landmark Extraction (Observation)

60

Extracted lines

Hessian line model

Extracted lines in model space

Raw laser range data



•  ...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


62

model space

Green: observation

Magenta: measurement prediction


Update

•  Kalman gain

•  State update (robot pose)

•  State covariance update

63

Red: posterior estimate

•  EKF Localization with Point Features


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


• Line and point landmarks


•  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

78

Global EKF Localization

Interpretation tree

79


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


Interpretation Tree [Grimson 1987], [Drumheller 1987], [Castellanos 1996], [Lim 2000]

Algorithm

• backtracking

• depth-first

• recursive

• uses geometric constraints

• worst-case exponential complexity

82


Pygmalion

α = 0.95 , p = 2

83


α = 0.95 , p = 3

Pygmalion

84


α = 0.95 , p = 4 texe: 633 ms PowerPC at

300 MHz

Pygmalion

85


α = 0.95 , p = 5

texe: 633 ms (PowerPC at 300 MHz)

Pygmalion

05.07.02, 17.23 h


α = 0.999

At Expo.02

[Arras et al. 03]

texe = 105 ms

05.07.02, 17.23 h


α = 0.999

At Expo.02

[Arras et al. 03]

05.07.02, 17.32 h


α = 0.999

At Expo.02

[Arras et al. 03]

05.07.02, 17.32 h


α = 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

Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf ·...

Documents

Transcript of Introduction to Mobile Robotics - George Mason Universitykosecka/cs685/cs685-ekf-features.pdf ·...