probabilistic robotics robot localization. 2 localization given map of the environment. sequence of...

Probabilistic Robotics

Robot Localization

2

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]

3

Localization

4

Localization

Position tracking

5

Localization

Global localization

6

Landmark-based Localization

7

Linearity Assumption Revisited

8

Non-linear Function

9

EKF Linearization (1)

10


11


12

•Prediction:

•Correction:

EKF Linearization: First Order Taylor Series Expansion

)(),(),(

)(),(

),(),(

1111

111

111

ttttttt

ttt

tttttt

xGugxug

xx

ugugxug

)()()(

)()(

)()(

ttttt

ttt

ttt

xHhxh

xx

hhxh

13

EKF Algorithm

1. Extended_Kalman_filter( t-1, t-1, ut, zt):

2. Prediction:3. 4.

5. Correction:6. 7. 8.

9. Return t, t

),( 1 ttt ug

tTtttt RGG 1

1)( tTttt

Tttt QHHHK

))(( ttttt hzK

tttt HKI )(

1

1),(

t

ttt x

ugG

t

tt x

hH

)(

ttttt uBA 1

tTtttt RAA 1

1)( tTttt

Tttt QCCCK

)( tttttt CzK

tttt CKI )(

14

1. EKF_localization ( t-1, t-1, ut, zt, m):

Prediction:

2.

3.

4.

5.

6.

),( 1 ttt ug Tttt

Ttttt VMVGG 1

,1,1,1

,1,1,1

,1,1,1

1

1

'''

'''

'''

),(

tytxt

tytxt

tytxt

t

ttt

yyy

xxx

x

ugG

tt

tt

tt

t

ttt

v

y

v

y

x

v

x

u

ugV

''

''

''

),( 1

2

42

3

22

21

0

0

tt

ttt

v

vM

Motion noise

Jacobian of g w.r.t location

Predicted mean

Predicted covariance

Jacobian of g w.r.t control

15

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

x

mhH

,,,

2,

2,

,2atanˆ

txtxyty

ytyxtxt

mm

mmz

tTtttt QHHS

1 tTttt SHK

2

2

0

0

r

rtQ

Predicted measurement mean

Pred. measurement covariance

Kalman gain

Updated mean

Updated covariance

Jacobian of h w.r.t location

16

EKF Prediction Step (known correspondences)

17

EKF Correction Step (known correspondences)

18

EKF Prediction Step (unknown correspondences)

19

EKF Correction Step (unknown correspondences)

20

EKF Prediction Step

21

EKF Observation Prediction Step

22

EKF Correction Step

23

Estimation Sequence (1)

24

Estimation Sequence (2)

25

Comparison to GroundTruth

26

EKF Summary

•Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k2.376 + n2)

•Not optimal!•Can diverge if nonlinearities are large!•Works surprisingly well even when all

assumptions are violated!

27

Linearization via Unscented Transform

EKF UKF

28

UKF Sigma-Point Estimate (2)

EKF UKF

29

UKF Sigma-Point Estimate (3)

EKF UKF

30

Unscented Transform

média a relação em espalhados estão points sigma os distantes

quão determinam que parâmetros a iguais sendok e com ,)(

2,...,1for )(2

1 )(

)1(

2

2000

nkn

nin

wwn

nw

nw

ic

imi

i

cm

Sigma points Weights

)( ii g

n

i

Tiiic

n

i

iim

w

w

2

0

2

0

))(('

'

Pass sigma points through nonlinear function

Recover mean and covariance

31

UKF_localization ( t-1, t-1, ut, zt, m):

Prediction:

2

42

3

22

21

0

0

tt

ttt

v

vM

2

2

0

0

r

rtQ

TTTt

at 000011

t

t

tat

Q

M

00

00

001

1

at

at

at

at

at

at 111111

xt

utt

xt ug 1,

L

i

T

txtit

xti

ict w

2

0,,

L

i

xti

imt w

2

0,

Motion noise

Measurement noise

Augmented state mean

Augmented covariance

Sigma points

Prediction of sigma points

Predicted mean

Predicted covariance

32

UKF_localization ( t-1, t-1, ut, zt, m):

Correction:

zt

xtt h

L

iti

imt wz

2

0,ˆ

Measurement sigma points

Predicted measurement mean

Pred. measurement covariance

Cross-covariance

Kalman gain

Updated mean

Updated covariance

Ttti

L

itti

ict zzwS ˆˆ ,

2

0,

Ttti

L

it

xti

ic

zxt zw ˆ,

2

0,

,

1, tzx

tt SK

)ˆ( ttttt zzK

Tttttt KSK

33

UKF Prediction Step

34

UKF Observation Prediction Step

35

UKF Correction Step

36

EKF Correction Step

37

Estimation Sequence

EKF PF UKF

38

Estimation Sequence

EKF UKF

39

Prediction Quality

EKF UKF

40

UKF Summary

•Highly efficient: Same complexity as EKF, with a constant factor slower in typical practical applications

•Better linearization than EKF: Accurate in first two terms of Taylor expansion (EKF only first term)

•Derivative-free: No Jacobians needed

•Still not optimal!

41

• [Arras et al. 98]:

• Laser range-finder and vision

• High precision (<1cm accuracy)

Kalman Filter-based System

[Courtesy of Kai Arras]

42

Map-based Localization

43

Monte Carlo (Particle Filter) Localization

44

Resampling Algorithm

45


46


1. Algorithm sample_normal_distribution(b):

2. return

1. Algorithm sample_triangular_distribution(b):

2. return

47


48


49


50


66


67


68

Multi-hypothesisTracking

69

• Belief is represented by multiple hypotheses

• Each hypothesis is tracked by a Kalman filter

• Additional problems:

• Data association: Which observation

corresponds to which hypothesis?

• Hypothesis management: When to add / delete

hypotheses?

• Huge body of literature on target tracking, motion

correspondence etc.

Localization With MHT

70

MHT: Implemented System (2)

Courtesy of P. Jensfelt and S. Kristensen

71

MHT: Implemented System (3)Example run

Map and trajectory

# hypotheses

#hypotheses vs. time

P(Hbest)

Courtesy of P. Jensfelt and S. Kristensen

probabilistic robotics robot localization. 2 localization given map of the environment. sequence of...

Documents

filter mt

st ekf

localization mt

t locationekf prediction

measurement dimensionality

ekf algorithm extended

terms of taylor expansion

motion noisejacobian