Kalman filter and SLAM problem

2005. 8. 5Young Ki Baik

Computer Vision Lab.

Seoul National University

Kalman filter and SLAM problem

Computer Vision Lab. SNU

Contents

References

Kalman filter

SLAM problem

Example (2D circular motion)

Demo

Conclusion and future work

Kalman filter and SLAM problem

Computer Vision Lab. SNU

References

An Introduction to the Kalman Filter

G. Welch and G. Bishop (SIGGRAPH 2001)

A Solution to the Simultaneous Localization and Map Building (SLAM) problem

Gamini Dissanayake. Et. Al. (IEEE Trans. Robotics and Automation 2001)

Lessons in Estimation Theory for Signal Processing, Communications and Control

Jerry M. Mendel (1995)

Kalman filter and SLAM problem

Computer Vision Lab. SNU

Kalman filter

What is a Kalman filter?Mathematical power tool

Optimal recursive data processing algorithm

Noise effect minimization

ApplicationsTracking (head, hands etc.)

Lip motion from video sequences of speakers

Fitting spline

Navigation

Lot’s of computer vision problem

Kalman filter and SLAM problem

Computer Vision Lab. SNU

Kalman filter

Example

Real location

Location with error

Measurement error

Localizing error (Processing error)

Refined location

Robot

Landmark

Movement noise

Sensor noise

How can we

obtain optimal

pose of robot and

landmark

simultaneously?

Kalman filter

Kalman filter and SLAM problem

Computer Vision Lab. SNU

Kalman filter

Example (Simple Gaussian form)

Assumption

All error form Gaussian noise

Estimated value

Measurement value

2, eex 2, eexN

2, mmxN 2, mmx

Kalman filter and SLAM problem

Computer Vision Lab. SNU

Example (Simple Gaussian form)

Optimal variance

Optimal mean

Kalman filter

2,xN

mme

ee

me

m xxx

22

2

22

2

222

111

me

emme

ee xxxx

22

2

iKxx e Kalman gain

Innovation

Kalman filter and SLAM problem

Computer Vision Lab. SNU

Example (Overall process)

Prediction

Update

Kalman filter

kqkBukAxkxe 1

kr(k)Hxkz ee

kQAkAPkP Te 1

QNwp ,0~

RNvp ,0~

uc

x

xba

x

x

k

k

k

k

001 1

1

A B

cubxaxx kkk 11

1 RHHPHPK T

eT

e

kHxkzKkxkx eme

kPKHIkP e

Kalman filter and SLAM problem

Computer Vision Lab. SNU

SLAM

What is SLAM problem?Can we do localization and mapping simultaneously?

If we have the solution to the SLAM problem…Allow robots to operate in an environment without a priori knowledge of a map

Open up a vast range of potential application for autonomous vehicles and robot

Kalman filter based approachResearch over the last decade has shown that SLAM is indeed possible

Kalman filter and SLAM problem

Computer Vision Lab. SNU

SLAM

Kalman filter and SLAM problem

Extended Kalman filter form for SLAM

Prediction

Observation

Update

kSJkPkK THxe

