System Identification Seminar on

Kalman Filtering

By: Mahsa Rezaei (909238)

School of Electrical and Computer Engineering

February 2012

1

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

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

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

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

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

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

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

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

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

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

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

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

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

Standard Kalman filter algorithm:

Step 0: Initialization of the filter at k=0.

𝒙 𝟎+ = 𝑬 𝒙𝟎

𝑷𝟎+ = 𝑬 𝒙𝟎 − 𝒙 𝟎

+ 𝒙𝟎 − 𝒙 𝟎+ 𝑻

Step 1: for k=1,2,… follow the following procedure.

a)

𝒙 𝒌− = 𝑨𝒌−𝟏𝒙 𝒌−𝟏

+ + 𝑩𝒌−𝟏𝒖𝒌−𝟏

𝑷𝒌− = 𝑨𝒌−𝟏𝑷𝒌−𝟏

+𝑨𝒌−𝟏𝑻 + 𝑳𝒌−𝟏𝑸𝒌−𝟏𝑳𝒌−𝟏

𝑻

b)

𝑲𝒌 = 𝑷𝒌−𝑯𝒌

𝑻 𝑯𝒌𝑷𝒌−𝑯𝒌

𝑻 +𝑴𝒌𝑹𝒌𝑴𝒌𝑻 −𝟏

c)

𝒙 𝒌+ = 𝒙 𝒌

− +𝑲𝒌 𝒚𝒌 −𝑯𝒌𝒙 𝒌−

𝑷𝒌+ = (𝑰 − 𝑲𝒌𝑯𝒌)𝑷𝒌

−

15

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

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

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

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

Figure 1 shows the true position of the vehicle, the measured position, and

the estimated position.

20

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

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

Figure 3 shows the True and Estimated Velocity

23

Figure 4 shows the error between the true velocity and the Kalman

filter‟s estimated velocity.

24

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

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

27

𝑷𝒌− = 𝑭𝒌−𝟏𝑷𝒌−𝟏

+𝑭𝒌−𝟏𝑻 + 𝑳𝒌−𝟏𝑸𝒌−𝟏𝑳𝒌−𝟏

𝑻

c)

𝑯𝒌 =𝝏𝒉𝒌𝝏𝒙

𝒙 𝒌−

𝑴𝒌 =𝝏𝒉𝒌𝝏𝒗

𝒙 𝒌−

d)

𝑲𝒌 = 𝑷𝒌−𝑯𝒌

𝑻 𝑯𝒌𝑷𝒌−𝑯𝒌

𝑻 +𝑴𝒌𝑹𝒌𝑴𝒌𝑻 −𝟏

e)

𝒙 𝒌+= 𝒙 𝒌

−+𝑲𝒌 𝒚𝒌 − 𝒉𝒌 𝒙 𝒌

− , 𝟎

𝑷𝒌+ = (𝑰 − 𝑲𝒌𝑯𝒌)𝑷𝒌

−

28

Example on Extended Kalman filter for Nonlinear

systems:

Consider a second order system as below:

𝒚

𝒖=

𝝎𝒏𝟐

𝒔𝟐 + 𝟐𝜻𝝎𝒏𝒔 + 𝝎𝒏𝟐

Define 𝑏 = 2𝜁𝜔𝑛

We want to estimate 𝝎𝒏𝟐.

In terms of differential equations:

𝒚 + 𝒃𝒚 + 𝝎𝒏𝟐𝒚 = 𝝎𝒏

𝟐𝒖

And so we define the states as:

𝒙𝟏 = 𝒚 𝒙𝟏 = 𝒙𝟐

𝒙𝟐 = 𝒚 𝒙𝟐 = −𝒃𝒙𝟐 − 𝒙𝟏𝒙𝟑 + 𝒙𝟑𝒖

𝒙𝟑 = 𝝎𝒏𝟐 𝒙𝟑 = 𝟎

29

Assume that:

𝝎𝒏 = 𝟏 : true value

𝜻 = 𝟎. 𝟏 : zeta

𝑸 =𝟏𝟎𝟎𝟎 𝟎𝟎 𝟎. 𝟏

: Covariance of process noise

𝑹 =𝟏𝟎 𝟎𝟎 𝟏𝟎

: Covariance of measurement noise

The simulation results are enclosed in the accompanying figures.

30

This figure shows the true and estimated value of 𝜔𝑛2

31

Variance of estimation error is implemented in this figure

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

33

Thank You