Advanced Control Systems SC3105et

Kalman filtering and LQG control

Keviczky
Tamas
Delft Center for Systems and Control
Delft University of Technology

Lecture outline
1. Review
LQ control
completing the squares
dynamic programming
2. Kalman filtering
3. Linear quadratic Gaussian control

Note: These slides are partly inspired by the slides for this course developed at the Department
of Automatic Control, Lund Institute of Technology (see http://www.control.lth.se/kursdr)
acs kf lqg.1

Lecture outline (continued)

Linear Quadratic (LQ) control
assumes full state information
Estimating state from measurements of output
= Kalman filtering
Combination of LQ and state estimation
= Linear Quadratic Gaussian (LQ) control
based on separation theorem

acs kf lqg.2

1. Review
1.1 LQ control
Minimize

N1
J = xT(k)Q1x(k) + 2xT(k)Q12u(k) + uT(k)Q2u(k) + xT(N)Q0x(N)
k=0

subject to x(k + 1) = x(k) + u(k) and x(0) = x0

Solution approach based on quadratic optimization problem and

dynamic programming
Results in state feedback controller u = L(k)x(k) with L(k)
determined by solution S(k) of Riccati recursion

acs kf lqg.3

1.2 Completing the squares

Find u that minimizes xTQxx + 2xTQxuu + uTQuu with Qu positive
definite
Let L be such that QuL = QTxu. Then
xTQxx + 2xTQxuu + uTQuu =
xT(Qx LTQu L)x + (u + Lx)TQu(u + Lx)
is minimized for u = Lx
and minimum value is xT(Qx LTQu L)x
T
Q
If Qu is positive definite, then L = Q1
u
xu

acs kf lqg.4

1.3 Dynamic programming

Principle of optimality: From any point on optimal trajectory,
remaining trajectory is also optimal
t2
t1

t3

allows to determine best control law over period [t2,t3]

independent of how state at t2 was reached
For N-step problem:
start from end at time k = N
now we can determine best control law for last step independent of how state at time N 1 was reached
iterate backward in time to initial time k = 0
acs kf lqg.5

2. Kalman filtering
LQ control requires full state information
In practice: only output measured
how to estimate states from noisy measurements of output?
Consider system
x(k + 1) = x(k) + u(k) + v(k)
y(k) = Cx(k) + e(k)
with v, e Gaussian zero-mean white noise process with






T
T
T
E v(k)v (k) = R1, E e(k)e (k) = R2, E v(k)e (k) = R12
and x(0) Gaussian distributed with


E x(0) = m0
h



T i
cov (x(0)) := E x(0) E [x(0)] x(0) E [x(0)]
= R0

acs kf lqg.6

2.1 Problem formulation

Given the data
Yk = {y(i), u(i) : 0 i k}
find the best (to be defined) estimate x(k
+ m) of x(k + m).
(m = 0 filtering, m > 0 prediction, m < 0 smoothing)

acs kf lqg.7

Some history

Norbert Wiener : Filtering, prediction, and smoothing using integral

equations. Spectral factorizations.
Rudolf E. Kalman: Filtering, prediction, and smoothing using statespace formulas. Riccati equations.

2.2 Kalman filter structure

Goal is to estimate x(k + 1) by linear combination of previous inputs and outputs
Estimator (cf. lecture on observers):


x(k
+ 1|k) = x(k|k

1) + u(k) + K(k) y(k) Cx(k|k

1)

with x(k
+ 1|k) estimate of state x at sample step k + 1 using information available at step k
Error dynamics x = x x governed by
x(k
+ 1) = x(k + 1) x(k
+ 1|k)


= x(k) + u(k) + v(k) x(k|k

1) u(k) K(k) y(k) Cx(k|k

1)


= x(k) + v(k) x(k|k

1) K(k) Cx(k) + e(k) Cx(k|k

1)
= ( K(k)C)x(k)
+ v(k) K(k)e(k)
acs kf lqg.8

2.3 Determination of Kalman gain

Error dynamics: x(k
+ 1) = ( K(k)C)x(k)
+ v(k) K(k)e(k)






If x(0)
=

= E x(0) x(0)

= E x(0) m0 = 0 and
 m0, then E x(0)
thus E x(k)

= 0 for all k
How to choose K(k)? minimize covariance of x(k)

We have
h



T i
cov (x(k))

= E x(k)
E [x(k)]

x(k)
E [x(k)]



T
= E x(k)
x (k)


T
So if we define P(k) = E x(k)
x (k) , then we have to determine
Kalman gain such that P(k) is minimized
acs kf lqg.9

2.3 Determination of Kalman gain (continued)

Error dynamics: x(k
+ 1) = ( K(k)C)x(k)
+ v(k) K(k)e(k)
We have


P(k + 1) = E x(k
+ 1)x (k + 1)


T
= E ( K(k)C)x(k)
+ v(k) K(k)e(k) ( K(k)C)x(k)
+ v(k) K(k)e(k)

Since x(k)
is independent of v(k) and e(k), this results in
h
P(k + 1) = E ( K(k)C)x(k)
xT(k)( K(k)C)T
T

+ v(k)v (k) + K(k)e(k)e (k)K (k) v(k)e (k)K (k) K(k)e(k)v (k)


T
= ( K(k)C) E x(k)
x (k) ( K(k)C)T + R1 + K(k)R2K T(k)
|
{z
}
P(k)
R12K T(k) K(k)RT12

acs kf lqg.10

2.3 Determination of Kalman gain (continued)

So
P(k + 1) = ( K(k)C)P(k)( K(k)C)T + R1 + K(k)R2K T(k) R12K T(k) K(k)RT12

T

T


T
T
T
T
T
= K(k) CP(k)C + R2 K (k) P(k)C + R12 K (k) K(k) CP (k) + R12


T
+ P(k) + R1

Minimize P(k + 1) with K(k) as decision variable

P(k + 1) is quadratic function of K(k)

Use completing-of-squares solution with u = K T(k) and x = I:

T

T
T
T
K(k) CP(k)C + R2 K (k) + P(k)C R12 K (k)
|{z} |
{z
} | {z } |
{z
} | {z }
u
u
Qu
Qxu
uT




T
T
T
T
+ K(k) CP (k) R12 + P(k) + R1
|{z} |
{z
} |
{z
}
Qx
uT
QTxu
acs kf lqg.11

2.3 Determination of Kalman gain (continued)

Results in:
K T(k) = L(k)x = L(k)
L(k) =
So

T
Q1
Q
u
xu

= CP(k)C + R2

1

P(k)C R12
T

K(k) = L (k) = P(k)C + R12 CP(k)C + R2

T

Furthermore,

P(k + 1) = Qx LTQuL

T
T
T
= P(k) + R1 K(k) CP(k)C + R2 K (k)

= P(k) + R1 P(k)C + R12 CP(k)C + R2

T

1

1

T

CP(k)

+ RT12

acs kf lqg.12

2.3 Determination of Kalman gain (continued)



T
Initial value: P(0) = E x(0)
x (0)
h



T i
= E x(0) x(0)

x(0) x(0)

h



T i
= E x(0) m0 x(0) m0
= cov (x(0)) = R0

Overall solution:


x(k
+ 1|k) = x(k|k

1) + u(k) + K(k) y(k) Cx(k|k

1)



1
T
T
K(k) = P(k)C + R12 CP(k)C + R2
P(k + 1) = P(k)T + R1 K(k) CP(k)CT + R2)K T(k)
P(0) = R0
x(0|
1) = m0

