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 Nstep 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 zeromean 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
+ 1k) = x(kk
1) + u(k) + K(k) y(k) Cx(kk
1)
with x(k
+ 1k) 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
+ 1k)
= x(k) + u(k) + v(k) x(kk
1) u(k) K(k) y(k) Cx(kk
1)
= x(k) + v(k) x(kk
1) K(k) Cx(k) + e(k) Cx(kk
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 completingofsquares 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
+ 1k) = x(kk
1) + u(k) + K(k) y(k) Cx(kk
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(kk)
= x(kk
1) + K(k) (y(k) Cx(kk
1))
1
T
T
K(k) = P(kk 1)C CP(kk 1)C + R2
P(kk) = P(kk 1) K(k)CP(kk 1)
Update estimate with y(k), compute Kalman gain, update error
covariance.
Step 2. (Onestep predictor)
x(k
+ 1k) = x(kk)
+ u(k)
P(k + 1k) = P(kk)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(kk)]
= E [x(kk
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 nonGaussian assumptions, nonlinear filters, particle filters
or movinghorizon estimators do a much better job.)
acs kf lqg.17
2.6 Example
Discretetime 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
+ 1k) = x(kk
1) + K(k) y(k) x(kk
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 steadystate variance
Small fixed K slower convergence, but better performance in steady state
acs kf lqg.22
Steadystate Kalman gain
Recall:
P(k + 1) = P(k) + R1 P(k)C + R12 CP(k)C + R2
T
Steadystate 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 steadystate 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 steadystate estimator gain L and steadystate error
covariance
P = E{(x  x[nn1])(x  x[nn1])}
(Riccati solution)
acs kf lqg.23
3. Linear quadratic Gaussian control
Given discretetime 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(kk
1)
(so u(k) = Lx(k) + Lx(k))
with closedloop 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(kk
1) results in closedloop system
x(k
+ 1k) = L(k) x(kk
1) + K(k) y Cx(kk
1)

{z
}
w(k)
It can be shown that for optimal K(k), w(k) is white noise
So dynamics become
x(k
+ 1k) = ( L(k))x(kk
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
+ 1k) = ( L(k))x(kk
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(kk
1)
with
x(k
+ 1k) = x(kk
1) + u(k) + K(y(k) Cx(kk
1)
Closedloop 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 closedloop 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
statefeedback 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
 highorder 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