kalman filter derivation - ati courses technical … · kalman filter derivation kalman filter...

Kalman FilterDerivation

Kalman FilterDerivation Overview

1. Discuss several useful matrix identities.

2. Derive Kalman filter algorithms.

3. Discuss alternate form (Alternate Gain Expression) of the Kalman filter.

Kalman FilterDerivation References

1. Applied Optimal Estimation. Edited by Arthur Gelb. M.I.T. Press 1986.

2. Introduction to Random Signals and Applied Kalman Filtering. 2ndEdition. R.G. Brown and P.Y.C. Hwang. John Wiley and Sons, Inc.New York. 1992.

Kalman FilterDerivation Kalman Filter Equations

In this section, we will derive the five Kalman filter equations

1. State extrapolation

2. Covariance Extrapolation

3. Kalman Gain Computation

4. State Update

5. Covariance Update

[ ]

[ ]

$ $

$ $ $

x x

P P Q

K P H H P H R

x x K z H x

P P K H P

-

- -

- -

- -

k k k

k k

k k k k k k k

k k k k k k

k k k k k

+ +

+ +

−

=

= +

= +

= + −

= −

1 1

+1 k k 1T

k

+1 +1 +1

T+1 +1 +1

T+1

1

+1 +1 +1 +1 +1 +1

+1 +1 +1 +1 +1

ΦΦ

ΦΦ ΦΦ1

Kalman FilterDerivation Definitions and Identities

Vector and Matrix Differentiation

Let be a scalar and be a column vector

Likewise for the matrix

z

z

x

z

xn nx

mxn

z

z

a

z

a n

z

am

z

amn

z x

A

δδδδ

δδδδ

δδδδ

δδδδ

δδδδ

δδδδ

δδδδ

δδδδ

x

A

==

==

1

1

11 1

1

M

L

M O M

L

,

Kalman FilterDerivation Definitions and Identities

(( )) (( )) (( ))

(( ))

(( ))

( )

( ( )

( )

( )

, dim dim

( ) ( )

1

2) 2

3 2 2 2

4

5 2

δδδδ

δδδδ

δδδδ

δδδδ

δδδδ

δδδδ

x yx

yy xx

x Nxx

N x N

A x b M A x bx

A M A x A M b A M A x b

AAC C

A C A C

AABA A B B

T T

T

TT T T

T

T

T

where is symmetric

trace

Note for to be square

trace where is symmetric

== ==

==

++ ++== ++ == ++

==

==

==

Kalman FilterDerivation Definitions and Identities

(( )) (( ))[[ ]] (( ))

[[ ]] [[ ]][[ ]] [[ ]]

[[ ]] [[ ]] [[ ]][[ ]]

( )

xHx z W Hx z H W Hx z ( W )

( )H H

H R HPH H H H R HPHH R HPH R P H R H PH

P H R H H R PH HPH R

( )

P H R H P H R H I H R HP H R HP

P H R H

6

2

7

8

1 1

1 1 1

1 1 1 1 1

1 1 1 1 1 1 1 1

1 1

Gradient Expression

for symmetric

Gain Expression oof

Inversion Lemma oof

T T

T T

T T T T T T

T T T T

T T T T

T T T T

T

δδδδ

−− −− == −−

==++ == ++++ == ++

++ == ++ ++

++ == ++ ++ −−

== ++

−− −−

−− −− −−

−− −− −− −− −−

−− −− −− −− −− −− −− −−

−− −−

Pr

Pr

[[ ]][[ ]]

[[ ]] [[ ]]

−− −− −− −−

−− −− −− −−

−− −− −− −−

++ −−

== −− ++

++ == −− ++

1 1 1 1

1 1 1 1

1 1 1 1

P H R H P H R HP

P P H R H H R HP

" "P H R H P PH HPH R H

T T

T T

T T T

Use identity above to obtain Inversion Lemma

Kalman FilterDerivation Assumptions

Assume the following form of the estimator

• linear

• recursive

Goal is to show that the Kalman Filter Equations provide the minimumvariance estimator over all unbiased estimators which have this form

No assumptions are made concerning the particular distribution of theprocess or measurement noise

Kalman FilterDerivation Model

Process:

Measurement:

Assumptions: [ ]

[ ]

[ ]

{ }

{ }{ }

{ }{ }

{ }

x x w

z H x

x

w

v

w w Q

v v R

x x P

w v

x w

x v

x

k k k k

k k k k

k

k

k j k kj

k j k kj

k j

k

j

k

k

k

k

j

+ += +

= +

=

= ∀

= ∀

=

=

=

= ∀

