Kalman Filter

Derivation

Kalman Filter
Derivation

Overview

1. Discuss several useful matrix identities.

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

Kalman Filter
Derivation

References

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

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

Kalman Filter
Derivation

Kalman Filter Equations

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

1. State extrapolation
x$ k +1 = k +1 x$ k

2. Covariance Extrapolation
Pk- +1 = k +1 Pk kT+1 + Qk

3. Kalman Gain Computation

K k +1 = Pk- +1 HTk +1 Hk +1 Pk- +1 HTk +1 + R k +1

4. State Update

x$ k +1 = x$ -k +1 + K k +1 z k +1 H k +1x$ -k +1

5. Covariance Update
Pk +1 = Pk- +1 K k +1 Hk +1 Pk- +1

Kalman Filter
Derivation

Definitions and Identities

Vector and Matrix Differentiation

Let z be a scalar and x be a column vector
z

x
z 1
= M
x
z

xn

nx1

Likewise, for the matrix A mxn

z
z
L

a1n
a
z 11
= M O M
A
z
z
L

amn
am1

Kalman Filter
Derivation

Definitions and Identities

(1)

x T y
y T x
=y=
x
x

(2)

x T Nx
= 2Nx
x

(3)

( A x + b) M( A x + b)
= 2 A TM A x + 2 A T M b = 2 A T M ( A x + b)
x

( where N is symmetric)

(4 )

(5 )

trace ( AC) = C T
A
Note for A C to be square, dim A = dim C T

trace ( ABA T ) = 2 A B
A

(where B is symmetric)

Kalman Filter
Derivation

(6)

(7)

Definitions and Identities

Gradient Expression

(Hx z) T W(Hx z) = 2H T W(Hx z)

x

Gain Expression Pr oof

HT = HT
H T R 1 HP H T + HT = H T + H TR 1 HP H T
H T R 1 [HP HT + R] = [P 1 + H TR 1 H] P HT

[P 1 + HT R 1 H] HTR 1 = PHT + [HPHT + R]

1

(8)

(for W symmetric )

Inversion Lemma Pr oof

1
1
[P 1 + HT R 1 H] = [P 1 + HTR 1H] [I + HTR 1HP HTR 1HP]
= [ P 1 + H T R 1 H] [P 1 + H TR 1H]P H TR 1HP
1

= P [ P 1 + H T R 1 H] H TR 1HP
1

Use identity above to obtain " Inversion Lemma"

1
1
[P 1 + HT R 1 H] = P PHT [HPH T + R] H

Kalman Filter
Derivation

Assumptions

Assume the following form of the estimator

linear
recursive
Goal is to show that the Kalman Filter Equations provide the minimum
variance estimator over all unbiased estimators which have this form

No assumptions are made concerning the particular distribution of the

process or measurement noise

Kalman Filter
Derivation

Model

Process:

x k +1 = k +1 x k + w k

Measurement:

zk = Hkxk + vk

Assumptions:

E [ x 0 ] = 0x
E [ w k ] = 0k
E [ v k ] = 0k

cov w k , w j = Q k kj

cov v k , v j = R k kj
cov{ x 0 , x 0 } = P0

cov w k , v j = 0 , k ,
cov{ x 0 , w k } = 0 , k ,

cov x 0 , v j = 0 , j

Kalman Filter
Derivation

1.

Assumptions

Assume that at time k + 1 we have available an unbiased estimate of

the state at time k
The error term ~
x k = x$ k x k in addition to having zero mean, has
cov ariance Pk

2.

At time k, we have a measurement zk available, where

zk = Hk x k + v k

Kalman Filter
Derivation

Goal

The goal is to find unbiased minimum variance estimator of the state at

time k+1, of the form
x$ k +1 = K k +1x$ k + K k +1zk + 1

Note that this estimator is both

linear in x$ k and zk +1
recursive - only processes the current measurement zk+1

Kalman Filter
Derivation

Derivation Steps

Step 1. For unbiased x$ k , develop exp ression for K k+1

.
obtain state update exp ression (equation 4).

Substitute for K k+1 to

Step 2. Develop exp ression for K k+1

that min imizes var iance.
2a. Define x$ k +1 (equation 1)
Define Pk +1 (equation 2)
2b. Define Pk +1 (equation 5 Joseph form)
2c. Define K k+1 (equation 3)
Define Pk +1 (equation 5 short form)

Kalman Filter
Derivation

Step 1

The unbiased criteria forces a certain relationship between K k+1 and

