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

[ ]
−1
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 
δ 
δz  1 
x

= M 
δx  δ 
z
δ 
 xn  nx1

Likewise, for the matrix A mxn

 δz δz 
δ L 
δ a1n
δ z  11 
a

= M O M 
δA  δ δz 
z
δ L 
 am1 δ amn 
Kalman Filter
Derivation Definitions and Identities

δx T y δy T x
(1) =y=
δx δx

δx T Nx
(2) = 2Nx ( where N is symmetric)
δx

δ ( A x + b) M( A x + b)
T

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

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

δ
(5 ) trace ( ABA T ) = 2 A B (where B is symmetric)
δA
Kalman Filter
Derivation Definitions and Identities

(6) Gradient Expression

δ
δx
[ ]
(Hx − z) T W(Hx − z) = 2H T W(Hx − z) (for W symmetric )

(7) 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 −1

(8) Inversion Lemma Pr oof

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

= [ 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"

[P −1 + HT R −1 H] = P − PHT [HPH T + R] H
−1 −1
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 ] = 0∀k

E [ v k ] = 0∀k

{ }
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 Assumptions

1. 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 Substitute for K ′k+1 to
obtain state update exp ression (equation 4).

′ that min imizes var iance.

Step 2. Develop exp ression for K k+1

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

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

= K k+1
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

′ = (I - K k+1 H k+1 ) Φk+1

K 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 residual of measurement
state 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 kT+1 ]
x k +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 has Cov Pk k +1
k +1

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

Pk +1 = E ~{
x k +1 ~
x −k +T1 }
= Φ E (~
k +1 {x −x k k ) ( ~
x k − x k ) T
}Φ kT+1 + E {w k+1 w kT+1}
= Φ 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 Step 2

Thus
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 [ ~
Pk +1 = Cov ~ x T k +1 ]
x k +1 ~

= [ 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
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
Kalman Filter
Derivation Step 2

(P HTK T ) = K H P
T
U sin g the identities
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 )
−1

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

[H ]
−1
K k +1 = Pk +1 H
- T
k +1 k +1
- T
Pk +1 H
k +1
+ 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
[ ]
−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.

[H ]
−1
K k +1 = Pk +1 H
- T
k+1 k +1
-
Pk +1 HT
k+1 + 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

[( ) ]
−1 −1
Pk +1 = Pk +1-
+H
T
k +1 R −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 k−1+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.

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

[(P ) ]
MIL −1 −1
= -
k +1
+H
T
k +1 R −1
k +1 Hk +1

(H ) [(P ) ]
-1 GE −1 −1
−1
K k +1 = Pk +1 H
- T
k +1 k +1
-
Pk +1 H T
k +1 + R k +1 = -
k +1
+H T
k +1 R k +1 Hk +1 HTk +1 R −k1+1

= Pk- +1 HTk +1 R−1

k +1