acs kf lqg.13

2.4 Common equivalent implementation

Step 0. (Initialization)
P(0| 1) = P(0) = R0
x(0|
1) = m0
Covariance and mean of initial state.

acs kf lqg.14

2.4 Common equivalent implementation (continued)

Step 1. (Corrector - use the most recent measurement)
x(k|k)

= x(k|k

1) + K(k) (y(k) Cx(k|k

1))

1
T
T
K(k) = P(k|k 1)C CP(k|k 1)C + R2
P(k|k) = P(k|k 1) K(k)CP(k|k 1)
Update estimate with y(k), compute Kalman gain, update error
covariance.
Step 2. (One-step predictor)
x(k
+ 1|k) = x(k|k)

+ u(k)
P(k + 1|k) = P(k|k)T + R1
Project the state and error covariance ahead.
Iterate Step 1 and 2, increase k.

acs kf lqg.15

2.5 Comments on Kalman filter solution

From the Kalman gain K(k) update equation, we see that a large
R2 (much measurement noise) leads to low influence of error on
estimate.
The P(k) estimation error covariance is a measure of the uncertainty of the estimate. It is updated as
P(k + 1) = P(k)T + R1 K(k) CP(k)CT + R2)K T(k)
The first two terms on the right represent the natural evolution of
the uncertainty, the last term shows how much uncertainty the
Kalman filter removes.

