Introduction to Kalman Filtering

A set of two lectures

Maria Isabel Ribeiro
Associate Professor
Instituto Superior Tcnico / Instituto de Sistemas e
Robtica
June 2000
All rights reserved
2
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
INTRODUCTION TO KALMAN FILTERING
What is a Kalman Filter ?
Introduction to the Concept
Which is the best estimate ?
Basic Assumptions
Discrete Kalman Filter
Problem Formulation
From the Assumptions to the Problem Solution
Towards the Solution
Filter dynamics
Prediction cycle
Filtering cycle
Summary
Properties of the Discrete KF
A simple example
The meaning of the error covariance matrix
The Extended Kalman Filter
3
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
WHAT IS A KALMAN FILTER?
Optimal Recursive Data Processing Algorithm
Measuring
devices
System
Kalman filter
System state
(desired but
not know)
Measurement
error sources
Controls
System error
sources
Observed
measurements
Optimal estimate of
system state
Typical Kalman filter application
4
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
WHAT IS A KALMAN FILTER?
Introduction to the Concept
Optimal Optimal Recursive Data Processing Algorithm
Dependent upon the criteria criteria chosen to evaluate performance
Under certain assumptions assumptions, KF is optimal optimal with respect to virtually any
criteria that makes sense.
KF incorporates all available information all available information
knowledge of the system and measurement device dynamics
statistical description of the system noises, measurement errors, and
uncertainty in the dynamics models
any available information about initial conditions of the variables of interest
5
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
WHAT IS A KALMAN FILTER?
Introduction to the concept
Optimal Optimal Recursive Data Processing Algorithm
)) 1 k ( v ), 1 k ( x ( h ) 1 k ( z
)) k ( w ), k ( u ), k ( x ( f ) 1 k ( x
+ + +
+
x - state
f - system dynamics
h - measurement function
u - controls
w - system error sources
v - measurement error sources
z - observed measurements
Given Given
f, h, noise characterization, initial conditons
z(0), z(1), z(2), , z(k)
Obtain Obtain
the best best estimate of x(k)
6
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
WHAT IS A KALMAN FILTER?
Introduction to the concept
Optimal Recursive Recursive Data Processing Algorithm
the KF does not require all previous data to be kept in storage and
reprocessed every time a new measurement is taken.
z(k) ..... z(2) z(1) ) 0 ( z
) k ( x

KF KF
1) z(k z(k) ..... z(2) z(1) ) 0 ( z +
) 1 k ( x

+
KF KF
) k ( x

KF KF
To evaluate the
KF only requires
and z(k+1)
) 1 k ( x

+
) k ( x

7
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
WHAT IS A KALMAN FILTER?
Introduction to the concept
Optimal Recursive Data Processing Data Processing Algorithm
The KF is a data processing algorithm
The KF is a computer program runing in a central processor
8
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
WHAT IS THE KALMAN FILTER ?
Which is the best estimate?
Any type of filter tries to obtain an optimal estimate of desired
quantities from data provided by a noisy environment.
Best = minimizing errors in some respect.
Bayesian viewpoint - the filter propagates the conditional probability
density of the desired quantities, conditioned on the knowledge of
the actual data coming from measuring devices
Why base the state estimation on the conditional probability density
function ?
9
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
WHAT IS A KALMAN FILTER?
Which is the best estimate?
Example
x(i) one dimensional position of a vehicle at time instant i
z(j) two dimensional vector describing the measurements of position
at time j by two separate radars
If z(1)=z
1
, z(2)=z
2
, ., z(j)=z
j
represents all the information we have on x(i) based (conditioned) on
the measurements acquired up to time i
given the value of all measurements taken up time i, this conditional pdf
indicates what the probability would be of x(i) assuming any particular
value or range of values.
) z ,...., z , z | x ( p
i 2 1 ) i ( z ),..., 2 ( z ), 1 ( z | ) i ( x
10
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
WHAT IS A KALMAN FILTER?
Which is the best estimate?
The shape of conveys the
amount of certainty we have in the knowledge of the value x.
) z ,...., z , z | x ( p
i 2 1
x
) z ,...., z , z | x ( p
i 2 1 ) i ( z ),..., 2 ( z ), 1 ( z | ) i ( x
Based on this conditional pdf, the estimate can be:
the mean - the center of probability mass (MMSE)
the mode - the value of x that has the highest probability (MAP)
the median - the value of x such that half the probability weight lies to
the left and half to the right of it.
11
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
WHAT IS THE KALMAN FILTER ?
Basic Assumptions
The Kalman Filter performs the conditional probability density
propagation
for systems that can be described through a LINEAR LINEAR model
in which system and measurement noises are WHITE WHITE and GAUSSIAN GAUSSIAN
Under these assumptions,
the conditional pdf is Gaussian
mean=mode=median
there is a unique best estimate of the state
the KF is the best filter among all the possible filter types
What happens if these assumptions are relaxed?
Is the KF still an optimal filter? In which class of filters?
12
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Problem Formulation
k k k k
k k k k k k k
v x C z
k , w G u B x A x
+
+ +
+
0
1
MOTIVATION
Given a discrete-time, linear, time-varying plant
with random initial state
driven by white plant noise
Given noisy measurements of linear combinations of the plant state
variables
Determine the best estimate of the system state variable
STATE DYNAMICS AND MEASUREMENT EQUATION
13
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Problem Formulation
VARIABLE DEFINITIONS
process) white - non c (stochasti vector state R x
n
k

sequence input tic determinis R u
m
k

mean) zero with (assumed
noise system Gaussian white R w
n
k

mean) zero with (assumed
noise t measuremen Gaussian white R v
r
k

sequence) white - non c (stochasti vector t measuremen R z
r
k

14
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Problem Formulation
INITIAL CONDITIONS
x
0
is a Gaussian random vector, with
mean
covariance matrix
STATE AND MEASUREMENT NOISE
zero mean E[w
k
]=E[v
k
]=0
{w
k
}, {v
k
} - white Gaussian sequences
0 0
x ] x [ E
0 P P ] ) x x )( x x [( E
T
0
0
T
0 0 0 0

1
]
1

1
1
]
1

,
_

,
_

k
k T
k
T
k
k
k
R
Q
v w
v
w
E
0
0
x(0), w
k
and v
j
are independent for all k and j
15
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Problem Formulation
DEFINITION OF FILTERING PROBLEM
Let k denote present value of time
Given the sequence of past inputs
Given the sequence of past measurements
Evaluate the best estimate of the state x(k)
} u ,... u , u { U
1 k 1 0
1 k
0

} z ,... z , z { Z
k 2 1
k
1

16
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Problem Formulation
Given x
0
Nature apply w
0
We apply u
0
The system moves to state x
1
We make a measurement z
1
Question: which is the best estimate of x
1
?
Nature apply w
1
We apply u
1
The system moves to state x
2
We make a measurement z
2
Question: which is the best estimate of x
2
?
.
.
.
) U , Z | x ( p
0
0
1
1
1
Answer: obtained from
) U , Z | x ( p
1
0
2
1
2
Answer: obtained from
k k k k
k k k k k k k
v x C z
k , w G u B x A x
+
+ +
+
0
1
17
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Problem Formulation
.
.
.
Question: which is the best estimate of x
k-1
?
Nature apply w
k-1
We apply u
k-1
The system moves to state x
k
We make a measurement z
k
Question: which is the best estimate of x
k
?
.
.
.
) U , Z | x ( p
2 k
0
1 k
1
1 k

) U , Z | x ( p
k k
k
1
0 1

Answer: obtained from

Answer: obtained from
18
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Towards the Solution
The filter has to propagate the conditional probability density
functions
) U , Z | x ( p
0
0
1
1
1
) U , Z | x ( p
1
0
2
1
2
) U , Z | x ( p
k k
k
2
0
1
1
1

) U , Z | x ( p
k k
k
1
0 1

) x ( p
0
.
.
.
) 1 | 1 ( x

) 2 | 2 ( x

) 1 k | 1 k ( x

) k | k ( x

.
.
.
.
.
.
.
.
.
19
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
From the Assumptions to the Problem Solution
The LINEARITY LINEARITY of
the system state equation
the system observation
equation
The GAUSSIAN GAUSSIAN nature of
the initial state, x
0
the system white white noise, w
k
the measurement white white noise,
v
k
) U , Z | x ( p
k k
k
1
0 1

is Gaussian
Uniquely characterized by
the conditional mean
the conditional covariance
] U , Z | x [ E ) k | k ( x

k k
k
1
0 1

] U , Z | x ; x cov[ ) k | k ( P
k k
k k
1
1 1

)) k | k ( P ), k | k ( x

( ~ ) U , Z | x ( p
1 k
0
k
0
k

20
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Towards the Solution
As the conditional probability density functions are Gaussian, the
Kalman filter only propagates the first two moments only propagates the first two moments
) U , Z | x ( p
0
0
1
1
1
) U , Z | x ( p
1
0
2
1
2
) U , Z | x ( p
k k
k
2
0
1
1
1

) U , Z | x ( p
k k
k
1
0 1

) x ( p
0
.
.
.
) | ( x

] U , Z | x [ E 1 1
0
0
1
1
1

.
.
.
.
.
.
.
.
.
) x ( p
0
) | ( x

] U , Z | x [ E 2 2
1
0
2
1
2

) k | k ( x

] U , Z | x [ E
k k
k
1 1
2
0
1
1
1

) k | k ( x

] U , Z | x [ E
k k
k

1
0 1
) 1 | 1 ( P
) 2 | 2 ( P
) 1 k | 1 k ( P
) k | k ( P
.
.
.
.
.
.
21
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Towards the Solution
We stated that the state estimate equals the conditional mean
] U , Z | x [ E ) k | k ( x

k k
k
1
0 1

Why?
Why not the mode of ?
Why not the median of ?
) U , Z | x ( p
k k
k
1
0 1

) U , Z | x ( p
k k
k
1
0 1

As is Gaussian
mean = mode = median
) U , Z | x ( p
k k
k
1
0 1

22
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Filter dynamics
KF dynamics is recursive
) U , Z | x ( p
k k
k
1
0 1

) U , Z | x ( p
k k
k
0
1
1
1
+
+
) U , Z | x ( p
k k
k
0 1
1 +
} u ,..., u , u { U
} z ,..., z , z { Z
k
k
k
k
1 1 0
1
0
2 1
1

} u , U { U
} z ,..., z , z { Z
k
k k
k
k
1
0 0
2 1
1

} u , U { U
} z , Z { Z
k
k k
k
k k
1
0 0
1 1
1
1

+
+

Prediction cycle Prediction cycle Filtering cycle Filtering cycle

What can you say about
x
k+1
before we make the
measurement z
k+1
How can we improve
our information on x
k+1
after we make the
measurement z
k+1
23
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Filter dynamics
) x ( p
0
) U | x ( p
0
0
1
prediction
) U , Z | x ( p
1
0
1
1
2
prediction
) U , Z | x ( p
0
0
1
1
1
filtering
) U , Z | x ( p
1
0
2
1
2
filtering
) U , Z | x ( p
k k
k
1
0 1

) U , Z | x ( p
k k
k
0 1
1 +
) U , Z | x ( p
k k
k
0
1
1
1
+
+
) U , Z | x ( p
k k
k
1
0
2
1
2
+ +
+
) U , Z | x ( p
k k
k
1
0 1
2
+
+
prediction
prediction
filtering
filtering
24
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Filter dynamics - Prediction cycle
Prediction cycle
) U , Z | x ( p
k k
k
1
0 1

)) k | k ( P ), k | k ( x

( ~ ) U , Z | x ( p
k k
k

1
0 1
assumed known
) U , Z | x ( p
k k
k
0 1
1 +
?
Is Gaussian
? ] U , Z | x ; cov[x k) | 1 P(k
? ) U , Z | x ( E ) k | k ( x

k k
k 1 k
k k
k
0 1
1
0 1
1
1
+ +
+
+
+
] U , Z | w [ E G ] U , Z | u [ E B ] U , Z | x [ E A ] U , Z | x [ E
w G u B x A x
k k
k k
k k
k k
k k
k k
k k
k
k k k k k k k
0 1 0 1 0 1 0 1
1
1
+ +
+ +
+
+
k k k
u B ) k | k ( x

A ) k | k ( x

+ +1
25
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Filter dynamics - Prediction cycle
Prediction cycle
] U , Z | x ; cov[x k) | 1 P(k
k k
k 1 k
0 1
1 + +
+
] ) y y )( y y [( E ] y ; y cov[
T

) k | k ( x

x ) k | k ( x
~
k
1 1
1
+ +
+
] U , Z | ) k | k ( x
~
) k | k ( x
~
[ E k) | 1 P(k
k k T
0 1
1 1 + + +
prediction
error
) u B ) k | k ( x

A ( w G u B x A ) k | k ( x

) k ( x
k k k k k k k k k
+ + + + + 1 1
k k k
w G ) k | k ( x
~
A ) k | k ( x
~
+ +1
T
k
k k
T
k
k
G Q G k)A | P(k A k) | 1 P(k + +
26
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Filter dynamics - Filtering cycle
Filtering cycle
+
1 + k
z
) U , Z | x ( p
k k
k
0
1
1
1
+
+
?
1 Passo
Measurement prediction
What can you say about z
k+1
before we
make the measurement z
k+1
) U , Z | x ( p
k k
k
0 1
1 +
)) k | k ( P ), k | k ( x

( 1 1 + +
) U , Z | z ( p
k k
k
0 1
1 +
) U , Z | v x C ( p
k k
k k k
0 1
1 1 1 + + +
+
) k | k ( x

C ) k | k ( z

] U , Z | z [ E
k
k k
k
1 1
1
0 1
1
+ +
+ +
1
1
1
0 1
1 1
1 1
+
+
+ + +
+ + +
k
T
k
k z
k k
k k
R C ) k | k ( P C ) k | k ( P ] U , Z | z ; z cov[
27
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Filter dynamics - Filtering cycle
Filtering cycle
2 Passo ) U , Z | x ( p
k k
k
0 1
1 +
] U , z , Z | x [ E ] U , Z | x [ E
k
k
k
k
k k
k
0
1
1
1
0
1
1
1 + +
+
+

x
m ] z | x [ E ] y | x [ E ] z , y | x [ E +
If x, y and z are jointly Gaussian and y and z are
statistically independent
Required result
] U ), k | k ( z
~
, Z | x [ E ] U , Z | x [ E
k k
k
k k
k
0 1
1
0
1
1
1
1 +
+
+
+
)} k | k ( z
~
, Z { e Z
k k
1
1
1
1
+
+
So equivalentes do ponto de
vista de infirmao contida
28
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Filter dynamics - Filtering cycle
Filtering cycle
)) k | k ( x

C z ( R C ) k | k ( P C C ) k | k ( P ) k | k ( x

) k | k ( x

k k k
T
k
k
T
k
1 1 1 1 1 1
1 1
1
1
1
1
1
+
1
]
1

+ + + + + + +
+ +

+
+
+
+
) k | k ( z

1 +
) k ( K 1 +
Kalman Gain
)) k | k ( x

C z )( k ( K ) k | k ( x

) k | k ( x

k k
1 1 1 1 1
1 1
+ + + + + +
+ +
) k | k ( P C R C ) k | k ( P C C ) k | k ( P ) k | k ( P ) k | k ( P
k k
T
k
k
T
k
1 1 1 1 1 1
1
1
1
1
1
1
+
1
]
1

+ + + + + +
+

+
+
+
+
measurement
prediction
) k | k ( z
~
1 +
29
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Dynamics
Linear System
Discrete Kalman Filter
k k k k
k k k k k k k
v x C z
k , w G u B x A x
+
+ +
+
0
1
k k k
u B ) k | k ( x

A ) k | k ( x

+ +1
T
k
k k
T
k
k
G Q G k)A | P(k A k) | 1 P(k + +
)) k | k ( x

C z )( k ( K ) k | k ( x

) k | k ( x

k k
1 1 1 1 1
1 1
+ + + + + +
+ +
) k | k ( P C R C ) k | k ( P C C ) k | k ( P ) k | k ( P ) k | k ( P
k k
T
k
k
T
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

+ + + +
k
T
k
k
T
k
R C ) k | k ( P C C ) k | k ( P ) k ( K
prediction
filtering
Initial
conditions
0
0 0 x ) | ( x

0
0 0 P ) | ( P
30
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Properties
The Discrete KF is a time-varying linear system
even when the system is time-invariant and has stationary noise
the Kalman gain is not constant
Does the Kalman gain matrix converges to a constant matrix? In
which conditions?
k k k k k | k k k k k | k
u B z K x

A ) C K I ( x

+ +
+ + + + + + 1 1 1 1 1 1
k k k k | k k k | k
Bu z K x

A ) C K I ( x

+ +
+ + + + + 1 1 1 1 1
31
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Properties
The state estimate is a linear function of the measurements
k k k k k | k k k k k | k
u B z K x

A ) C K I ( x

+ +
+ + + + + + 1 1 1 1 1 1
KF dyamics in terms of the filtering estimate
Assuming null inputs for the sake of
simplicity
k

3 3 2 2 2 1 1 1 2 0 0 0 1 2 3 3
2 2 1 1 1 0 0 0 1 2 2
1 1 0 0 0 1 1
0 0 0
z K z K z K x

z K z K x

z K x

x x

| |
| |
| |
|
+ + +
+ +
+

32
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Properties
Innovation process
z(k+1) carries information on x(k+1) that was not available on
this new information is represented by r(k+1) - innovation process
Properties of the innovation process
the innovations r(k) are orthogonal to z(i)
the innovations are uncorrelated/white noise
this test can be used to acess if the filter is operating correctly
) k | k ( x

C z r
k k k
1
1 1 1
+
+ + +
? ) U , Z | x ( E ) k | k ( x

k k
k
0 1
1
1
+
+
k
Z
1
1 2 1 0 k ,..., , i , )] i ( z ) k ( r [ E
T
k i , )] i ( r ) k ( r [ E
T
0
33
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Properties
Covariance matrix of the innovation process
1
1
1
1 1
+
+
+
+ + +
k
T
k
k
R C ) K | K ( P C ) k ( S
1
1
1
1
1
1 1 1

+
+
+
+
1
]
1

+ + + +
k
T
k
k
T
k
R C ) k | k ( P C C ) k | k ( P ) k ( K
1
1 1
1 1

+ +
+ +
k
T
k
S C ) k | k ( P ) k ( K
34
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Properties
The Discrete KF provides an unbiased estimate of the state
is an unbiased estimate of the state x(k+1), providing that the
initial conditions are
Is this still true if the filter initial conditions are not the specified ?
1 1 + + k | k
x

0
0 0 x ) | ( x

0
0 0 P ) | ( P
35
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Steady state Kalman Filter
Time invariant system and stationay white system and observation
noise
Filter dynamics
k k k
k k k
v Cx z
k , Gw Ax x
+
+
+
0
1
R ] v v [ E
Q ] w w [ E
T
k
k
T
k
k

T T T T T
GQG A ) k | k ( CP ] R C ) k | k ( CP [ C ) k | k ( AP 1)A - k | P(k A k) | 1 P(k + + +

1 1 1
1
)) k | k ( x

C z )( k ( K ) k | k ( x

A ) k | k ( x

k
1 1 1 1 1
1
+ + + + + +
+
Discrete Riccati Equation
) k ( K
36
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Steady state Kalman Filter
If Q is positive definite, is controllable, and (A,C) is
observable, then
the steady state Kalman filter exists
the limit exists
is the unique, finite positive-semidefinite solution to the algebraic
equation
is independent of provided that
the steady-state Kalman filter is assymptotically unbiased
) Q G , A (

+ P ) k | k ( P lim
k
1

P
T T - T - T - T - -
GQG A P C ] R C P C [ C P A A P A P + +

1
0
P

P
0
0
P
1

+ ] R C P C [ C P K
T - T -
37
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
MEANING OF THE COVARIANCE MATRIX
Generals on Gaussian pdf
Let z be a Gaussian random vector of dimension n
P - covariance matrix - symetric, positive defined
Probability density function
[ ] ( )( ) [ ] P m z m z E , m z E
T

( )
( )
1
]
1

) m z ( P m z exp
P det
) z ( p
T
n
1
2
1
2
1
n=1
n=2
38
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
MEANING OF THE COVARIANCE MATRIX
Generals on Gaussian pdf
Locus of points where the fdp is greater or equal than a given
threshold
n=1 line segment n=2 ellipse and inner points
n=3 3D ellipsoid and inner points n>3 hiperellipsoid and inner points
If
the ellipsoid axis are aligned with the axis of the referencial where the
vector z is defined
length of the ellipse semi-axis =
) , , , ( diag P
n
2 2
2
2
1
!
K ) m z ( P ) m z (
1 T


K
i

1
K
) m z (
K ) m z ( P ) m z (
n
1 i
2
i
2
i i
1 T

39
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
MEANING OF THE COVARIANCE MATRIX
Generals on Gaussian pdf - Error elipsoid
Example
n=2
1
]
1

