Optimal filtering with Kalman filters and smoothers – a
Manual for Matlab toolbox EKF/UKF
Jouni Hartikainen and Simo Särkkä
Laboratory of Computational Engineering,
Helsinki University of Technology,
P.O.Box 9203, FIN-02015 TKK, Espoo, Finland
jmjharti@cc.hut.fi, simo.sarkka@hut.fi
August 7, 2007
Version 1.0
Abstract
In this paper we present a documentation for optimal filtering toolbox for mathematical soft-
ware package Matlab. The methods in the toolbox include Kalman filter, extended Kalman filter
and unscented Kalman filter for discrete time state space models. Also included in the toolbox
are the Rauch-Tung-Striebel and Forward-Backward smoother counter-parts for each filter, which
can be used to smooth the previous state estimates, after obtaining new measurements. The usage
and function of each method are illustrated with five demonstrations problems.
1
Contents
1 Introduction 4
2 Discrete-Time State Space Models 4
2.1 Linear state space estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.1.1 Discretization of continuous-time linear time-invariant systems . . . . . . . . 6
2.1.2 Kalman filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.1.3 Kalman smoother . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.1.4 Demonstration: 2D CWPA-model . . . . . . . . . . . . . . . . . . . . . . . 8
2.2 Nonlinear state space estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.2.1 Taylor Series Based Approximations . . . . . . . . . . . . . . . . . . . . . . 12
2.2.2 Extended Kalman filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.2.3 Extended Kalman smoother . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.2.4 Demonstration: Tracking a random sine signal . . . . . . . . . . . . . . . . 16
2.2.5 Unscented Transform . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
2.2.6 Unscented Kalman filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
2.2.7 Unscented Kalman smoother . . . . . . . . . . . . . . . . . . . . . . . . . . 26
2.2.8 Demonstration: UNGM-model . . . . . . . . . . . . . . . . . . . . . . . . . 27
2.2.9 Demonstration: Bearings Only Tracking . . . . . . . . . . . . . . . . . . . . 31
2.2.10 Demonstration: Reentry Vehicle Tracking . . . . . . . . . . . . . . . . . . . 34
3 Functions in the toolbox 43
3.1 Discrete-time state space estimation . . . . . . . . . . . . . . . . . . . . . . . . . . 44
3.1.1 Linear models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
3.1.2 Nonlinear models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
3.2 Other functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
2
Preface
Most of the software provided with this toolbox are originally created by Simo Särkkä while he was
doing research on his doctoral thesis (Särkkä, 2006b) in the Laboratory of Computational Engineer-
ing (LCE) at Helsinki University of Technology (HUT). This document has been written by Jouni
Hartikainen at LCE during spring 2007 with a little help from Simo Särkkä. Jouni also checked
and commented the software code thoroughly. Many (small) bugs were fixed, and also some new
functions were implemented (for example 2nd order EKF and augmented form UKF). Jouni also
provided the software code for first three demonstrations, modified the two last ones a bit, and ran
all the simulations.
First author would like to thank Doc. Aki Vehtari for helpful comments during the work and for
coming up with the idea of this toolbox in the first place. Prof. Jouko Lampinen also deserves thanks
for ultimately making this work possible.
3
1 Introduction
The term optimal filtering refers to methodology used for estimating the state of a time varying
system, from which we observe indirect noisy measurements. The state refers to the physical state,
which can be described by dynamic variables, such as position, velocity and acceleration of a mov-
ing object. The noise in the measurements means that there is a certain degree of uncertainty in
them. The dynamic system evolves as a function of time, and there is also noise in the dynamics of
system, process noise, meaning that the dynamic system cannot be modelled entirely deterministi-
cally. In this context, the term filtering basically means the process of filtering out the noise in the
measurements and providing an optimal estimate for the state given the observed measurements and
the assumptions made about the dynamic system.
This toolbox provides basic tools for estimating the state of a linear dynamic system, the Kalman
filter, and also two extensions for it, the extended Kalman filter (EKF) and unscented Kalman filter
(UKF), both of which can be used for estimating the states of nonlinear dynamic systems. Also the
smoother counterparts of the filters are provided. Smoothing in this context means giving an estimate
of the state of the system on some time step given all the measurements including ones encountered
after that particular time step, in other words, the smoother gives a smoothed estimate for the history
of the system’s evolved state given all the measurements obtained so far.
This documentation is organized as follows:
• First we briefly introduce the concept of discrete-time state space models. After that we con-
sider linear, discrete-time state space models in more detail and review Kalman filter, which
is the basic method for recursively solving the linear state space estimation problems. Also
Kalman smoother is introduced. After that the function of Kalman filter and smoother and
their usage in this toolbox in demonstrated with one example (CWPA-model).
• Next we move from linear to nonlinear state space models and review the extended Kalman
filter (and smoother), which is the classical extension to Kalman filter for nonlinear estimation.
The usage of EKF in this toolbox is illustrated exclusively with one example (Tracking a
random sine signal), which also compares the performances of EKF, UKF and their smoother
counter-parts.
• After EKF we review unscented Kalman filter (and smoother), which is a newer extension to
traditional Kalman filter to cover nonlinear filtering problems. The usage of UKF is illustrated
with one example (UNGM-model), which also demonstrates the differences between different
nonlinear filtering techniques.
• To give a more thorough demonstration to the provided methods two more classical nonlinear
filtering examples are provided (Bearings Only Tracking and Reentry Vehicle Tracking).
• Lastly we list and describe briefly all the functions included in the toolbox.
The mathematical notation used in this document follows the notation used in (Särkkä, 2006b).
2 Discrete-Time State Space Models
In this section we shall consider models where the states are defined in discrete-time models. The
models are defined recursively in terms of distributions
x
k
∼ p(x
k
|x
k−1
)
y
k
∼ p(y
k
|x
k
),
(1)
where
4
• x
k
∈ R
n
is the state of the system on the time step k.
• y
k
∈ R
m
is the measurement on the time step k.
• p(x
k
|x
k−1
) is the dynamic model which charaterizes the dynamic behaviour of the system.
Usually the model is a probability density (continous state), but it can also be a counting
measure (discrete state), or a combination of them, if the state is both continuous and discrete.
• p(y
k
|x
k
) is the model for measurements, which describes how the measurements are dis-
tributed given the state. This model characterizes how the dynamic model is perceived by the
observers.
A system defined this way has the so called Markov-property, which means that the state x
k
given
x
k−1
is independent from the history of states and measurements, which can also be expressed with
the following equality:
p(x
k
|x
1:k−1
, y
1:k−1
) = p(x
k
|x
k−1
). (2)
The past doesn’t depend on the future given the present, which is the same as
p(x
k−1
|x
k:T
, y
k:T
) = p(x
k−1
|x
k
). (3)
The same applies also to measurements meaning that the measurement y
k
is independent from the
histories of measurements and states, which can be expressed with equality
p(y
k
|x
1:k
, y
1:k−1
) = p(y
k
|x
k
). (4)
In actual application problems, we are interested in predicting and estimating dynamic system’s
state given the measurements obtained so far. In probabilistic terms, we are interested in the predic-
tive distribution for the state at the next time step
p(x
k
|y
1:k−1
), (5)
and in the marginal posterior distribution for the state at the current time step
p(x
k
|y
1:k
). (6)
The formal solutions for these distribution are given by the following recursive Bayesian filtering
equations (e.g. Särkkä, 2006b):
p(x
k
|y
1:k−1
) =
Z
p(x
k
|x
k−1
) p(x
k−1
|y
1:k−1
) dx
k−1
. (7)
and
p(x
k
|y
1:k
) =
1
Z
k
p(y
k
|x
k
) p(x
k
|y
1:k−1
), (8)
where the normalization constant Z
k
is given as
Z
k
=
Z
p(y
k
|x
k
) p(x
k
|y
1:k−1
) dx
k
. (9)
In many cases we are also interested in smoothed state estimates of previous time steps given the
measurements obtained so far. In other words, we are interested in the marginal posterior distribution
p(x
k
|y
1:T
), (10)
where T > k. As with the filtering equations above also in this case we can express the formal
solution as a set of recursive Bayesian equations (e.g. Särkkä, 2006b):
p(x
k+1
|y
1:k
) =
Z
p(x
k+1
|x
k
) p(x
k
|y
1:k
) dx
k
p(x
k
|y
1:T
) = p(x
k
|y
1:k
)
Z
p(x
k+1
|x
k
) p(x
k+1
|y
1:T
)
p(x
k+1
|y
1:k
)
dx
k+1
.
(11)
5