acs kf lqg.16

2.5 Comments on Kalman filter solution (continued)

The Kalman filter gives an unbiased estimate, i.e.,
E [x(k|k)]

= E [x(k|k

1)] = E [x(k)]
If the noise is uncorrelated with x(0), then the Kalman filter is
optimal, i.e., no other linear filter gives a smaller variance of the
estimation error.
(For non-Gaussian assumptions, nonlinear filters, particle filters
or moving-horizon estimators do a much better job.)

acs kf lqg.17

2.6 Example
Discrete-time system x(k + 1) = x(k)
y(k) = x(k) + e(k)
constant state, to be reconstructed from noisy measurements
Measurement noise e has standard deviation (so R2 = 2)
x(0) has covariance R0 = 0.5
No process noise v, so R1 = 0 and R12 = 0
Kalman filter is given by


x(k
+ 1|k) = x(k|k

1) + K(k) y(k) x(k|k

1)
P(k)
K(k) =
P(k) + 2
2

P(k)
2
T
P(k + 1) = P(k) K(k)(P(k) + )K (k) =
P(k) + 2
P(0) = R0 = 0.5
x(0|
1) = m0 = 0

acs kf lqg.18

2.6 Example (continued)

x(k)

0
1

100

200

300

400

500

600

0
0
0.5

100

200

300

400

500

600

0
0

100

200

300

400

500

600

P(k)

K(k)

2
0
0.5

acs kf lqg.19

2.6 Example (continued)

x(k)

K = 0.05

0
1
2
0

100

200

300

400

500

600

400

500

600

400

500

600

x(k)

K = 0.01

0
1
2
0

100

200

300

x(k)

Optimal gain

0
1
2
0

100

200

300
k

acs kf lqg.20

Example To think about

1. What is the problem with a large (small) K in the observer?
2. What does the Kalman filter do?
3. How would you change P(0) in the Kalman filter to get a smoother
(but slower) transient in x(k)?

4. In practice, R1, R2, and R0 are often tuning parameters. What are
their influence on the estimate?

acs kf lqg.21

Example To think about (continued)

x(k)

K = 0.05
0
1
2
0

100

200

300

400

500

600

400

500

600

x(k)

K = 0.01
0
1
2
0

100

200

300

x(k)

Optimal gain
0
1
2
0

100

200

300
400
500
600
k
Large fixed K rapid initial convergence, but large steady-state variance
Small fixed K slower convergence, but better performance in steady state
acs kf lqg.22

Steady-state Kalman gain

Recall:

P(k + 1) = P(k) + R1 P(k)C + R12 CP(k)C + R2

T

Steady-state solution given by

+ R1 PC
+ R12 CPC
+ R2
P = P
T

is also Riccati equation!

1

1

CP(k)

+ RT12

CP

+ RT12

Note: compare with steady-state LQ controller:

T T


T
T
1
T
T
+ Q1 S
+ Q12 ( S
+ Q2) S
+ Q12
S = S

Duality
Equivalence between LQ control problem and Kalman filter state
estimation problem
LQ
k

Q0
Q1
Q12
S
L

Kalman
N k
T
CT
R0
R1
R12
P
KT

How to find Kalman filter using matlab?

Command [KEST,L,P] = kalman(SYS,QN,RN,NN)
Calculates (full) Kalman estimator KEST for system SYS
x[n+1] = Ax[n] + Bu[n] + Gw[n]
{State equation}
y[n]
= Cx[n] + Du[n] + Hw[n] + v[n] {Measurements}
with known inputs u, process noise w, measurement noise v, and
noise covariances
E{ww} = QN,

E{vv} = RN,

E{wv} = NN,

Note: Construct SYS as SYS=ss(A,[B G],C,[D H],-1)

