probabilistic robotics: motion model/ekf localization

City College of New York

1

Dr. Jizhong Xiao

Department of Electrical Engineering

CUNY City College

[email protected]

Probabilistic Robotics: Motion Model/EKF Localization

Advanced Mobile Robotics


2

Robot Motion• Robot motion is inherently uncertain.• How can we model this uncertainty?


• Prediction (Action)

• Correction (Measurement)

Bayes Filter Revisit

111 )(),|()( tttttt dxxbelxuxpxbel

)()|()( tttt xbelxzpxbel

111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel


4

Probabilistic Motion Models

• To implement the Bayes Filter, we need the transition model p(xt | xt-1, u).

• The term p(xt | xt-1, u) specifies a posterior probability, that action u carries the robot from xt-1 to xt.

• In this section we will specify, how p(xt | xt-1, u) can be modeled based on the motion equations.


5

Coordinate Systems• In general the configuration of a robot can be described

by six parameters.

• Three-dimensional cartesian coordinates plus three Euler angles pitch, roll, and tilt.

• Throughout this section, we consider robots operating on a planar surface.

• The state space of such systems is three-dimensional (x, y, ).


6

Typical Motion Models• In practice, one often finds two types of motion

models:

– Odometry-based

– Velocity-based (dead reckoning)

• Odometry-based models are used when systems are equipped with wheel encoders.

• Velocity-based models have to be applied when no wheel encoders are given.

• They calculate the new pose based on the velocities and the time elapsed.


7

Example Wheel EncodersThese modules require +5V and GND to power them, and provide a 0 to 5V output. They provide +5V output when they "see" white, and a 0V output when they "see" black.

These disks are manufactured out of high quality laminated color plastic to offer a very crisp black to white transition. This enables a wheel encoder sensor to easily see the transitions.

Source: http://www.active-robots.com/


8

Dead Reckoning• Derived from “deduced reckoning.”• Mathematical procedure for determining the present

location of a vehicle.• Achieved by calculating the current pose of the

vehicle based on its velocities and the time elapsed.

• Odometry tends to be more accurate than velocity model,

• But, Odometry is only available after executing a motion command, cannot be used for motion planning


9

Reasons for Motion Errors

bump

ideal casedifferent wheeldiameters

carpetand many more …


Odometry Model

22 )'()'( yyxxtrans

)','(atan21 xxyyrot

12 ' rotrot

• Robot moves from to . • Odometry information .

,, yx ',',' yx

transrotrotu ,, 21

trans1rot

2rot

,, yx

',',' yx

Relative motion information, “rotation” “translation” “rotation”


11

The atan2 Function• Extends the inverse tangent and correctly copes with the signs of x and y.


Noise Model for Odometry

• The measured motion is given by the true motion corrupted with independent noise.

||||11 211

ˆtransrotrotrot

||||22 221

ˆtransrotrotrot

|||| 2143

ˆrotrottranstranstrans


Typical Distributions for Probabilistic Motion Models

2

2

22

1

22

1)(

x

ex

2

2

2

6

||6

6|x|if0)(2

xx

Normal distribution Triangular distribution


14

Calculating the Probability (zero-centered)

• For a normal distribution

• For a triangular distribution

1. Algorithm prob_normal_distribution(a, b):

2. return

1. Algorithm prob_triangular_distribution(a,b):

2. return


15

Calculating the Posterior Given xt, xt-1, and u

22 )'()'( yyxxtrans )','(atan21 xxyyrot

12 ' rotrot 22 )'()'(ˆ yyxxtrans )','(atan2ˆ

1 xxyyrot

12ˆ'ˆrotrot

)ˆ|ˆ|,ˆ(prob trans21rot11rot1rot1 p|))ˆ||ˆ(|ˆ,ˆ(prob rot2rot14trans3transtrans2 p

)ˆ|ˆ|,ˆ(prob trans22rot12rot2rot3 p

1. Algorithm motion_model_odometry (xt, xt-1, u)

2.

3.

4.

5.

6.

7.

8.

9.

10.

11. return p1 · p2 · p3

odometry values (u)

values of interest (xt-1, xt)

Ttt xxu 1

Tt yxx )(1

Tt yxx )(

An initial pose Xt-1

A hypothesized final pose Xt

A pair of poses u obtained from odometry

),( baprobImplements an error distribution over a with zero mean and standard deviation b),( 1ttt xuxp


Application• Repeated application of the sensor model for

short movements.• Typical banana-shaped distributions obtained for

2d-projection of 3d posterior.

x’ u

p(xt| u, xt-1)

u

x’

Posterior distributions of the robot’s pose upon executing the motion command illustrated by the solid line. The darker a location, the more likely it is.


17

Velocity-Based Model

v

ucontrol v

r Rotation radius


18

Equation for the Velocity ModelInstantaneous center of curvature (ICC) at (xc , yc)

sinrxx c cosryy c

Initial pose Tt yxx 1

Keeping constant speed, after ∆t time interval, ideal robot will be at T

t yxx

t

try

trx

y

x

c

c

)cos(

)sin(

t

trr

trr

y

x

)cos(cos

)sin(sin Corrected, -90


19

Velocity-based Motion Model

With and are the state vectors at time t-1 and t respectively

t

tvv

tvv

y

x

y

x

t

tt

t

t

t

tt

t

t

t

ˆ

)ˆcos(ˆ

ˆcos

ˆ

ˆ

)ˆsin(ˆ

ˆsin

ˆ

ˆ

'

'

'

Tt yxx 1 Tt yxx '''

The true motion is described by a translation velocity and a rotational velocity

tv tMotion Control with additive Gaussian noise

),0(

ˆ

ˆ

243

221 )(

tt

t

v

v

t

t

t

t Mvvv

tt

tt

Tttt vu )(

2

43

221

)(0

0)(

tt

ttt v

vM

Circular motion assumption leads to degeneracy ,2 noise variables v and w 3D poseAssume robot rotates when arrives at its final pose

tt ˆ

65

ˆ v


20

Velocity-based Motion ModelMotion Model:

tt

tvv

tvv

y

x

y

x

t

tt

t

t

t

tt

t

t

t

ˆˆ

)ˆcos(ˆ

ˆcos

ˆ

ˆ

)ˆsin(ˆ

ˆsin

ˆ

ˆ

'

'

'

243

221 )(

ˆ

ˆ

tt

tt

v

v

t

t

t

t vv

65

ˆ v

1 to 4 are robot-specific error parameters determining the velocity control noise

5 and 6 are robot-specific error parameters determining the standard deviation of the additional rotational noise


21

Probabilistic Motion Model

Center of circle:

with

How to compute ?),( 1ttt xuxp Move with a fixed velocity during ∆t resulting in a circular trajectory from to

Tt yxx 1 T

t yxx

Radius of the circle:

2*2*2*2** )()()()( yyxxyyxxr

Change of heading direction: ),(2tan),(2tan **** xxyyaxxyya

t

r

t

distv

*

ˆt

ˆˆ

t

(angle of the final rotation)


22

Posterior Probability for Velocity Model

Motion error: verr ,werr and

Center of circle

Radius of the circle

Change of heading direction


Examples (velocity based)


25

Summary

• We discussed motion models for odometry-based and velocity-based systems

• We discussed ways to calculate the posterior probability p(x| x’, u).

• Typically the calculations are done in fixed time intervals t.

• In practice, the parameters of the models have to be learned.

• We also discussed an extended motion model that takes the map into account.


26

Localization, Where am I??

• Given – Map of the environment.– Sequence of measurements/motions.

• Wanted– Estimate of the robot’s position.

• Problem classes– Position tracking (initial robot pose is known)– Global localization (initial robot pose is unknown)– Kidnapped robot problem (recovery)


27

Markov Localization

Markov Localization: The straightforward application of Bayes filters to the localization problem


• Prediction (Action)

• Correction (Measurement)

Bayes Filter Revisit

111 )(),|()( tttttt dxxbelxuxpxbel

)()|()( tttt xbelxzpxbel

111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel


29

• Prediction:

• Correction:

EKF Linearization

)(),(),(

)(),(

),(),(

1111

111

111

ttttttt

ttt

tttttt

xGugxug

xx

ugugxug

)()()(

)()(

)()(

ttttt

ttt

ttt

xHhxh

xx

hhxh

First Order Taylor Expansion


30

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 )(


31

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

Prediction:

2.

3.

4.

5.

6.

),( 1 ttt ug T

tttTtttt 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

43

221

||||0

0||||

tt

ttt

v

vM

Motion noise covariance

Matrix from the control

Jacobian of g w.r.t location

Predicted mean

Predicted covariance

Jacobian of g w.r.t control


32


With and are the state vectors at time t-1 and t respectively

t

tvv

tvv

y

x

y

x

t

tt

t

t

t

tt

t

t

t

ˆ

)ˆcos(ˆ

ˆcos

ˆ

ˆ

)ˆsin(ˆ

ˆsin

ˆ

ˆ

'

'

'

Tt yxx 1 Tt yxx '''

The true motion is described by a translation velocity and a rotational velocity

tv t

Motion Control with additive Gaussian noise

),0(

ˆ

ˆ

243

221 )(

tt

t

v

v

t

t

t

t Mvvv

tt

tt

Tttt vu )(

2

43

221

)(0

0)(

tt

ttt v

vM


33


),0()cos(cos

)sin(sin

'

'

'

t

t

tt

t

t

t

tt

t

t

t

RN

t

tvv

tvv

y

x

y

x

),0(),( 1 tttt RNxugx

)(),(),(

)(),(

),(),(

1111

111

111

ttttttt

ttt

tttttt

xGugxug

xx

ugugxug

Motion Model:


34


),0()cos(cos

)sin(sin

'

'

'

t

t

tt

t

t

t

tt

t

t

t

RN

t

tvv

tvv

y

x

y

x

,1,1,1

,1,1,1

',1

'

,1

'

,1

'

1

111

),(),(

tytxt

tytxt

tytxt

t

ttttt

yyy

xxx

x

ugxG

Derivative of g along x’ dimension, w.r.t. x at

1t

xt

x

,1

Jacobian of g w.r.t location


35


),0()cos(cos

)sin(sin

'

'

'

t

t

tt

t

t

t

tt

t

t

t

RN

t

tvv

tvv

y

x

y

x

Derivative of g w.r.t. the motion parameters, evaluated at and

1t

tt

tt

tt

t

ttt

v

y

v

y

x

v

x

u

ugV

''

''

''

),( 1

t

ttvtvt

ttvtvt

t

tt

t

tt

t

t

t

tt

t

tt

t

t

0

)sin())cos((cos)cos(cos

)cos())sin((sin)sin(sin

2

2

Tttt

Ttttt VMVGG 1

Mapping between the motion noise in control space to the motion noise in state space

Jacobian of g w.r.t control

tu


36

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


37

Feature-Based Measurement Model

2

2

2

)),(2tan

)()(

,,

2,

2,

s

r

j

xjyj

yjxj

it

it

it

s

xmyma

ymxm

s

r

)()(

)()( ttt

ttt x

x

hhxh

,

,

,

,

,

,),(

t

t

t

t

yt

t

yt

t

xt

t

xt

t

t

tt

rrr

x

mhH

),0(),,( ttit QNmjxhz

• Jacobian of h w.r.t location

Is the landmark that corresponds to the measurement of

itzi

tCj


38

EKF Localizationwith known

correspondences


39

EKF Localizationwith unknown

correspondences

Maximum likelihood estimator


40

EKF Prediction Step


41

EKF Observation Prediction Step


42

EKF Correction Step


43

Estimation Sequence (1)


44

Estimation Sequence (2)


45

Comparison to Ground Truth


46

UKF Localization?

• Given – Map of the environment.– Sequence of measurements/motions.

• Wanted– Estimate of the robot’s position.

• UKF localization


47

Unscented Transform

nin

wwn

nw

nw

ic

imi

i

cm

2,...,1for )(2

1 )(

)1( 2000

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 covarianceFor n-dimensional Gaussianλ is scaling parameter that determine how far the sigma points are spread from the meanIf the distribution is an exact Gaussian, β=2 is the optimal choice.


48

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

Prediction:

2

43

221

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


49

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


50

UKF Prediction Step


51

UKF Observation Prediction Step


52

UKF Correction Step


53

EKF Correction Step


54

Estimation Sequence

EKF PF UKF


55

Estimation Sequence

EKF UKF


56

Prediction Quality

EKF UKF


57

• [Arras et al. 98]:

• Laser range-finder and vision

• High precision (<1cm accuracy)

Kalman Filter-based System

[Courtesy of Kai Arras]


58

Multi-hypothesisTracking


59

• 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


60

• Hypotheses are extracted from Laser Range Finder

(LRF) scans• Each hypothesis has probability of being the correct

one:

• Hypothesis probability is computed using Bayes’ rule

• Hypotheses with low probability are deleted.

• New candidates are extracted from LRF scans.

MHT: Implemented System (1)

)}(,,ˆ{ iiii HPxH

},{ jjj RzC

)(

)()|()|(

sP

HPHsPsHP ii

i

[Jensfelt et al. ’00]


61

MHT: Implemented System (2)

Courtesy of P. Jensfelt and S. Kristensen


62

MHT: Implemented System (3)Example run

Map and trajectory

# hypotheses

#hypotheses vs. time

P(Hbest)

Courtesy of P. Jensfelt and S. Kristensen


63

Thank You

probabilistic robotics: motion model/ekf localization

Documents