2
2 12
12
2
1
P
1
]
1

2
2
2
1
0
0
P
40
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
MEANING OF THE COVARIANCE MATRIX
Generals on Gaussian pdf -Error ellipsoid and axis orientation
Error ellipsoid
P=P
T
- to distinct eigenvalues correspond orthogonal eigenvectors
Assuming that P is diagonalizable
Error ellipoid (after coordinate transformation)
At the new coordinate system, the ellipsoid axis are aligned with the
axis of the new referencial
K ) m z ( P ) m z (
z
1 T
z


with
1
TDT P

) , , , ( diag D
n 2 1
!
I TT
T

K ) m w ( D ) m w (
K ) m z ( T TD ) m z (
w
1 T
w
z
T 1 T
z

z T w
T

41
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
MEANING OF THE COVARIANCE MATRIX
Generals on Gaussian pdf -Error elipsis and referencial axis
n=2
[ ] K
m y
m x
m y m x
y
x
y
x
y x

1
1
]
1

1
1
]
1

1
2
2
0
0
1
2
2
2
2

K
) m y (

K
) m x (
y
y
x
x

x
m
y
m
x
K
y
K
ellipse
1
]
1

y
x
z
42
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
MEANING OF THE COVARIANCE MATRIX
Generals on Gaussian pdf -Error ellipse and referencial axis
n=2
1
1
]
1

