101
ROBOT MAPPING AND EKF SLAM

ROBOT MAPPING AND EKF SLAM. Slides for the book: Probabilistic Robotics Authors: – Sebastian Thrun – Wolfram Burgard – Dieter Fox Publisher: – MIT Press,

Embed Size (px)

Citation preview

ROBOT MAPPING AND

EKF SLAM

Slides for the book:Probabilistic Robotics

• Authors:– Sebastian Thrun– Wolfram Burgard– Dieter Fox

• Publisher:– MIT Press, 2005.

• Web site for the book & more slides:http://www.probabilistic-robotics.org/

Good example of EKF

Landmark-based Localization

Extended Kalman Filters

Nonlinear Dynamic Systems

nonlinear

Non-Gaussian Distributions in Kalman filters?

• We use Taylor expansion to linearize

Last slide main observation

First Order Taylor

Expansions

Reminder: Jacobian Matrix

x=(x1,x2,…,xn)nonlinear

Tangent plane represents multi-dimensional derivative

Reminder: Jacobian Matrix

Jacobian Matrix represents the gradient of a scalar valued function

EKF Linearization is done through First Order Taylor Expansion

Linearization for prediction

Linearization for correction

vector

vectormatrix

Matrix-vector multiplication again

• When we insert to Kalman filter equations

()

( )

From previous slide

nonlinear

• When we insert to Kalman filter equations

)

From previous slide

nonlinear

(

EKF Linearization: First Order Taylor Series Expansion. Matrix vs Scalar

• Scalar case

• MATRIX case

EKF Algorithm compared with Kalman Algorithm

Was in linear Kalman

Extended Kalman Filter Algorithm

Linearity Assumption Revisited

Mean of p(x)

Linear mapping of Mean: y=ax+b

Mean of p(y)

Grey represents true distribution of p(y)

This is ideal case of pushing gaussian

through a linear system

Non-linear Function

Gaussian of p(y)

Non-linear function g(x)

Mean of p(x)

This is a real case of pushing a gaussian

through a non-linear system

Grey represents true distribution of p(y)

We are approximating grey by blue, not good

EKF Linearization (1): Taylor approximation and EKF Gaussian

• Better than in last slide.

• The mean is closer to grey shape

This example shows that Gaussian of EKF better represents estimated value than the Gaussian mean

• Now the height is reduced to 2.

• More uncertainty

• EKF even better

EKF Linearization (2)

• Even more difference of EKF Gaussian and real Gaussian, but better estimation

EKF Linearization (3)

Smaller max value

Conclusion: NON-GAUSSIAN distribution of output requires EKF or something better that would calculate the mean better

• Gaussian with high value of standard deviation

• In this case the input Gaussian does not create an output Gaussian!

• And Gaussian estimation of output is worse than EKF estimation of Gaussian with respect to mean

Narrow and short gaussian

EKF Linearization (5)Gaussian similar to EKF gaussian

In this case EKF works similarly to KF

Summary on Extended Kalman Filters

Literature on Kalman Filter and EKF

EKF Localization Example

Localization of two-dimensional robot

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

1. EKF_localization ( mt-1, St-1, ut, zt, m):

Prediction Phase:

2.

3.

4. ),( 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

43

221

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

(x,y,)

velocity

Radial velocityVelocity and radial velocity are controls

1. EKF_localization ( mt-1, St-1, ut, zt, m):

Correction phase:

2.

3.

4.

5.

6.

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

Tttt 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

r = range = angle

EKF Prediction Step for four cases

Updated mean

Updated covariance

),( 1 ttt ug Tttt

Ttttt VMVGG 1

Predicted mean

Predicted covariance

32

EKF Prediction Step for four cases: now we take other system

Updated mean

Updated covariance

),( 1 ttt ug

Tttt

Ttttt VMVGG 1

Predicted mean

Predicted covariance

33

EKF Observation Prediction Step

Z = measurement

robot

landmark

Predicted robot covariance

Similarly for three other cases but sizes and shapes are different

34

EKF Correction Step

Estimation Sequence (1)

landmarkInitial true robot Initial

estimated robot

Based on measurement only Based on Kalman

observe that now our measurements are less precise

Estimation Sequence (2)

And we see a difference between estimated and real positions of the robot

37

Comparison to GroundTruth

38

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!

SLAM Review

Noise in motion

Noise in measurement

EKF SLAM

• State vector has size 3+2n

2N coordinates for N features

3 coordinates for robot’s pose

State Representation for EKF SLAM

Mean Value Matrix (Vector)

Covariance matrix

State Representation for EKF SLAM

Mean Value Matrix Covariance matrix

Please observe how these matrices are partitioned to submatrices

• Just another notation for the same stuff as in last slide

• One more often used notation for the same stuff as in last slide

Filter Cycle for the EKF SLAM

In green we show the part related to state prediction

• Now in green we have new estimated state and new estimated covariance matrix

A concrete example of EKF SLAM

EKF-SLAM: a concrete example of robot on a plane

• Robot moves in the plane• Velocity-based motion model:

1. V = speed2. = radial velocity

• Robot observes point landmarks• Range-bearing sensor • Known data association to landmark points• Known number of landmarks.

N landmarks

State Vector of mean estimates

Covariance matrix

Nothing known about landmarks

Last slide

See next slideRemember notations F T x and g of our matrices

Now our task is to calculate matrix G

Note that we denote the Jacobian of motion by Gx

t

• From previous slides

This slides just shows how to calculate the Jacobian using a new notation

Noise in motion

Noise in measurement

Use notation F from one of previous slides

Now we have to calculate Jacobian H and calculate Kalman Gain Matrix K

Implementation Notes

Loop Closing

EKF SLAM Properties

Practical Examples of applications