Also returns steady-state estimator gain L and steady-state error
covariance
P = E{(x - x[n|n-1])(x - x[n|n-1])}

(Riccati solution)
acs kf lqg.23

3. Linear quadratic Gaussian control

Given discrete-time LTI system
x(k + 1) = x(k) + u(k) + v(k)
y(k) = Cx(k) + e(k)
with
"

T# 



R1 R12
v(k) v(k)
E x(0) = m0, cov (x(0)) = R0, E
= T
e(k) e(k)
R12 R2
find linear control law y(0), y(1), . . . , y(k 1) 7 u(k) that minimizes
"
#

N1
E xTQ1x + 2xTQ12u + uTQ2u + xT(N)Q0x(N)
k=0

Solution: Separation principle

acs kf lqg.24

3.1 Separation principle

Makes it possible to use control law
u(k) = Lx(k|k

1)
(so u(k) = Lx(k) + Lx(k))

with closed-loop dynamics

x(k + 1) = x(k) Lx(k) + Lx(k)
+ v(k)
and to view term Lx(k)
as part of noise
solve LQ problem and estimation problem separately

acs kf lqg.25

Proof of separation principle

Solution of optimal observer design problem does not depend on
input u
So using state feedback does not influence optimality
Kalman filter still optimal
Using u(k) = L(k)x(k|k

1) results in closed-loop system

x(k
+ 1|k) = L(k) x(k|k

1) + K(k) y Cx(k|k

1)
|
{z
}
w(k)
It can be shown that for optimal K(k), w(k) is white noise
So dynamics become
x(k
+ 1|k) = ( L(k))x(k|k

1) + K(k)w(k)

Proof of separation principle (continued)

For simplicity we assume Q0 = 0
For u = Lx,
control design problem in terms of x and x becomes
"
#
N1

T
T
T
x
Q
x
+
2x
Q
u
+
u
Q2u
12
1
k=0
"

min E
L

N1

= min E
L

T
T
T T
(
x

+
x)

Q
(
x

+
x)

+
2(
x

+
x)

Q
L
x

+
x

L Q2Lx
1
12

k=0

 T 
Since it can be shown that E x Qx = 0, we get
"
N1

min E
L

T
T
T
T T
x

Q
x

+
x

Q
x

+
2
x

Q
L
x

+
x

L Q2Lx
1
1
12

k=0

 T 
E x Qx does not depend on L and is in fact minimal for Kalman
gain K

Proof of separation principle (continued)

Hence, we get
min E
L

"

N1

T
T
T T
x

Q
x

+
2
x

Q
L
x

+
x

L Q2Lx
12
1

k=0

subject to
x(k
+ 1|k) = ( L(k))x(k|k

1) + K(k)w(k)
= stochastic LQ problem (but with x instead of x and
with u = Lx already filled in)
L(k) as computed before still optimal

3.2 LQG problem: Solution

Process
noise

y
System

State feedback
LQ gain

L(k)

State estimator u

Kalman gain

K(k)

Measurement
noise

LQG controller

acs kf lqg.26

Stationary LQG control

Solution:

u(k) = Lx(k|k

1)

with
x(k
+ 1|k) = x(k|k

1) + u(k) + K(y(k) Cx(k|k

1)
Closed-loop dynamics (with error state x(k),

see slide acs kf lqg.8):

x(k + 1) = ( L)x(k) + Lx(k)
+ v(k)
x(k
+ 1) = ( KC)x(k)
+ v(k) Ke(k)
or

 

 
 
x(k + 1)
L
L
x(k)
I
0
=
+
v(k) +
e(k)
x(k
+ 1)
0
KC x(k)

I
K
dynamics of closed-loop system determined by dynamics of
LQ controller and of optimal filter

acs kf lqg.27

How to construct LQG controller using matlab?

Command RLQG = lqgreg(KEST,K) produces LQG controller by
connecting Kalman estimator KEST designed with kalman and
state-feedback gain K designed with dlqr

acs kf lqg.28

Pros and cons of LQG

+ stabilizing
+ good robustness for SISO
+ works for MIMO
- robustness can be very bad for MIMO
- high-order controller
- how to choose weights?

acs kf lqg.29

Summary
Kalman filtering
state estimator such that error covariance is minimized
LQG control
separation principle LQ + Kalman

acs kf lqg.30