K k +1. What we will see, over the next few pages, is that this criteria
requires that K k+1 = k +1 K k +1 Hk +1 k +1.
For x$ k +1 to be unbiased, E [ x$ k +1 x k +1 ] = 0.
14243
~
x
k +1

Kalman Filter
Derivation

Step 1

Substituting for the terms in the brackets gives

x$ k +1 x k +1 = K k+1 x$ k + K k +1 z k +1 x k +1

Adding and subtracting two terms, and further substitution gives

= K k+1x$ k +1 + K k +1 (Hk+1 x k+1 + v k+1 ) - x k +1 K k+1
x + K k+1x k
144k244
3
Add and Subtract

Rearranging terms gives

= K k+1[ x$ k + x k ] + K k +1(Hk+1 [ k +1 x k + w k +1 ] + v k+1 ) - ( k +1 x k + w k +1 ) + K k+1
= K k+1
[ x$ k + x k ] + [K k +1 Hk+1 k +1 k +1 + K k+1 ] x k + (K k +1 Hk +1 I) w k+1 + K k+1v k

Kalman Filter
Derivation

Step 1

The final step is to take the expectation of this expression and set it equal
to zero. For the right hard side to be equal to zero, the following must be
true
E [ x$ k +1 x k +1 ] = [K k+1 Hk+1 k +1 - k+1 + K k+1
] E[ x k ] = 0

which implies
K k+1 Hk+1 k+1 - k+1 + K k+1
=0

or
K k+1
= (I - K k+1 H k+1 ) k+1

Kalman Filter
Derivation

Step 1

Thus, to satisfy the unbiased criteria:

x$ k+1 = (I - K k+1 Hk+1 ) k+1 x$ k + K k+1 z k+1

or equivalently
x$ k+1 = k+1 x$ k + K k+1 ( z k+1 H k +1 k +1 x$ k )
12
4 4
3 14444244443
extrapolated
state

residual of measurement
and prediction of
measurement

which is the state update equation (equation 4)

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

Kalman Filter
Derivation

Step 1

The estimation error is

~
x k +1 = x$ k +1 x k +1

The covariance of this error

12

Pk +1 looks like O

n2

The goal will be to find Kk+1 such that

12 + L + n2 = Trace Pk +1

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

Kalman Filter
Derivation

Step 2

Step 2 is to find K k +1 which min imizes Trace Pk +1, where

Pk +1 = E [ ~
x k +1 ~
x kT+1 ]
A. First, find the cov ariance of the extrapolated estimate error,
Pk-+1 (equation 2).
The extrapolated estimate is defined as (equation 1)
X$ -k+1 = k +1 X$ k

Kalman Filter
Derivation

Step 2

The extrapolated estimate error is then

~
x -k+1 = x$ -k+1 x k +1 = k +1 + x$ k k +1 x k + w k +1
we can see x$ -k+1 - x k +1 = k +1 [ x$ k +1 - x k ] + w
k +1
{
1424
3
1424
3
has Cov Q
has cov P
k +1

has Cov Pk

k +1

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

x k +1 ~
x k +T1
Pk +1 = E ~
= E (~
x x
k +1

T
~
x

x
)
(
)
} kT+1 + E {w k+1 w kT+1}
k
k
k

= k +1 Pk kT+1 + Q k +1

Thus, equation 2 is

Pk+1 = k +1 Pk kT+1 + Q k +1

Kalman Filter
Derivation

Step 2

B. Find the cov ariance Pk +1 (the cov ariance of the final estimation error, equation 5).
It will be a function of K k +1 and Pk- +1 .

U sin g the identity x$ k +1 = x$ -k+1 K k +1 z k +1 Hk +1x$ -k+1 in

~
x k +1 = x$ k +1 x k +1
Gives
~
x k +1 = [ I K k +1Hk +1 ] x$ -k+1 + K k +1 z k +1 x k +1
= [ I K k +1Hk +1 ] x$ -k+1 + K k +1 [ Hk +1 x k +1 + v k +1 ] x k +1
= x$ -k+1 K k +1Hk +1 x -k+1 + K k +1 H Tk +1 x k +1 + K k +1v k +1 x k +1

= x$ -k+1 x k +1 K k +1Hk +1 x$ -k+1 x k +1 + K k +1v k +1

= [ I K k +1Hk +1 ] x$ -k+1 x k +1 + K k +1 v k +1
= [ I K k +1Hk +1 ] ~
x -k+1 K k +1 v k +1

Kalman Filter
Derivation

Thus

Step 2

~
x k +1 = x$ k +1 x k +1 = [ I K k +1 Hk +1 ] ~
x -k+1 + K k +1 x k +1

