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

download Introduction to Mobile Robotics - UvA ... Introduction to Mobile Robotics Author arnoud Created Date

of 55

  • date post

    28-May-2020
  • Category

    Documents

  • view

    3
  • download

    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

    start with Gaussians and perform only linear transformations.

    ),(~ ),(~

    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