2
y y x
y x
2
x
P
[ ] K
y
x
y x
y y x
y x x

1
]
1

1
1
]
1

1
2
2
1
2
2
2
1
2
1

K
w

K
w

[ ]
[ ]
2
y
2
x
2 2 2
y
2
x
2
y
2
x 2
2
y
2
x
2 2 2
y
2
x
2
y
2
x 1
4 ) (
2
1
4 ) (
2
1
+ +
+ + +
1
K
2
K
x
y
1
w
2
w

2
y
2
x
2
y
2
x
y x
1
,
4 4

,
2
tan
2
1

,
_

1
]
1

y
x
z
43
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Probabilistic interpretation of the error ellipsoid
Given and it is possible to define the locus where, with a
given probability, the values of the random vector x(k) ly.
)) k | k ( P ), k | k ( x

( ~ ) U , Z | x ( p
1 k
0
k
0
k

) k | k ( x
) k | k ( P
Hiperellipsoid with center in and with semi-axis
proportional to the eigenvalues of
) k | k ( x

) k | k ( P
44
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Probabilistic interpretation of the error ellipsoid
)) k | k ( P ), k | k ( x

( ~ ) U , Z | x ( p
1 k
0
k
0
k

Example for n=2

) k | k ( x

k
x
} K )] k | k ( x

x [ ) k | k ( P )] k | k ( x

x [ : x { M
k
T
k k

1
} M x Pr{
k

is a function of K
a pre-specified values of this probability can be obtained by an
apropriate choice of K
45
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
Probabilistic interpretation of the error ellipsoid
How to chose K for a desired probability?
Just consult a Chi square distribution table
)) k | k ( P ), k | k ( x

( ~ ) U , Z | x ( p
1 k
0
k
0
k

n
k
R x
K )] k | k ( x

x [ ) k | k ( P )] k | k ( x

x [
k
T
k

1
(Scalar) random variable with a distribution
with n degrees of reedom
2

Probability = 90%
n=1 K=2.71
n=2 K=4.61
Probability = 95%
n=1 K=3.84
n=2 K=5.99
46
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
The error ellipsoid and the filter dynamics
Prediction cycle
k k k
u B ) k | k ( x

A ) k | k ( x

+ + 1
T
k
k k
T
k
k
G Q G k)A | P(k A k) | 1 P(k + +
) k | k ( P
) k | k ( P 1 +
k
Q
) k | k ( x

) k | k ( x

1 +
k
x
k
u
0
k
w
k k k k k k k
w G u B x A x + +
+1
1 + k
x
47
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
DISCRETE KALMAN FILTER
The error ellipsoid and the filter dynamics
Filtering cycle
k
R
) k | k ( P 1 +
) k | k ( x

1 +
0
1 1 1 1 + + + +
+
k k k k
v x C z
1 + k
x
) k | k ( x

C
k
1
1
+
+
1 + k
z
1 + k
v
1 + k
r
1 + k
r
) k ( S 1 +
) k | k ( x

1 1 + +
1 + k
x
) k | k ( P 1 1 + +
) k | k ( x

C z r
k k k
1
1 1 1
+
+ + +
1
1
1
1 1
+
+
+
+ + +
k
T
k
k
R C ) K | K ( P C ) k ( S
) k ( r ) k ( K ) k | k ( x

) k | k ( x

1 1 1 1 1 + + + + + +
) k | k ( P C ) k ( K ) k | k ( P ) k | k ( P
k
1 1 1 1 1
1
+ + + + +
+
48
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
Extended Kalman Filter
Non linear Non linear dynamics
White Gaussian White Gaussian system and observation noise
QUESTION: Which is the MMSE (minimum mean-square error)
estimate of x(k+1)?
Conditional mean
Due to the non-linearity of the system,
k k k k
k k k k 1 k
v ) x ( h z
w ) u , x ( f x
+
+
+
) P , x ( N ~ x
0 0 0
kj k
T
j k
kj k
T
j k
R ] v v [ E
Q ] w w [ E


? ) U , Z | x ( E ) k | k ( x

k k
k
0 1
1
1
+
+
) U , Z | x ( p
k k
k
1
0 1

) U , Z | x ( p
k k
k
0 1
1 +
are non Gaussian
49
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
Extended Kalman Filter
(Optimal) (Optimal) ANSWER: The MMSE estimate is given by a non-linear
filter, that propagates the conditonal pdf.
The EKF EKF gives an approximation of the optimal estimate
The non-linearities are approximated by a linearized version of the non-linear
model around the last state estimate.
For this approximation to be valid, this linearization should be a good
approximation of the non-linear model in all the unceratinty domain associated
with the state estimate.
50
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
Extended Kalman Filter
) U , Z | x ( p
k k
k
1
0 1

) k | k ( x

) U , Z | x ( p
k k
k
0 1
1 +
k k k k k
w ) u , x ( f x +
+1
linearize
around
) k | k ( x

) k | k ( x

1 +
) U , Z | x ( p
k k
k
0
1
1
1
+
+
Apply KF to the
linear dynamics
linearize 1 1 1 1 + + + +
+
k k k k
v ) x ( h z
around
) k | k ( x

1 +
Apply KF to the
linear dynamics
) k | k ( x

1 1 + +
51
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
Extended Kalman Filter
) k | k ( x

k k k k k
w ) u , x ( f x +
+1
linearize
around
.... ) x

x ( f ) u , x

( f ) u , x ( f
k | k k k k k | k k k k k
+ +
) x

f ) u , x

( f ( w x f x
k | k k k k | k k k k k k
+ +
+1
known input Prediction cycle of KF
) x

f ) u , x

( f ( x

f x

k | k k k k | k k k | k k k | k
+
+1
k
T
k k
Q f ) k | k ( P f ) k | k ( P + +1
52
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
Extended Kalman Filter
) k | k ( x

1 +
linearize
around
.... ) x

x ( h ) x

( h ) x ( h
k | k k k k | k k k k
+ +
+ + + + + + + 1 1 1 1 1 1 1
) x

h ) x

( h ( v x h z
k | k k k | k k k k k k 1 1 1 1 1 1 1 + + + + + + +
+ +
known input Update cycle of KF
)] x

( h z [ ) R h ) k | k ( P h ( h ) k | k ( P x

k | k k k k
T
k k
T
k
k | k k | k 1 1 1
1
1
1 1 1
1 1 1
1 1
+ + +

+
+ + +
+ + +
+ + + +
) k | k ( P h ] R h ) k | k ( P h [ h ) k | k ( P ) k | k ( P ) k | k ( P
k
k
T
k k
T
k
1 1 1 1 1 1
1
1
1
1 1 1
+ + + + + + +
+

+
+ + +
1 1 1 1 + + + +
+
k k k k
v ) x ( h z
53
M. I sabel Ribeir o - J une. 2000 I nst it ut o de Sist emas e Robt ica/ I nst it ut o Super ior Tcnico
References
Anderson, Moore, Optimal Filtering, Prentice-Hall, 1979.
M. Athans, Dynamic Stochastic Estimation, Prediction and Smoothing, Series of Lectures,
Spring 1999.
E. W. Kamen, J. K. Su, Introduction to Optimal Estimation, Springer, 1999.
Peter S. Maybeck, The Kalman Filter: an Introduction to Concepts
Jerry M. Mendel, Lessons in Digital Estimation Theory, Prentice-Hall, 1987.
M.Isabel Ribeiro, Notas Dispersas sobre Filtragem de Kalman CAPS, IST, June 1989
(http://www.isr.ist.utl.pt/~mir)