Taking the expected value of both sides will provide us with an expression
for Pk+1
x k +1 = E [ ~
x k +1 ~
x T k +1 ]
Pk +1 = Cov ~

= [ I K k +1 Hk +1 ]Pk- +1 [ I K k +1 Hk +1 ] + K k +1 R k +1 K Tk +1
T

Now we have found the expression for the covariance update,

equation 5. Note that Pk +1 is a function of K k +1, Pk+1, and R k +1.

Kalman Filter
Derivation

Step 2

c. The final step is to find an expression for Kk+1 (equation 3) which

minimizes the trace of Pk+1.
U sin g P as shorthand notation for Pk- +1 , K for K k +1, R for R k +1, and H for Hk +1
Pk +1 = (I K H) P (I K H) + K R K T
= (I K H) P (I H T K T ) + K R K T
= P K H P P HTK T + K H P H T K T + K R K T
Tr Pk +1 = Tr P 2 Tr K H P + Tr K(H P H T )K T + Tr K R K T
T

Kalman Filter
Derivation

U sin g the identities

Step 2

(P HTK T ) = K H P
T

Tr (P H TK T ) = Tr (K H P)

Tr A B A T
= 2 A B where B is symmetric
A
Tr A C
= CT
A
We obtain the partial of Tr (Pk +1 )with request to K
Tr Pk +1
= 2 P HT + 2 K H P HT + 2 K
K

Kalman Filter
Derivation

Step 2

Taking the partial with respect to K,

Tr Pk + 1
= 2 P H T + 2 K H P H T + 2 K
K

And setting this equal to zero and solving for K gives

K = P H T (H P H T + R )

which is the Kalman gain (equation 3)!

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

Kalman Filter
Derivation

Step 2

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

K k +1 = Pk +1 H
-

T
k +1

[H

k +1

T
k +1

Pk +1 H

+ R k +1

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

Pk +1 = (I K k +1 Hk +1 ) Pk- +1 [ I K k +1Hk +1 ] + K k +1 R k +1 K Tk +1
T

To get the Pk+1 expression

Pk +1 = (I K k +1 Hk +1 ) Pk- +1

Kalman Filter
Derivation

Summary

Thus we have the recursive algorithm

x$ k +1 = x$ -k +1 + K k +1 z k +1 Hk +1 x$ -k +1

where
x$ -k +1 = k +1 x$ k
Pk- +1 = k +1 Pk kT+1 + Q k +1

K k +1 = Pk- +1 HTk +1 Hk +1 Pk- +1 HTk +1 + R k +1

Pk +1 = [ I K k +1 K k +1 ]Pk- +1

Kalman Filter
Derivation

Alternate Gain Expression

The standard Kalman Filter algorithm computes the gain Kk+1, then computes the
updated covariance Pk+1 as a function of the gain.
K k +1 = Pk +1 H
-

[H

T
k+1

k +1

T
k+1

Pk +1 H

+ R k +1

Pk +1 = [ I K k +1 Hk +1 ] Pk- +1

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), so
this formulation is desirable.

Kalman Filter
Derivation

Alternate Gain Expression

Another formulation exists which involves reversing this, i.e. computing

the updated covariance Pk+1 first, and finding Kk+1 as a function of Pk+1.
This form is

[(

Pk +1 = Pk +1
-

+H

T
k +1

1
k +1

H k +1

K k +1 = Pk +1 HTk +1 R k1+1

Note that this involves computing the inverse of n dimensional matrices,

where n = dim x, the state (in addition to R k1+1 )
There are situations where this would be computationally preferable - if
dim x < dim z and Rk+1 is of a simple form (I, diagonal, etc.)

Kalman Filter
Derivation

Alternate Gain Expression

Derivation of the Alternate formulation follows directly from the Matrix

Inversion Lemma (MIL) and Gain Expression (GE) Identity given earlier.

(H

Pk +1 = (I K k +1 Hk +1 )Pk +1 = Pk +1 - Pk +1 H
-

[(P

T
k +1

(H

MIL

K k +1 = Pk +1 H
-

k +1

k +1

+H

T
k +1

T
k +1

Pk +1 H

= Pk- +1 HTk +1 R1
k +1

1
k +1

T
k +1

Hk +1

k +1

Pk +1 H

T
k +1

+ R k +1

-1

Hk +1 Pk- +1

+ R k +1

-1

GE

[(P )
-

k +1

+H

T
k +1

1
k +1

Hk +1

HTk +1 R k1+1