tutorial on kalman filter

7/28/2019 Tutorial on Kalman filter

1/47

Tutorial on Kalman Filters

Recognition Systems

Compiled by: Tadele Belay

MSc, Mechatronics

Presented to: Prof. Farid Melgani

University of Trento,

Italy

2013 1


2/47

Rudolf Emil Kalman:

o Born 1930 in Hungary

o BS and MS from MIT

o PhD 1957 from Columbia

o Filter developed in 1960-61

o Now retired

2


3/47

The Kalman filter, also known as linear quadratic

estimation (LQE), is an algorithm that uses a

series of measurements observed over time,

containing noise (random variations) and other

inaccuracies, and produces estimates of unknown

variables that tend to be more precise than those

based on a single measurement alone

3


4/47

an optimal estimator (best)

It is recursive

4


5/47

Kalman Filtering gets popular because of:

Good results in practice due to optimality and structure.

Convenient form for online real time processing.

Easy to formulate and implement given a basic

understanding.

Measurement equations need not be inverted.

5


6/47

What is it used for?

Tracking missiles

Tracking heads/hands/drumsticks

Extracting lip motion from video

Fitting Bezier patches to point data

Economics

Navigation

6


7/47

Other applications of Kalman Filtering (or Filtering in

general):

Your Car GPS (predict and update location)

Surface to Air Missile (hitting the target)

Ship or Rocket navigation (Appollo 11 used some

sort of filtering to make sure it didnt miss the

Moon!)

7


8/47

kkkk

kkkk1k

wCy

v

x

BuxAx

State Estimation

Measurement

kk

kk

kkk

k

k

k

R,Qmatricescovariance

(known)withnoiseGaussianwhitemean,-zeroarew,v

(known)matricessystemddimensioneelyappropriatareC,B,A

