EE565:Mobile Roboticsweb.lums.edu.pk/~akn/Files/Other/teaching/mobile... · EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation 13 Components of a Kalman Filter H

EE565:Mobile Robotics

Welcome

Dr. Ahmad Kamal Nasir

16.02.2015 Dr. Ahmad Kamal Nasir 1

EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation

Today’s Objectives

• Recursive State Estimation: Bayes Filter

• Linear Kalman Filter

• Extended Kalman Filter



3

Bayes Formula

evidence

prior likelihood

)(

)()|()(

)()|()()|(),(

yP

xPxyPyxP

xPxyPyPyxPyxP

16.02.2015 Dr. Ahmad Kamal Nasir

)()|(

1)(

)()|()(

)()|()(

1

xPxyPyP

xPxyPyP

xPxyPyxP

x


4

Simple Example of State Estimation

• Suppose a robot obtains measurement z

• What is P(open|z)?



5

Example • P(z|open) = 0.6 P(z|open) = 0.3

• P(open) = P(open) = 0.5

67.03

2

5.03.05.06.0

5.06.0)|(

)()|()()|(

)()|()|(

zopenP

openpopenzPopenpopenzP

openPopenzPzopenP

• z raises the probability that the door is open.



• Prediction

• Correction

Bayes Filter

111 )(),|()( tttttt dxxbelxuxpxbel

)()|()( tttt xbelxzpxbel



Kalman Filter

• Bayes filter with Gaussians

• Developed in the late 1950's

• Most relevant Bayes filter variant in practice

• Applications range from economics, weather forecasting, satellite navigation to robotics and many more.

• The Kalman filter “algorithm” is a couple of matrix multiplications!



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



Gaussians


1D

2D

3D


),(~),(~ 22

2

abaNYbaXY

NX

Properties of Gaussians

2

2

2

1

22

2

2

1

2

112

2

2

1

2

2212

222

2

111 1,~)()(

),(~

),(~

NXpXp

NX

NX



• We stay Gaussian as long as we start with Gaussians and

perform only linear transformations.

),(~),(~

TAABANYBAXY

NX

Multivariate Gaussians

1

2

1

1

2

21

11

21

221

222

111 1,~)()(

),(~

),(~

NXpXp

NX

NX



12

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



13

Components of a Kalman Filter

t

Matrix (nxn) that describes how the state evolves from t to t-1 without controls or

noise.

tA

Matrix (nxl) that describes how the control ut changes the state from t to t-1. tB

Matrix (kxn) that describes how to map the state xt to an observation zt.

tC

t

Random variables representing the process and measurement noise that are assumed to be independent and normally distributed with covariance Rt and Qt respectively.



14

Kalman Filter Updates in 1D

t

T

tttt

ttttt

tRAA

uBAxbel

1

1)(

2

,

222

1)(

tactttt

ttttt

ta

ubaxbel


How to get the magenta one? State prediction step


15

Kalman Filter Updates in 1D

1)(with )(

)()(

t

T

ttt

T

ttttttt

tttttt

t QCCCKCKI

CzKxbel

2

,

2

2

22with

)1(

)()(

tobst

tt

ttt

ttttt

t KK

zKxbel


How to get the blue one? Kalman correction step


16

Kalman Filter Updates


Prediction

Measurements Correction


• Prediction

• Correction

Bayes Filter: Reminder

111 )(),|()( tttttt dxxbelxuxpxbel

)()|()( tttt xbelxzpxbel



18

0000 ,;)( xNxbel

Linear Gaussian Systems: Initialization

• Initial belief is normally distributed:



19

• Dynamics are linear function of state and control plus additive noise:

tttttt uBxAx 1

Linear Gaussian Systems: Dynamics

ttttttttt RuBxAxNxuxp ,;),|( 11

1111

111

,;~,;~

)(),|()(

ttttttttt

tttttt

xNRuBxAxN

dxxbelxuxpxbel



20

Linear Gaussian Systems: Dynamics

t

T

tttt

ttttt

t

tttt

T

tt

tttttt

T

tttttt

ttttttttt

tttttt

RAA

uBAxbel

dxxx

uBxAxRuBxAxxbel

xNRuBxAxN

dxxbelxuxpxbel

1

1

111

1

111

1

1

1

1111

111

)(

)()(2

1exp

)()(2

1exp)(

,;~,;~

)(),|()(



21

• Observations are linear function of state plus additive noise:

tttt xCz

Linear Gaussian Systems: Observations

tttttt QxCzNxzp ,;)|(

ttttttt

tttt

xNQxCzN

xbelxzpxbel

,;~,;~

)()|()(



22

Linear Gaussian Systems: Observations

1

11

)(with )(

)()(

)()(2

1exp)()(

2

1exp)(

,;~,;~

)()|()(

t

T

ttt

T

ttttttt

tttttt

t

ttt

T

tttttt

T

tttt

ttttttt

tttt

QCCCKCKI

CzKxbel

xxxCzQxCzxbel

xNQxCzN

xbelxzpxbel



23

Kalman Filter Algorithm

1. Algorithm Kalman_filter( t-1, t-1, ut, zt):

2. Prediction:

3.

4.

5. Correction:

6.

7.

8.

9. Return t, t

ttttt uBA 1

t

T

tttt RAA 1

1)( t

T

ttt

T

ttt QCCCK

)( tttttt CzK

tttt CKI )(









26

The Prediction-Correction-Cycle

t

T

tttt

ttttt

tRAA

uBAxbel

1

1)(

2

,

222

1)(

tactttt

ttttt

ta

ubaxbel

Prediction



27


1)(,)(

)()(

t

T

ttt

T

ttttttt

tttttt

t QCCCKCKI

CzKxbel

2

,

2

2

22 ,

)1(

)()(

tobst

tt

ttt

ttttt

t KK

zKxbel

Correction



28


1)(,)(

)()(

t

T

ttt

T

ttttttt

tttttt

t QCCCKCKI

CzKxbel

2

,

2

2

22 ,

)1(

)()(

tobst

tt

ttt

ttttt

t KK

zKxbel

t

T

tttt

ttttt

tRAA

uBAxbel

1

1)(

2

,

222

1)(

tactttt

ttttt

ta

ubaxbel

Correction

Prediction



29

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!



30

Nonlinear Dynamic Systems

• Most realistic robotic problems involve nonlinear functions



31

Linearity Assumption Revisited



32

Non-linear Function


Non-Guassian

• The non-linear functions lead to non-Gaussian distributions

• Kalman filter is not applicable anymore!

What can be done to resolve this?


33

• Prediction:

• Correction:

EKF Linearization: First Order Taylor Series Expansion

)(),(),(

)(),(

),(),(

1111

11

1

111

ttttttt

tt

t

tttttt

xGugxug

xx

ugugxug

)()()(

)()(

)()(

ttttt

tt

t

ttt

xHhxh

xx

hhxh


Jacobian matrices


Reminder: Jacobian Matrix

• It is a non-square matrix in 𝑛 × 𝑚 general

• Given a vector-valued function

• The Jacobian matrix is defined as



Reminder: Jacobian Matrix

• It is the orientation of the tangent plane to the vector-valued function at a given point

• Generalizes the gradient of a scalar valued function



36

• Prediction:

• Correction:

EKF Linearization: First Order Taylor Series Expansion

)(),(),(

)(),(

),(),(

1111

11

1

111

ttttttt

tt

t

tttttt

xGugxug

xx

ugugxug

)()()(

)()(

)()(

ttttt

tt

t

ttt

xHhxh

xx

hhxh


Linear functions


37

EKF Linearization



38

EKF Linearization (Cont.)



39

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

ttt

x

ugG

t

tt

x

hH

)(

ttttt uBA 1

t

T

tttt RAA 1

1)( t

T

ttt

T

ttt QCCCK

)( tttttt CzK

tttt CKI )(



Google Car – Tracking using KF



41

Landmark-based Localization


EKF localization with landmarks (point features)


42

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

ttt

yyy

xxx

x

ugG

tt

tt

tt

t

ttt

v

y

v

y

x

v

x

u

ugV

''

''

''

),( 1

2

43

2

21

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



43

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

3.

5.

6.

7.

8.

9.

10.

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

t

T

tttt QHHS

1 t

T

ttt SHK

2

2

0

0

r

r

tQ

Predicted measurement mean

Pred. measurement covariance

Kalman gain

Updated mean

Updated covariance

Jacobian of h w.r.t location



Summary

• Recursive State Estimation: Bayes Filter

• Linear Kalman Filter

• Extended Kalman Filter

– Works well in practice for moderate nonlinearities



Questions


Documents

EE565:Mobile Roboticsweb.lums.edu.pk/~akn/Files/Other/teaching/mobile... · EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation 13 Components of a Kalman Filter H