The document describes a seminar on Kalman filtering. It provides an outline of topics to be covered, including motivation, history, what a Kalman filter is, applications, advantages, how it works, criteria for estimators, the standard Kalman filter algorithm, an example of using it for linear systems, extending it to nonlinear systems using the extended Kalman filter algorithm, and another example applying it to a nonlinear system. It aims to introduce Kalman filtering, covering its development, methodology, uses, and implementation for both linear and nonlinear dynamic systems.
1. System Identification Seminar on
Kalman Filtering
By: Mahsa Rezaei (909238)
School of Electrical and Computer Engineering
February 2012
1
2. Outline:
Motivation
Historical Prospective
What is a Kalman filter?
Applications
Advantages
The problem
How does Kalman filter work?
Estimation in linear systems
What criteria should our estimator satisfy?
Preparation before going to Algorithm
Standard Kalman filter Algorithm
Example on Kalman filter for linear systems (Vehicle Navigation)
Shift to nonlinear systems
Extended Kalman filter Algorithm
Example on Extended Kalman filter for Nonlinear Systems
References 2
3. Motivation:
filtering is desirable in many situations in engineering and
embedded systems.
For example, radio communication signals are corrupted with
noise.
A good filtering algorithm can remove the noise from
electromagnetic signals while retaining the useful information.
Another example is power supply voltages. Uninterruptible power
supplies are devices that filter line voltages in order to smooth out
undesirable fluctuations that might otherwise shorten the lifespan
of electrical devices such as computers and printers.
3
4. Historical Prospective:
In 1960 and 1961 Rudolf Emil Kalman published his papers
on a recursive predictive filter that is based on the use of state
space techniques and recursive algorithms and therewith he
revolutionized the field of estimation.
Like many new technologies, the Kalman filter was developed
to solve a specific problem, in this case, spacecraft navigation
for the Apollo space program. Since that time the so-called
Kalman filter has been the subject of extensive research and
applications.
Sometimes the filter is referred to as the Kalman-Bucy filter
because of Richard Bucy‟s early work on the topic, conducted
jointly with Kalman.
4
5. What is a Kalman filter?
The Kalman filter is a set of mathematical equations that
provides an efficient computational (recursive) means to
estimate the state of a dynamic system, in a way that
minimizes the mean of the squared error.
The filter is very powerful in the sense that it supports
estimations of past, present, and even future states, and it can
do so even when the precise nature of the modeled system is
unknown.
5
6. Applications:
The applications of a Kalman filter are numerous:
Tracking objects (e.g., missiles, faces, heads, hands)
All forms of navigation (aerospace, land, and marine),
Economics
Fuzzy logic
Neural network training
The detection of underground radioactivity,
Many computer vision applications
Many more
The principal uses of Kalman filtering have been in „Modern‟
control systems, in the tracking and navigation of all sorts of
vehicles, and predictive design of estimation and control systems.
6
7. Advantages:
1.The Kalman filter is implementable in the form of an algorithm
for a digital computer. This implementation may be slower, but it
is capable of much greater accuracy than had been achieved with
analog filters.
2.The Kalman filter does not require that the deterministic
dynamics or the random processes have stationary properties,
and many applications of importance include non-stationary
stochastic processes.
3.The Kalman filter is compatible with the state-space
formulation of optimal controllers for dynamic systems .
4.The Kalman filter requires less additional mathematical
preparation to learn and use than other filters.
7
8. The problem:
Measuring
Devices Estimator
Measurement
Error Sources
System State
(desired but not
known)
External
Controls
Observed
Measurements
Optimal
Estimate of
System State
System
Error Sources
System
Black
Box
• System state cannot be measured directly
• Need to estimate “optimally” from measurements
8
9. How does Kalman filter work?
The Kalman filter estimates the state of a dynamic system.
This dynamic system can be disturbed by some noise, mostly
assumed as white noise. To improve the estimated state, the
Kalman filter uses measurements that are related to the state
but disturbed as well.
Thus the Kalman filter consists of two steps:
1.The prediction
2.The correction
9
10. In the first step the state is predicted with the dynamic model.
In the second step it is corrected with the observation model, so that
the error covariance of the estimator is minimized. In this sense it is an
optimal estimator.
This procedure is repeated for each time step with the state of the
previous time step as initial value. Therefore the kalman filter is called
a recursive filter.
10
11. Estimation in linear systems:
Many physical processes, such as a vehicle driving along a road, a satellite
orbiting the earth, a motor shaft driven by winding currents, or a sinusoidal radio-
frequency carrier signal, can be approximated as linear systems.
A linear system is simply a process that can be described by the following two
equations:
𝒙 𝒌 = 𝑨 𝒌−𝟏 𝒙 𝒌−𝟏 + 𝑩 𝒌−𝟏 𝒖 𝒌−𝟏 + 𝑳 𝒌−𝟏 𝒘 𝒌−𝟏
𝒚 𝒌 = 𝑯 𝒌 𝒙 𝒌 + 𝑴 𝒌 𝒗 𝒌
𝒙 𝒏×𝟏: State vector
𝒖 𝒓×𝟏: input vector
𝒘 𝒑×𝟏: Process(state) noise
𝒚 𝒎×𝟏: measurement(output) vector
𝒗 𝒈×𝟏: measurement error
𝒌 = 𝟎, 𝟏, 𝟐, … :Discrete time index
𝑨 𝒏×𝒏: transition matrix which relates the state at time k to the one at k-1
𝑩 𝒏×𝒓: relates the optional control input u to the state x
𝑯 𝒎×𝒏: relates the measurement to the state
11
12. What criteria should our estimator satisfy?
We want an estimator that gives an accurate estimate of the true
state even though we cannot directly measure it. What criteria
should our estimator satisfy? Two obvious requirements come to
mind.
First, we want the average value of our state estimate to be equal
to the average value of the true state. That is, we don‟t want our
estimate to be biased one way or another.
Second, we want a state estimate that varies from the true state as
little as possible. That is, not only do we want the average of the
state estimate to be equal to the average of the true state, but we
also want an estimator that results in the smallest possible
variation of the state estimate.
12
13. Preparation before going to algorithm:
1.Definition:
𝒙−: a priori state, the predicted value before the update
𝒙+: a posteriori state, the corrected value after the update
2.Expected value of a random variable x is:
𝑬 𝒙 = 𝒙 𝑷 𝒙 𝒅𝒙 ≈ 𝒙 =
𝟏
𝑵
𝒙𝒊
𝑵
𝒊=𝟏
Expected value of a vector X is by component:
𝑬 𝑿 = 𝑿 = 𝑿 𝟏, 𝑿 𝟐, … , 𝑿 𝒏
𝑻
13
14. 3.For the expressed criteria to be satisfied, certain condition
about the noise- that affects our system- should be assumed:
𝑬 𝒘 𝒌 = 𝟎
𝑬 𝒗 𝒌 = 𝟎
𝑬 𝒘 𝒌 𝒗 𝒌 = 𝟎 𝒘 𝒌~(𝟎, 𝑸 𝒌)
𝑬 𝒘 𝒌 𝒘𝒋
𝑻 = 𝟎 𝒇𝒐𝒓 𝒌 ≠ 𝒋
𝑬 𝒗 𝒌 𝒗𝒋
𝑻
= 𝟎 𝒇𝒐𝒓 𝒌 ≠ 𝒋 𝒗 𝒌~(𝟎, 𝑹 𝒌)
4.Definition:
𝑸 𝒌= 𝑬 𝒘 𝒌 𝒘 𝒌
𝑻 : Covariance of w
𝑹 𝒌 = 𝑬(𝒗 𝒌 𝒗 𝒌
𝑻) : Covariance of v
14
16. Example on standard Kalman filter for linear
systems (Vehicle Navigation):
we want to model a vehicle going in a straight line. We can say that the
state consists of the vehicle position p and velocity v. The input u is the
commanded acceleration and the output y is the measured position. we
are able to change the acceleration and measure the position every T
seconds.
In this case, elementary laws of physics say that the position p and
velocity v will be governed by the following equations:
𝒑 𝒌 = 𝒑 𝒌−𝟏 + 𝑻𝒗 𝒌−𝟏 +
𝟏
𝟐
𝑻 𝟐
𝒖 𝒌−𝟏 + 𝒑 𝒌−𝟏
𝒗 𝒌 = 𝒗 𝒌−𝟏 + 𝑻𝒖 𝒌−𝟏 + 𝒗 𝒌
Where 𝑝 𝑘 and 𝑣 𝑘 are position and velocity noise respectively.
Now we can define a state vector x that consists of position and velocity:
𝒙 𝒌 =
𝒑 𝒌
𝒗 𝒌
16
17. Finally, knowing that the measured output is equal to the
position, we can write our linear system equations as follows:
𝒙 𝒌 =
𝟏 𝑻
𝟎 𝟏
𝒙 𝒌−𝟏 +
𝑻 𝟐
𝟐
𝑻
𝒖 𝒌 + 𝒘 𝒌
𝒚 𝒌 = 𝟏 𝟎 𝒙 𝒌 + 𝒛 𝒌
𝑧 𝑘 is the measurement noise due to such things as
instrumentation errors.
If we want to control the vehicle with some sort of feedback
system, we need an accurate estimate of the position p and the
velocity v. In other words, we need a way to estimate the state x.
This is where the Kalman filter comes in.
17
18. The position is measured with an error of 10 feet (one standard deviation).
The commanded acceleration is a constant 1 foot/sec2. The acceleration
noise is 0.2 feet/sec2 (one standard deviation). The position is measured 10
times per second (T = 0.1).
Since T = 0.1, the linear model that represents our system can be derived
from the system model presented earlier as follows:
𝒙 𝒌 =
𝟏 𝟎. 𝟏
𝟎 𝟏
𝒙 𝒌−𝟏 +
𝟎. 𝟎𝟎𝟓
𝟎. 𝟏
𝒖 𝒌 + 𝒘 𝒌
𝒚 𝒌 = 𝟏 𝟎 𝒙 𝒌 + 𝒛 𝒌
Because the standard deviation of the measurement noise is 10 feet, the 𝑅 𝑘
matrix is simply equal to 100.
Now we need to derive the 𝑄 𝑘 matrix.
18
19. Since the position is proportional to 0.005 times the acceleration, and the
acceleration noise is 0.2 feet/sec2, the variance of the position noise is
(0.005)2× (0.2)2= 10−6.
Similarly, since the velocity is proportional to 0.1 times the acceleration, the
variance of the velocity noise is (0.1)2× (0.2)2= 4 × 10−4.
Finally, the covariance of the position noise and velocity noise is equal to
the standard deviation of the position noise times the standard deviation of
the velocity noise, which can be calculated as 0.005 × 0.2 × 0.1 × 0.2
= 2 × 10−5.
We can combine all of these calculations to obtain the following matrix for
𝑄 𝑘 :
𝑸 𝒌 = 𝑬 𝒙𝒙 𝑻
= 𝑬
𝒑
𝒗
𝒑 𝒗 = 𝑬
𝒑 𝟐 𝒑𝒗
𝒑𝒗 𝒗 𝟐
= 𝟏𝟎−𝟔 𝟐 × 𝟏𝟎−𝟓
𝟐 × 𝟏𝟎−𝟓 𝟒 × 𝟏𝟎−𝟒
I simulated the Kalman filter for this problem using Matlab. The results are
shown in the accompanying figures.
19
20. Figure 1 shows the true position of the vehicle, the measured position, and
the estimated position.
20
21. The two smooth curves are the true position and the estimated position, and
they are almost too close to distinguish from one another. The noisy-looking
curve is the measured position. For better comparison, part of the figure is
magnified here:
21
22. Figure 2 shows the error between the true position and the measured
position, and the error between the true position and the Kalman filter‟s
estimated position.
22
24. Figure 4 shows the error between the true velocity and the Kalman
filter‟s estimated velocity.
24
25. Shift to Nonlinear Systems:
We have discussed state estimation for linear systems. But what if we want
to estimate the states of a nonlinear system?
As a matter of fact, almost all real engineering processes are nonlinear.
Some can be approximated by linear systems but some cannot.
This was recognized early in the history of Kalman filters and led to the
development of the “extended Kalman filter” which is simply an
extension of linear Kalman filter theory to nonlinear systems.
A Nonlinear system is simply a process that can be described by the
following two equations:
𝒙 𝒌 = 𝒇 𝒌−𝟏 𝒙 𝒌−𝟏, 𝒖 𝒌−𝟏 , 𝒘 𝒌−𝟏
𝒚 𝒌 = 𝒉 𝒌 𝒙 𝒌, 𝒖 𝒌 , 𝒗 𝒌
f : System function
h : Output function
25
26. Extended Kalman filter Algorithm:
Step 0: Initialization of the filter at k=0
𝒙 𝟎
+
= 𝑬 𝒙 𝟎
𝑷 𝟎
+
= 𝑬 𝒙 𝟎 − 𝒙 𝟎
+
𝒙 𝟎 − 𝒙 𝟎
+ 𝑻
Step 1: for k=1,2,… follow the following procedure.
a)
𝑭 𝒌−𝟏 =
𝝏𝒇 𝒌−𝟏
𝝏𝒙
𝒙 𝒌−𝟏
+
𝑳 𝒌−𝟏 =
𝝏𝒇 𝒌−𝟏
𝝏𝒘
𝒙 𝒌−𝟏
+
b)
𝒙 𝒌
−
= 𝒇 𝒌−𝟏 𝒙 𝒌−𝟏
+
, 𝒖 𝒌−𝟏, 𝟎
26
32. 32
References:
1. J. S. Meditch. Stochastic optimal linear estimation and
control
2. Simon J. Julier and Jeffrey K. Uhlmann. A new extension of
the Kalman filter to nonlinear systems.
3. Mohinder S. Grewal and Angus P. Andrews. Kalman
filtering: Theory and Practice using Matlab
4. Rachel Kleinbauer . Kalman filtering Implementation with
Matlab
5. Greg Welch and Gary Bishop. An introduction to the Kalman
filter