measured)(known,vectoroutputldimensiona-ptheisy(known)vectorinputldimensiona-mtheisu

(unknown)vectorstateldimensiona-ntheisx

.. (1)

.. (2)

Given the linear dynamical system:

Observation vector

Control vector

8


9/47

Measuring

Devices

Estimator

Measurement

Error Sources

System State(desired but

not known)

External

Controls

Observed

Measurements

Optimal

Estimate of

System State

System

Error Sources

System

Black

Box

9


10/47

kkkkk1k vuBxAx

called the state of the system

Contains all of the information about the

present state of the system,

but we cannot measure x directly.

10


11/47

kkkkk1k vuBxAx

Previous state vector

kkkkk1k vuBxAx

o Input to the system

o It is known

11


12/47

kkkkk1k vuBxAx

Is called the process noise

kkkk wxCy

is the measured output

It help us to obtain an estimate of x

but we cannot necessarily take the information

from yk b/c it is corrupted by noise

12


13/47

kkkk wxCy

State vector

kkkk wxCy

called the measurement noise sequences ofwhite, zero mean,

Gaussian noise with zero mean

also for Vk

13


14/47

how can we determine the best estimate of the state x?

We want an estimator that gives an accurate estimate

of the true state though we cannot directly measure it.

What criteria should our estimator satisfy?

First, we want the average value of our state estimate

to be equal to the average value of the true state. i.e no

bias. Mathematically, = > E(estimate) = E(state)

Second, we want a state estimate that varies from the

true state as little as possible. Mathematically, we want

to find the estimator with the smallest possible error

variance 14


15/47

It so happens that the Kalman filter is the estimator that

satisfies these two criteria.

But the Kalman filter solution does not apply unless we

can satisfy certain assumptions about the noise that

affects our system.

Assumptions are:

1. Statevector & measurement noise are independent.2. Statevector estimation shown to be Gaussian hence

system noise and measurement noise are

uncorrelated.

15


16/47

Background of the equations

Let us estimate a random variable with 2 Gaussianobservations:

Suppose we are estimating the value of a random

variable x:

Sensor1 = value y1, and has a variance 12

Sensor2 = value y2, and has a variance 22

We can show the combined estimate has a combined

variance 2 w/c is harmonic mean of the twovariances and weighted mean would be given as:

2

2

2

1

2

2

2

12'

22

2

2

1

2

112

2

2

1

2

2 yyy

2

2

2)(

2

1)(

y

eyp

16


17/47

Observations:

The new variance is smaller than the two variances

(at most eqaul to both)

The new mean is weighted sum of the old mean

with higher weightage given to the lower variance

measurement

In the case where the two variances are equal, the

mean is (y1+y2)/2

17


18/47

Suppose that the measurements of above are done at

t1 and t2 times and remained constant throughout.

After the first measurement the estimate of x;

11 yx

22

2

2

1

2

112

2

2

1

2

22 yyx

After the second measurement

)yy(y 1222

2

1

2

11

)xy(Kxx 12212

112

2

2

1

2

122

2

2

1

2

11 yyyy

Weighted average

18


19/47

)xy(Kxx 12212

2

2

2

1

21

Correction terms introducedby the second observation

Best prediction before the second

observation

If22 < < 1

2 (if the second measurement has high

variance relative to first) then K(t=2) = 1, and X2(k2) = y2 .

Hence, the correction form is small. 19


20/47

To go from the preceding discussion to Kalman filter;

We just need to say that the system has dynamics. So ( xk+1 xk ).

We shall have two arrangements.

1. One coming from our estimate of xk pushed through transition

model: has variance x2 and t

2 (transition variance)

2. A direct measurement of xk+1 ; has a variance of2

y

Xk Xk+1

yK yK+1

+N(0,x)

2t

2y

22 = y

2

12 = t

2 + x2

20


21/47

New variance is harmonic mean of12 & 22and a newestimate is:

estimatefrom

transition

model

estimatefrom

transition

model(pred

.)

K(k+1)new

estimate

Note that:

As we move from k+1 to k+2 the prior Xt+1 now has the

new variance that we just computed

So x2 has changed but t

2 and y2 remained the same

Kt+2 which depends on x2 , t

2 , and y2 changes, but it is

pre-computed.

estimate

from

sensor(me

asnt)

21


22/47

The computational origins of the Filters

Priori state estimation error at step k:

Posteriori estimation error:

Posteriori as a linear combination of an Priori:

kkkk

kkkkk1k

wxCy

vuBxAx

.. (1)

.. (2)22


23/47

The Probabilistic origins of the Filter

The a posteriori state estimate reflects the

mean of the state distribution

The a posteriori state estimate error

covariance reflects the variance of the statedistribution

.. (3)

.. (4)

.. (5)23


24/47

An update equation for the new estimate, combing the

old estimate with measurement data thus;

kkkkk xCyKxx

Kalman gain

Innovation or

measurement residual

kkk x

Cyi

.. (6)

.

.

.

(7)

Filtered state estimate = predicted state estimate + Gain * Error

prior estimate of xk

24


25/47

Substitution of (2) into (6) gives;

kkkkkkk xCwxCKxx .. (8)

.. (9)

Substitution of (8) into (4) gives;

])wK)xx)(CKI((

)wK)xx)(CKI[(EP

T

kkkkk

kkkkkk

Is the error of the prior estimate, it is clear that this is uncorrelated

with the measurement noise and therefore the expectation may berewritten as:

T

k

T

kkk

T

k

T

kkkkkk

K)ww(EK

)CKI]()xx)(xx[(E)CKI(P

.. (10)

25


26/47

k

T

kkk

T

k

T

kkkkkk K)ww(EK)CKI]()xx)(xx[(E)CKI(

kP R(measurement errorcovariance)

T

kk

T

kkkk RKK)CKI(P)CKI(P

- - - prior estimate of the Pk

.... (11)

Equation (11) is the error covariance update equation.

The diagonal of the covariance matrix contains the

mean squared errors as shown

.... (12)

The sum of the diagonal elements of a matrix is thetrace of a matrix.26


27/47

Propositions:

Let A, B square matrices, and C be a square symmetric

matrix, then:

d(trace(AB)/dA = BT

d(trace(ACAT))/dA = 2AC

The trace of a product can be rewritten as:

trace(XTY) = trace(XYT) = trace(YXT)

The trace is a linear map. That is

trace(A+B) = trace(A) + trace(B)

27


28/47

Therefore the mean squared error may be minimised by minimising

the trace of Pkwhich in turn will minimise the trace of Pkk The trace of

Pk is first differentiated with respect to Kk and the result set to zero in

order to find the conditions of this minimum:

By expanding equation (11)

T

k

T

kk

T

k

T

kkkkk K)RCCP(KKCPCPKPP

Note that the trace of a matrix is equal to the trace of its transpose,

therefore it may be written as;

]K)RCCP(K[T]CPK[T2]P[T]P[TT

k

T

kkkkkk

where; T [Pk ] is the trace of the matrix Pk

... (13)

28


29/47

Differentiating with respect to Kk gives;

)RCCP(K2)CP(T2dK

]P[dT Tkkk

k

k

.... (14)

Setting to zero and re-arranging gives;

Now solving for Kk gives;

)RCCP(K2)CP(2 TkkTk

1T

k

T

kT

k

T

k

k )RCCP(CP)RCCP(

)CP(

K

Equation (16) is the Kalman gain equation

.... (15)

.... (16)

29


30/47

If we are sure about measurements:

Measurement error covariance (R) decreases to zero

K decreases and weights residual more heavily than

prediction

Larger value of R the measurement error covariance

(indicates poorer quality of measurements)

If we are sure about prediction

Prediction error covariance P-k decreases to zero

K increases and weights prediction more heavily than

residual

30


31/47

Finally, substitution of equation (16) into (13) , and

simplifying further yields;

k1T

k

T

kkk CP)RCCP(CPPP

kk

kkk

P)CKI(

CPKP

..... (17)

Equation 17 is the update equation for the error

covariance matrix with optimal gain. The three

equations 6, 16, and 17 develop an estimate of the

variable xk. State projection is achieved using;

k1k xAx

..... (18)

31


32/47

To complete the recursion it is necessary to find an

equation which projects the error covariance matrix

into the next time interval, k + 1. This is achieved byfirst forming an expressions for the prior error:

1k1k1k xxe

kkk1k v)xx(Ax

]vv[E])Ae)(Ae[(E]ee[EPT

kk

T

kk

T

1k1k1k

Extending equation 4 to time k + 1;

QAAPP k1k

This completes the recursive filter.

Q32

Initial Estimates


34/47

34

Theoretical Basis

-k+1 = Ayk + Buk

P-k+1 = APkAT + Q

Prediction (Time Update)

(1) Project the state ahead

(2) Project the error

covariance ahead

Correction (Measurement Update)

(1) Compute the Kalman Gain

(2) Update estimate with measurt yk

(3) Update Error Covariance

k = -k + K(zk - H

-k )

K = P-k+1HT(HP-k+1H

T + R)-1

Pk+1 = (I - KH)P-k+1


36/47

1D Example

Previous belief

Predicted belief

Measurement with uncertainty

Corrected belief

36


37/47

The Kalman filter addresses the general problem of

trying to estimate the state of a discrete-time

controlled process that is governed by a linear

stochastic difference equation.

But what happens if the process to be estimated and

(or) the measurement relationship to the process isnon-linear?

37


38/47

ExtendedKalman Filter

Suppose the state-evolution andmeasurement equations are non-linear:

process noise v is drawn from N(0,Q), withcovariance matrix

Q.

measurement noise w is drawn from N(0,R),with covariance matrix R.

kkk1k v),(f uxx

kkk w)(cy x

Process model

Observation model

(a)

(b)

38


39/47

Suppose the knowledge on xk at time k is

),P,(N~ kkk xx

)P,(N~ 1k1k1k xxthen xk+1 at time k + 1 follows;

(d)

(c)

Predict using process model

The process model (a) is linearized at the current

estimate x^k using the first order Taylor series expansion;

kkkxkk1k v)xx(f),(f uxx

Where, fx is the Jacobian of function f with respect to x

evaluated at x^k. Higher orders are ignored

(e)

39


40/47

kkxkxkk1k vxfxf),(f uxx

System (e) is a linear process model

UkF

kkk1k vUFx x

Now applying the linear KF prediction formula;

)u,x(fxf)u,x(fxf......

UxF

kkkxkkkx

kk1k

x

QfPfQFPFPT

xkx

T

1k

(f)

(g)

(h)40


41/47

Update using observation

After the prediction step, the current estimate of the

state is x-k+1.The observation model (b) is linearized at the current

estimate x-k+1, using Taylor series expansion.

1k1k1k1k1k w)xx(c)(cy x

1k1k1k1k1k wcxxc)(cy x

CYk+1

1k1k1k wCxY (i)41


42/47

Now applying the linear KF update formula using the

linear observation model (i), we have;

)xCY(Kxx 1k1k1k1k

))(cy(Kx

)xcxc)(cy(Kx

1k1k1k

1k1k1k1k1k

x

x

,KSKPP T1k1k

1T

1k

1T

1k

T

1k

T

1k

ScPSCPK

RcPcRCPCS

Where S=innovation covariance and K= kalman gain given as:

42

O it ti f th EKF i d b th f ll i


43/47

One iteration of the EKF is composed by the following

consecutive steps:

43


44/47

It this important to state that the EKF is not an optimal filter, but

rathar it is implemented based on a set of approximations. Thus, the

matrices P(k|k) and P(k + 1|k) do not represent the true covariance of

the state estimates.44


45/47

Kalman filter is derived for guissian, linear system, and gives a

best estimation. It is a recursive data processing algorithm -

doesnt need to store all previous measurements and reprocess

all data each time step

Extended kalman filter is derived for non linear system, gives an

approximation of the optimal estimate. Contrary to the Kalmanfilter, the EKF may diverge, if the consecutive linearizations are

not a good approximation of the linear model in all the

associated uncertainty domain. 45


46/47

46

The Kalman filtering equations provide an estimate of the

state and its error covariance recursively. The estimate

and its quality depend on the system parameters and the

noise statistics fed as inputs to the estimator.

One problem with the Kalman filter is its numerical stability. If

the process noise covariance Qk

is small, round-off error often

causes a small positive eigenvalue to be computed as a negativenumber.

When the state transition and observation modelsthat is, the

predict and update functions and are highly non-linear, the

extended Kalman filter can give particularly poor performance.

This is because the covariance is propagated through

linearization of the underlying non-linear model
http://en.wikipedia.org/wiki/Numerical_stabilityhttp://en.wikipedia.org/wiki/Numerical_stability


47/47

1. Kalman, R. E. 1960. A New Approach to Linear Filtering and Prediction

Problems, Transaction of the ASME--Journal of Basic Engineering, pp.

35-45 (March 1960).

2. Welch, G and Bishop, G. 2006. An introduction to the Kalman Filter,

http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

3. Shoudong Huang, Understanding Extended Kalman Filter- Part III:

Extended Kalman Filter April 23,2010

http://services.eng.uts.edu.au/~sdhuang/Extended%20Kalman%20Filter

_Shoudong.pdf
http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdfhttp://services.eng.uts.edu.au/~sdhuang/Extended%20Kalman%20Filter_Shoudong.pdfhttp://services.eng.uts.edu.au/~sdhuang/Extended%20Kalman%20Filter_Shoudong.pdfhttp://services.eng.uts.edu.au/~sdhuang/Extended%20Kalman%20Filter_Shoudong.pdfhttp://services.eng.uts.edu.au/~sdhuang/Extended%20Kalman%20Filter_Shoudong.pdfhttp://services.eng.uts.edu.au/~sdhuang/Extended%20Kalman%20Filter_Shoudong.pdfhttp://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

tutorial on kalman filter

Documents