of 55

• date post

28-May-2020
• Category

Documents

• view

3

0

Embed Size (px)

Transcript of Introduction to Mobile Robotics - UvA ... Introduction to Mobile Robotics Author arnoud Created Date

• Probabilistic Robotics

Bayes Filter Implementations

Gaussian filters

• •Prediction

•Correction

Bayes Filter Reminder

111 )(),|()(  tttttt dxxbelxuxpxbel

)()|()( tttt xbelxzpxbel 

• Gaussians

2

2)(

2

1

2

2

1 )(

:),(~)(





 

x

exp

Nxp

- 

Univariate

)()( 2

1

2/12/

1

)2(

1 )(

:)(~)(

μxΣμx

Σ x

Σμx

 

 t

ep

,Νp

d

Multivariate

• ),(~ ),(~ 22

2

 

abaNY baXY

NX 

  



Properties of Gaussians

 

 

 

 

  

 2

2

2

1

22

2

2

1

2

1 12

2

2

1

2

2 212

222

2

111 1,~)()( ),(~

),(~

 



 





 NXpXp

NX

NX

• • We stay in the “Gaussian world” as long as we

),(~ ),(~

TAABANY BAXY

NX 

  



 

Multivariate Gaussians

 

  



 



 

  

  1

2

1

1

2

21

1 1

21

2 21

222

111 1 ,~)()(

),(~

),(~ 

 NXpXp

NX

NX

• 6

Discrete Kalman Filter

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

• Kalman Filter Algorithm

• Kalman Filter Algorithm

• Prediction • Observation

• Matching • Correction

• 9

The Prediction-Correction-Cycle

  



 

t

T

tttt

ttttt

t RAA

uBA xbel

1

1 )(



  



 

2

,

222

1 )(

tactttt

ttttt

t a

uba xbel





Prediction

• 10

The Prediction-Correction-Cycle

1)(, )(

)( )( 

  



  t

T

ttt

T

ttt tttt

tttttt

t QCCCK CKI

CzK xbel



2

,

2

2

22 ,

)1(

)( )(

tobst

t t

ttt

ttttt

t K K

zK xbel







 

  



 

Correction

• 11

The Prediction-Correction-Cycle

1)(, )(

)( )( 

  



  t

T

ttt

T

ttt tttt

tttttt

t QCCCK CKI

CzK xbel



2

,

2

2

22 ,

)1(

)( )(

tobst

t t

ttt

ttttt

t K K

zK xbel







 

  



 

  



 

t

T

tttt

ttttt

t RAA

uBA xbel

1

1 )(



  



 

2

,

222

1 )(

tactttt

ttttt

t a

uba xbel





Correction

Prediction

• 12

Kalman Filter Summary

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

•Optimal for linear Gaussian systems!

•Most robotics systems are nonlinear!

• 13

Nonlinear Dynamic Systems

•Most realistic robotic problems involve nonlinear functions

),( 1 ttt xugx

)( tt xhz 

• 14

Linearity Assumption Revisited

• 15

Non-linear Function

• 16

EKF Linearization (1)

• 17

EKF Linearization (2)

• 18

EKF Linearization (3)

• 19

• Prediction:

•Correction:

EKF Linearization: First Order Taylor Series Expansion

)(),(),(

)( ),(

),(),(

1111

11

1

1 11





 



 

 

ttttttt

tt

t

tt tttt

xGugxug

x x

ug ugxug



 

)()()(

)( )(

)()(

ttttt

tt

t

t tt

xHhxh

x x

h hxh



 



 

 

• 20

• 21

• 22

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 

t

T

tttt RGG  1

1)(  t T

ttt

T

ttt QHHHK

))(( ttttt hzK  

tttt HKI  )(

1

1),(

 

t

tt t

x

ug G

t

t t

x

h H

 

)(

ttttt uBA  1

t

T

tttt RAA  1

1)(  t T

ttt

T

ttt QCCCK

)( tttttt CzK  

tttt CKI  )(

• 23

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]

• 24

Landmark-based Localization

• 25

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

3.

5.

6.

7.

8.

),( 1 ttt ug  T

ttt

T

tttt VMVGG  1

      

      

 

 











,1,1,1

,1,1,1

,1,1,1

1

1

'''

'''

'''

),(

tytxt

tytxt

tytxt

t

tt t

yyy

xxx

x

ug G

      

      

 

  

tt

tt

tt

t

tt t

v

y

v

y

x

v

x

u

ug V

 

 

''

''

''

),( 1

    

 

 

2

43

2

21

||||0

0||||

tt

tt t

v

v M



 Motion noise

Jacobian of g w.r.t locati