Peter S. Maybeck – “Stochastic models, estimation, and control” Greg Welch et Al. –...

Peter S. Maybeck – “Stochastic models, estimation, and control”Greg Welch et Al. – ”Introduction to the Kalman filter”Sebastian Thrun, Udacity. “Artificial Intelligence for Robotics –Kalman Filters”Micheal Williams - “KalmanFilters” , http://users.cecs.anu.edu.au/~hartley/Vision-Reading-Course/Kalman-filters.ppt

4. FEBUAR 2014

LESSON2 – KALMAN FILTERSJESPER JESSEN, 201300253

4. FEBUAR 2014

LESSON 2 – KALMAN FILTERS


4. FEBUAR 20142

AGENDA›Applications›What is a Kalman Filter›Conceptual Overview›Design Kalman Filter›Example ›Demo MATLAB


4. FEBUAR 2014

APPLICATIONS›Tracking Objects › Missiles › Lip reading› Hands

›Economics

›Navigation (Google car)

3


4. FEBUAR 2014

WHAT IS A KALMAN FILTER›Recursive data processing algorithm

›Recursive?› KF does not require all previous data to be kept in storage

›Kalman filter finds the most optimum averaging factor for each consequent state. Also somehow remembers a little bit about the past states

4


4. FEBUAR 2014

PREVIOUS LESSON›Monte Carlo localization › World divided info discrete grids› Approximates the posterior distribution as histogram

›Now we look at KF› The posterior distribution is given by a Gaussian› Gaussian is a cont. Function › Area underneath sums to one

5


4. FEBUAR 2014

CONCEPTUAL OVERVIEW›Lost on the 1D line

› A measurement at t1: Mean = z1 and Variance = 2z1

› Optimal estimate of position is: (t1) = z1

› Variance of error in estimate: 2x (t1) = 2

z1

› Boat in same position at time t2

6

0 10 20 30 40 50 60 70 80 90 1000

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16


4. FEBUAR 2014

› So we have the prediction -(t2)› Better Measurement at t2: Mean = z2 and Variance = z2

› Need to correct the prediction due to measurement to get (t2)› Closer to more trusted measurement

0 10 20 30 40 50 60 70 80 90 1000

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

CONCEPTUAL OVERVIEW

7

prediction -(t2)measurement z(t2)


4. FEBUAR 2014

CONCEPTUAL OVERVIEW

› Corrected mean is the new optimal estimate of position› New variance is smaller than either of the previous two variances

8

0 10 20 30 40 50 60 70 80 90 1000

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

measurement z(t2)

prediction -(t2)

corrected optimal estimate (t2)


4. FEBUAR 2014

CONCEPTUAL OVERVIEW

9

0 10 20 30 40 50 60 70 80 90 1000

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

› Optimal estimate (t2) = µ

22

2 2 2 2

2 2 2 22 2

2 2 2 2

2 2 2

2 2

2 2

2 2 2 2

xz

x z x z

z x x xz x

x z x z

x z x

x z

x x

x z x z

x x z

x z x xx z

x z x

x z x x K z x K

: prediction (a priori estimate): update (a posteriori estimate)K: Kalman gain


4. FEBUAR 2014

CONCEPTUAL OVERVIEW

10

0 10 20 30 40 50 60 70 80 90 1000

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

› At time t3, boat moves with velocity dx/dt=u› Naïve approach: Shift probability to the right to predict› This would work if we knew the velocity exactly (perfect model)

Naïve Prediction -(t3)(t2)


4. FEBUAR 2014

› Better to assume imperfect model by adding Gaussian noise› dx/dt = u + w› u= nominal velocity› w= noise term or uncertainty (variance)› Distribution for prediction moves and spreads out

CONCEPTUAL OVERVIEW

11

0 10 20 30 40 50 60 70 80 90 1000

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

(t2)Naïve Prediction -(t3)

Prediction -(t3)


4. FEBUAR 2014

› Now we take a measurement at t3

› Need to once again correct the prediction› Same as before

CONCEPTUAL OVERVIEW

12

0 10 20 30 40 50 60 70 80 90 1000

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

Prediction -

(t3)

Measurement z(t3)

Corrected optimal estimate (t3)

3 2 3 2

2 2 23 2 3 2

( ) ( ) ( )

( ) ( ) ( )x x w

x t x t u t t

t t t t

3 3 3 3 3

2 2 23 3 3 3

23

3 2 23 3

( ) ( ) ( )( ( ))

( ) ( ) ( ) ( )

( )( )

( )

x x x

x

x z

x t x t K t z x t

t t K t t

tK t

t


4. FEBUAR 2014

DESIGN KALMAN FILTERS

13


4. FEBUAR 2014


14

› Process to be estimated: (state space)

› Kalman Filter

xk = Axk-1 + Buk + wk-1

zk = Hxk + vk

Process Noise (w) with covariance Q

Measurement Noise (v) with covariance R

Prediction: - is estimate based on measurements at previous time-steps

k = -k + K(zk - H -

k )

Correction: k has additional information – the measurement at time k

K = P-kHT(HP-

kHT + R)-1

-k = Axk-1 + Buk

P-k = APk-1AT + Q

Pk = (I - KH)P-k

P-k: Prior error covariance


4. FEBUAR 2014


15


4. FEBUAR 2014


16


4. FEBUAR 2014

› Consider an object falling under a constant gravitational field. Let y(t) denote the height of the object, then:

y t g

0 0y t y t g t t

2

0 0 0 02

gy t y t y t t t t t

0Discrete time system with 1t t t

1 1 12

gy k y k g y k y k y k

KALMAN FILTERS - EXAMPLE

17


4. FEBUAR 2014

› Construct state space model from equations, when we are able to perform measurements, zk, of the height.

that is, find A, B, uk and H in:


18

1 1 12


xk = Axk-1 + Buk

zk = Hxk


4. FEBUAR 2014

› Construct state space model from equations, when we are able to perform measurements, zk, of the height.

Solution:


19

1 1 12


1

11

1 1 0.5

0 1 1

1 0

k k

k kk k

k k kk

k

y yg

y yA B

H yz

y

x x u

z x

A B

H

uXk-1Xk

Xkzk


4. FEBUAR 2014

EXAMPLE›MATLAB demo

20

Peter S. Maybeck – “Stochastic models, estimation, and control” Greg Welch et Al. –...

Documents

Transcript of Peter S. Maybeck – “Stochastic models, estimation, and control” Greg Welch et Al. –...