1)(

xFJFx

xHJHx

kxHkz ee

kzkzki em )(

kikKkxkx e )( kKkSkKkPkP T

e )(

kQkJkPkJkP TFxFxe 1

kukxFkxe ,1

kRJkPJkS THxeHx )(

: Previous value

: Input and measure

: Function

: Computed value

iFL LFJi

Kalman filter and SLAM problem

Computer Vision Lab. SNU

Implementation

Example (2D circular motion)

x, z, L

x : Position and direction of robot

and L

z : Distance and angle from robot point of view

L : Landmark position

Setting of x and P

NN L

L

N

e

L

L

eN

e

yxekukB

kL

kL

kx

I

I

kA

kL

kL

kx

0

0

1

1

)1(

000

0

00

00

11 11

,,

xxx yx ,,

,d

yx LL ,

kBukAxkxe 1

Kalman filter and SLAM problem

Computer Vision Lab. SNU

Implementation

Example (2D circular motion)

Initial x and P

Setting of x and P with landmark

0

0

0

0

0

0

x

x

x

x y

x

1.000

01.00

001.0

p

?

?

*),,(

kL

kL

kx

x

y

x

yx

2

2

2,,

00

00

00

Ly

Lx

yx

e

e

e

p

3x1 3x3

5x1 5x5

Kalman filter and SLAM problem

Computer Vision Lab. SNU

Example (2D circular motion)

Control input

Implementation

1cos1 kxvtkxkx cxx

1sin1 kxvtkxkx cyy

ˆ1 ctkxkx

xxx yx ,,

,v

: 2d position and direction

: Velocity and angular velocity

ct : time (constant)

0.2 0.04, 1.0, ,ˆ , ctv

: Circular motion with radius 25

F

Kalman filter and SLAM problem

Computer Vision Lab. SNU

Implementation

Example (2D circular motion)

Real motion

1cos 1 kxtGsvkxkx cvxx

1sin 1 kxtGsvkxkx cvyy

wc Gstkxkx ˆ1

6.1vs

G : Zero mean unit variance

Gaussian random value

vs

ws: Control error for velocity

: Control error for

angular velocity

023.0ws

White line : Control input motion

Pink line : Real motion

Large circle : robot

Kalman filter and SLAM problem

Computer Vision Lab. SNU

Example (2D circular motion)

Predicted and measured information of land mark

Prediction

Measurement

Implementation

kx

kxkL

kxkLkz e

yeyi

xexi

ie

1

1tan 1

iL : Position of i-th landmark (x,y)

iz : Distance and angle (d, )

from a robot point of view

kxkLkz yxeiide ),(1

H

sensor from idmz

sensor from imz

sensor

r

Range r = 20.0

Kalman filter and SLAM problem

Computer Vision Lab. SNU

Implementation

Example (2D circular motion)

Jacobian matrix for F

xFJFx

1cos1 kxvtkxkx cxx

1sin1 kxvtkxkx cyy

ˆ1 ctkxkx

F

c

c

c

t

kxvt

kxvt

00

1cos0.10

1sin00.1

uFJFu

c

c

c

t

kxt

kxt

0

01sin

01cos

Kalman filter and SLAM problem

Computer Vision Lab. SNU

Implementation

Example (2D circular motion)

Jacobian matrix for H

xHJHx

kx

kxkL

kxkLkz e

yeyi

xexi

ie

1

1tan 1

kxkLkz yxeiide ),(1

HiHL LHJ

i

kxkLd yxei ),(1

0.111

011

2

)(

2

)(

)()(

d

kxkL

d

kxkLd

kLkx

d

kLkx

xexiyeyi

yiyexixe

2

)(

2

)(

)()(

11

11

d

kxkL

d

kLkxd

kxkL

d

kxkL

xexiyiye

yeyixexi

Kalman filter and SLAM problem

Computer Vision Lab. SNU

Example (2D circular motion)

Error covariant matrix

Covariant matrix of control error

Covariant matrix of measurement error

Implementation

TFuinputFu JSJkQ

2

2

0

0

w

inputs

sS v

vs

ws: Control error for velocity

: Control error for

angular velocity

2

2

0

0

w

d

r

rkR

dr

wr: measurement error for

distance: Measurement error for

angle

Kalman filter and SLAM problem

Computer Vision Lab. SNU

Implementation

Kalman filter and SLAM problem

Extended Kalman filter form for SLAM

Prediction

Observation

Update

kSJkPkK THxe

1)(

xFJFx

xHJHx

kxHkz ee

kzkzki em )(

kikKkxkx e )( kKkSkKkPkP T

e )(

kQkJkPkJkP TFxFxe 1

kukxFkxe ,1

kRJkPJkS THxeHx )(

: Previous value

: Input and measure

: Function

: Computed value

iFL LFJi

Kalman filter and SLAM problem

Computer Vision Lab. SNU

Implementation

Demo

Large Circle(white, pink, yellow) : robot

White line : control input path

Pink line : real path

Small white circle : Real landmark

Yellow line : Estimated path (EKF)

Large light blue circle

: Detected (and estimated) landmark

Blue ellipse : Uncertainty boundary

Kalman filter and SLAM problem

Computer Vision Lab. SNU

Conclusion

Conclusion

Simple example and demo

Possibility of solution for SLAM problem using EKF

In the limit of successive observations, the error in

estimated position of landmarks become fully correlated.

Future work

Considering closing loop and kidnapping problem

Applying EKF to general structure (robot) using vision

sensor