= ∀

= ∀

1 1

0 0

0 0 0

0

0

0

0

0

0

0

ΦΦ

v

cov ,

cov ,

cov ,

cov , , ,

cov , , ,

cov , ,

E

E

E

µµ

δδ

δδ

Kalman FilterDerivation Assumptions

1 1

2

.

~ $ ,cov

. , ,

Assume that at time k we have available an unbiased estimate ofthe state at time k

The error term in addition to having zero mean hasariance

At time k we have a measurement available where

k k k

k

k

k k k k

++

== −−

== ++

x x xP

z

z H x v

Kalman FilterDerivation Goal

The goal is to find unbiased minimum variance estimator of the state attime k+1, of the form

Note that this estimator is both

linear in

recursive - only processes the current measurement zk+1

$ $x K x K zk k k k k++ ++ ++ ++== ′′ ++1 1 1 1

$x zk kand ++1

Kalman FilterDerivation Derivation Steps

Step For unbiased develop ression for Substitute for toobtain state update ression equation

Step Develop ression for that imizes iance

a Define equation

Define equation

b Define equation Joseph form

c Define equation

Define equation short form

k

k

k

k

k

k

14

2

2 1

2)

2 5

2 3

5

1

1

1

1

. $ , exp .exp ( ).

. exp min var .

. $ ( )

(

. ( )

. ( )

( )

x K′′ ′′

′′

−−

−−

++

++

++

++

K

K

x

P

P

K

P

+1

k+1 k+1

k+1

Kalman FilterDerivation Step 1

[[ ]]

The unbiased criteria forces a certain relationship between andWhat we will see over the next few pages is that this criteria

requires that

For to be unbiased

k

k k k k

k k k

k

′′

′′ == −−

−− ==

++

++ ++ ++ ++

++ ++ ++

++

K

K

x x

x

k+1

k+1

KK H

x

1

1 1 1 1

1 1 1

1

0

. , ,.

$ , $

~.

ΦΦ ΦΦ

E1 24 34

Kalman FilterDerivation Step 1

Substituting for the terms in the brackets gives

Adding and subtracting two terms, and further substitution gives

Rearranging terms gives

(( ))

[[ ]] [[ ]](( )) (( ))

[[ ]]

$ $

$

$

$

x x K x x

K x H x v x K x K x

K x x H x + v - x K

K x x H

k+1 k+1

k+1

k k k k k k

k k k k

k k k k k k k k k

k k k k k

Add and Subtract

++ ++ ++ ++ ++

++ ++ ++

++ ++ ++ ++ ++

++ ++ ++

−− == ′′ ++ −−

== ′′ ++ −− ′′ ++ ′′

== ′′ ++ ++ ++ ++ ++ ′′

== ′′ ++ ++ −− ++ ′′

1 1 1 1 1

1 1 1

1 1 1 1 1

1 1 1

k+1

k+1 k+1 k+1 k+1 k+1 k+1 k

k+1 k+1

k+1

+ -

K z

K

K w w

K

1 244 344

ΦΦ ΦΦ

ΦΦ ΦΦ[[ ]] (( ))K x + w K vk k+1 k+1 kk+1 K H Ik k++ ++ −− ++1 1

Kalman FilterDerivation Step 1

The final step is to take the expectation of this expression and set it equalto zero. For the right hard side to be equal to zero, the following must betrue

which implies

or

[[ ]] [[ ]] [[ ]]

(( ))

E E k$x x K H K

K H - K

K = I - K H

k+1 k+1

k+1 k+1 k+1 k+1

k+1 k+1 k+1

k k++ ++−− == ++ ′′ ==

++ ′′ ==

′′

1 1 0

0

k+1 k+1

k+1

k+1

-ΦΦ ΦΦ

ΦΦ ΦΦ

ΦΦ

k 1+ x

Kalman FilterDerivation Step 1

Thus, to satisfy the unbiased criteria:

or equivalently

which is the state update equation (equation 4)

It remains to find the value of Kk+1 which minimizes the covariance of theestimation error

(( ))

(( ))

$ $

$ $ $

x = I -K H x K z

x = x K z H x

k+1 k+1 k+1 k+1 k k+1 k+1

k+1 k+1 k k+1 k+1 k

ΦΦ

ΦΦ ΦΦ

++

++ −− ++ ++

extrapolatedstate

k k

residual of measurementand prediction of

measurement

124 34 1 24444 344441 1

Kalman FilterDerivation Step 1

The estimation error is

The covariance of this error

The goal will be to find Kk+1 such that

is minimized (i.e., minimum variance).

~ $x x xk k k

k

n

n k

looks like

Trace

++ ++ ++

++

++

== −−

++ ++ ==

1 1 1

1

12

2

12 2

1

P

P

σσ

σσ

σσ σσ

O

L

Kalman FilterDerivation Step 2

[[ ]]

Step is to find which imizes Trace where

A First find the ariance of the extrapolated estimate error

equation

The extrapolated estimate is defined as equation

k k

k k kT

k

k k k

2

2).

1

1 1

1 1 1

1

K P

P

P-

-

++ ++

++ ++ ++

++

==

==

min ,

~ ~

. , cov ,

(

( )

$ $

E x x

X X

+1

+1ΦΦ

Kalman FilterDerivation Step 2

The extrapolated estimate error is then

To obtain equation 2, take the expected value of both sides

Thus, equation 2 is

[[ ]] {

{{ }}(( ))(( )){{ }} {{ }}

~ $ $

$ $

~ ~

~ ~

cov

x x x x x w

x - x x - x w

x x

x x x x

P P Q

k k k k k k k k

k k

hask

k k k

has Cov k

k

has Cov k

k k k

T

k k k k k

T

kT

kT

k k kT

k

k k k

we can see

+1 +1

+1

- -

-

P

w w

P Q

P P

== −− == ++ −− ++

== ++

==

== −− −− ++

== ++

==

++ ++ ++ ++

++

++

++ ++ ++

++

++ ++ ++−−

++ ++ ++

++ ++ ++

++−−

++

1 1 1 1

1

1

1 1 1

1

1 1 1

1 1 1

1 1 1

1 1

ΦΦ ΦΦ

ΦΦ

ΦΦ ΦΦ

ΦΦ ΦΦ

ΦΦ ΦΦ

1 24 34 1 24 34

E

E E k+1

kT

k++ ++++1 1Q

Kalman FilterDerivation Step 2

(( ))

[[ ]]

[[ ]] [[ ]]

B Find the ariance the ariance of the final estimation error equationIt will be a function of and

U g the identity in

Gives

k

k k

k k k k k k

k k k

k k k k k k k

k k k k k k k k

. cov ( cov , )..

sin $ $ $

~ $

~ $

$

$

PK P

K z H

I H

I H H

-

- -

-

-

++

++

++ ++ ++ ++

++ ++ ++

++ ++ ++ ++ ++ ++

++ ++ ++ ++ ++ ++ ++

== −− −−

== −−

== −− ++ −−

== −− ++ ++ −−

==

1

1

1 1 1 1

1 1 1

1 1 1 1 1 1

1 1 1 1 1 1 1

5

+1

+1 +1

+1

+1

x x x

x x x

x K x K z x

K x K x v x

x

[[ ]] [[ ]]

[[ ]][[ ]][[ ]]

k k k k k k

Tk k k k

k k k k k k k k

k k k k k k

k k k k k

+1 +1

+1 +1

+1

+1

- -

- -

-

-

H

I H

I H

−− ++ ++ −−

== −− −− −− ++

== −− −− ++

== −− −−

++ ++ ++ ++ ++ ++ ++ ++

++ ++ ++ ++ ++ ++

++ ++ ++ ++ ++

++ ++ ++ ++

K x K H x K v x

x x K H x x K v

K x x K v

K x K v

1 1 1 1 1 1 1 1

1 1 1 1 1 1

1 1 1 1 1

1 1 1 1

$ $

$

~

Kalman FilterDerivation Step 2

Thus

Taking the expected value of both sides will provide us with an expressionfor Pk+1

Now we have found the expression for the covariance update,equation 5. Note that

[[ ]]

[[ ]]

[[ ]] [[ ]]

~ $ ~

~ ~ ~

, , .

x x x x K x

x x xT

k k k k k k k k

k k k k

k k k k k

T

k k k

T

k k k k

Cov

is a function of and

++ ++ ++ ++ ++ ++ ++

++ ++ ++ ++

++ ++ ++ ++ ++ ++ ++

++ ++ ++−−

++

== −− == −− ++

== ==

== −− −− ++

1 1 1 1 1 1 1

1 1 1 1

1 1 1 1 1 1 1

1 1 1 1

I K H

P

I K H P I K H K R K

P K P R

-

-

+1

+1

E

Kalman FilterDerivation Step 2

c. The final step is to find an expression for Kk+1 (equation 3) whichminimizes the trace of Pk+1.

(( )) (( ))(( )) (( ))

(( ))

U g as shorthand notation for for for and for

Tr Tr Tr Tr Tr

k k k k

kT T

T T T

T T T T T

kT T T

sin , , ,P P K K R R H H

P I K H P I K H K R KI K H P I H K K R K

P K H P P H K K H P H K K R KP P K H P K H P H K K R K

-

+1 ++ ++ ++

++

++

== −− −− ++== −− −− ++== −− −− ++ ++

== −− ++ ++

1 1 1

1

1 2

Kalman FilterDerivation Step 2

(( ))(( )) (( ))

U g the identities

Tr Tr

Trwhere is symmetric

Tr

We obtain the partial of Tr P with request to KTr

T T T

T T

T

T

k

k T T

sin

( )

P H K K H P

P H K K H P

A B AA

A B B

A CA

C

PK

PH K H P H K

====

==

==

== −− ++ ++

++

++

δδδδ

δδδδ

δδδδ

2

2 2 2

1

1

Kalman FilterDerivation Step 2

Taking the partial with respect to K,

And setting this equal to zero and solving for K gives

which is the Kalman gain (equation 3)!

It can be verified that this is indeed a minimum (reference Gelb, pg. 109).

(( ))

δδδδ

Tr k T T

T T

PK

P H K H P H K

K P H H P H R

++

−−

== −− ++ ++

== ++

1

1

2 2 2

Kalman FilterDerivation Step 2

Now we have an expression for Kk+1 that optimizes the estimate

We can substitute Kk+1 in our Pk+1 expression

To get the Pk+1 expression

[ ]

( ) [ ]

( )

K P H H P H R

P I K H P I K H K R K

P I K H P

- -

-

-

k k k

Tk k k

Tk

k k k k k k

T

k k k

T

k k k k

+ + + + +

−

+ + + + + + + +

+ + +

= +

= − − +

= −

1 1 1 1 1

1

1 1 1 1 1 1 1 1

1 1 1

+1 +1

+1

+1

Kalman FilterDerivation Summary

Thus we have the recursive algorithm

where

[ ]

[ ][ ]

$ $ $

$ $

x x K z H x

x xk

k k k k k k

k k

k k k kT

k

k k kT

k k kT

k

k k k k

+ + + +

+

+ + +

+ + + + +

−

+ + +

= + −

== +

= += −

1 1 1 1

1

1 1 1

1 1 1 1 1

1

1 1 1

+1 +1

+1

+1

+1 +1

+1

- -

-

-

- -

-

P P Q

K P H H P H R

P I K K P

ΦΦΦΦ ΦΦ

Kalman FilterDerivation Alternate Gain Expression

The standard Kalman Filter algorithm computes the gain Kk+1, then computes theupdated covariance Pk+1 as a function of the gain.

This computation involves taking the inverse of a m x m matrix, where m = dim z

Usually dim z < dim x (size measurement vector smaller than number of states), sothis formulation is desirable.

[ ][ ]

K P H H P H R

P I K H P

- -

-

k k k k k

k k k k

+ + +

−

+ + +

= +

= −

1 1 1

1

1 1 1

+1 k+1T

+1 k+1T

+1

Kalman FilterDerivation Alternate Gain Expression

Another formulation exists which involves reversing this, i.e. computingthe updated covariance Pk+1 first, and finding Kk+1 as a function of Pk+1.

This form is

Note that this involves computing the inverse of n dimensional matrices,where n = dim x, the state (in addition to )

There are situations where this would be computationally preferable - ifdim x < dim z and Rk+1 is of a simple form (I, diagonal, etc.)

Rk++−−

11

( )[ ]P P H R H

K P H R

-k k k

Tk k

k k kT

k

+

−

+ +−

+

−

+ + + +−

= +

=

1

1

1 11

1

1

1 1 1 11

+1

Kalman FilterDerivation Alternate Gain Expression

Derivation of the Alternate formulation follows directly from the MatrixInversion Lemma (MIL) and Gain Expression (GE) Identity given earlier.

( ) ( )

( )[ ]

( ) ( )[ ]

P I K H P P -P H H P H R H P

P H R H

K P H H P H R P H R H H R

P H R

- - - - -1 -

-

- - -1 -

-

k k k k k k kT

k k kT

k k k

k kT

k k

k k kT

k k kT

k k kT

k k kT

k

k kT

k

MIL

GE

+ + + + + + + +

−

+ +−

+

−

+ + + + +

−

+ +−

+

−

+ +−

+ +

= − = +

= +

= + = +

=

1 1 1 1 1 1 1 1

1

1 11

1

1

1 1 1 1 1

1

1 11

1

1

1 11

1 1

+1 +1 +1 +1 +1

+1

+1 +1 +1

+1

−1