8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
1/37
Advanced Control Systems SC3105et
Kalman filtering and LQG control
Tamas Keviczky
Delft Center for Systems and ControlDelft University of Technology
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
2/37
Lecture outline
1. Review
LQ control
completing the squaresdynamic 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
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
3/37
Lecture outline (continued)
Linear Quadratic (LQ) controlassumesfull state information
Estimating state from measurements of output= Kalman filtering
Combination of LQ and state estimation= Linear Quadratic Gaussian (LQ) controlbased onseparation theorem
acs kf lqg.2
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
4/37
1. Review
1.1 LQ control
Minimize
J=N1
k=0
xT(k)Q1x(k) + 2x
T(k)Q12u(k) + uT(k)Q2u(k)
+xT(N)Q0x(N)
subject tox(k+ 1) = x(k) + u(k)andx(0) =x0
Solution approach based on quadratic optimization problem anddynamic programming
Results in state feedback controller u
=L
(k
)x
(k
)withL
(k
)determined by solutionS(k)of Riccati recursion
acs kf lqg.3
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
5/37
1.2 Completing the squares
Find uthat minimizes xTQxx + 2xTQxuu + u
TQuuwith Qu positive
definite
LetLbe such thatQuL=QTxu. Then
xTQxx + 2xTQxuu + u
TQuu=
xT(QxLTQuL)x + (u +Lx)
TQu(u +Lx)
is minimized for u=Lx
and minimum value isxT(Qx LTQuL)x
IfQuis positive definite, then L=Q1
u QTxu
acs kf lqg.4
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
6/37
1.3 Dynamic programming
Principle of optimality: From any point on optimal trajectory,remaining trajectory is also optimal
t1
t2t3
allows to determine best control law over period[t2, t3]
independent of how state att2was reached
ForN-step problem:
start from end at time k=Nnow we can determine best control law for last step indepen-
dent of how state at time N1was reached
iterate backward in time to initial time k= 0 acs kf lqg.5
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
7/37
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)withv,eGaussian zero-mean white noise process with
Ev(k)vT(k)=R1, Ee(k)eT(k)=R2, Ev(k)eT(k)=R12andx(0)Gaussian distributed with
E
x(0)
=m0
cov(x(0)):=Ex(0)E [x(0)]x(0)E [x(0)]T=R0 acs kf lqg.6
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
8/37
2.1 Problem formulation
Given the data
Yk={y(i), u(i) : 0ik}
find the best (to be defined) estimate x(k+ m)ofx(k+ m).(m= 0filtering,m>0prediction,m
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
9/37
Some history
Norbert Wiener: Filtering, prediction, and smoothing using integralequations. Spectral factorizations.
Rudolf E. Kalman: Filtering, prediction, and smoothing using state-
space formulas. Riccati equations.
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
10/37
2.2 Kalman filter structure
Goal is to estimate x(k+ 1)by linear combination of previous in-puts and outputs
Estimator (cf. lecture on observers):x(k+ 1|k) = x(k|k1) + u(k) + K(k)
y(k)Cx(k|k1)
with x(k+ 1|k)estimate of statexat sample stepk+ 1using infor-
mation available at step k
Error dynamics x=x xgoverned by
x(k+ 1) =x(k+ 1) x(k+ 1|k)
= x(k) + u(k) + v(k) x(k|k1)u(k)K(k)y(k)Cx(k|k1)
= x(k) + v(k) x(k|k1)K(k)
Cx(k) + e(k)Cx(k|k1)
= (K(k)C) x(k) + v(k)K(k)e(k)
acs kf lqg.8
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
11/37
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) =m0, then Ex(0)
= E
x(0) x(0)
= E
x(0)
m0= 0and
thus E x(k)= 0for all kHow to choose K(k)? minimize covariance of x(k)
We have
cov( x(k)) = E
x(k)E [ x(k)]
x(k)E [ x(k)]T
= E
x(k) xT(k)
So if we defineP(k) = E x(k) xT(k), then we have to determineKalman gain such thatP(k)is minimized
acs kf lqg.9
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
12/37
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) xT(k+ 1)= E
(K(k)C) x(k) + v(k)K(k)e(k)
(K(k)C) x(k) + v(k)K(k)e(k)
T
Since x(k)is independent ofv(k)ande(k), this results in
P(k+ 1) = E
(K(k)C) x(k) xT(k)(K(k)C)T
+ v(k)vT(k) + K(k)e(k)eT(k)KT(k) v(k)eT(k)KT(k)K(k)e(k)vT(k)= (K(k)C)E x(k) xT(k) P(k)
(K(k)C)T +R1+ K(k)R2KT(k)
R12KT(k)K(k)RT12
acs kf lqg.10
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
13/37
2.3 Determination of Kalman gain (continued)
SoP(k+ 1) = (K(k)C)P(k)(K(k)C)T +R1+ K(k)R2K
T(k)R12KT(k)K(k)RT12
=K(k)CP(k)CT +R2KT(k) P(k)CT +R12KT(k)K(k)CPT(k)T +RT12+
P(k)T +R1
MinimizeP(k+ 1)withK(k)as decision variable
P(k+ 1)is quadratic function ofK(k)Use completing-of-squares solution with u=KT(k)andx=I:
K(k)uT
CP(k)CT +R2 Qu
KT(k) u
+P(k)CTR12 Qxu
KT(k) u
+ K(k)uT CPT(k)TRT
12 QTxu+P(k)
T +R1 Qx acs kf lqg.11
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
14/37
2.3 Determination of Kalman gain (continued)
Results in:
KT(k) =L(k)x=L(k)
L(k) =Q1u Q
Txu= CP(k)CT +R21P(k)CTR12T
SoK(k) =LT(k) = P(k)C
T +R12CP(k)CT +R2
1
Furthermore,
P(k+ 1) =QxLTQuL
= P(k)T +R1K(k)CP(k)CT +R2KT(k)= P(k)T +R1
P(k)CT +R12
CP(k)CT +R2
1CP(k)T +RT12
acs kf lqg.12
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
15/37
2.3 Determination of Kalman gain (continued)
Initial value: P(0) = E
x(0) xT(0)
= E
x(0) x(0)
x(0) x(0)
T
= Ex(0)m0x(0)m0T= cov(x(0)) =R0
Overall solution:x(k+ 1|k) = x(k|k1) + u(k) + K(k)
y(k)Cx(k|k1)
K(k) = P(k)C
T +R12CP(k)CT +R2
1
P(k+ 1) = P(k)T +R1 K(k)CP(k)CT +R2)KT(k)P(0) =R0
x(0|1) =m0
acs kf lqg.13
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
16/37
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
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
17/37
2.4 Common equivalent implementation (continued)
Step 1.(Corrector - use the most recent measurement)
x(k|k) = x(k|k1) + K(k) (y(k)Cx(k|k1))
K(k) =P(k|k1)CTCP(k|k1)CT +R21P(k|k) =P(k|k1)K(k)CP(k|k1)
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
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
18/37
2.5 Comments on Kalman filter solution
From the Kalman gainK(k)update equation, we see that a largeR2(much measurement noise) leads to low influence of error onestimate.
The P(k)estimation error covariance is a measure of the uncer-tainty of the estimate. It is updated as
P(k+1
) =
P(k)T
+R1
K(k)CP(k)CT +R2)KT(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
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
19/37
2.5 Comments on Kalman filter solution (continued)
The Kalman filter gives an unbiased estimate, i.e.,
E [ x(k|k)] = E [ x(k|k1)] = E [x(k)]
If the noise is uncorrelated with x(0), then the Kalman filter isoptimal, i.e., no other linearfilter gives a smaller variance of theestimation error.
(For non-Gaussian assumptions, nonlinear filters, particle filtersor moving-horizon estimators do a much better job.)
acs kf lqg.17
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
20/37
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 noiseehas standard deviation (soR2= 2)
x(0)has covarianceR0= 0.5No process noisev, soR1= 0andR12= 0
Kalman filter is given by
x(k+ 1|k) = x(k|k1) + K(k)
y(k) x(k|k1)
K(k) = P(k)P(k) +2P(k+ 1) =P(k)K(k)(P(k) + 2)KT(k) =
2P(k)
P(k) + 2
P(0) =R0= 0.5 x(0|1) =m0= 0 acs kf lqg.18
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
21/37
2.6 Example (continued)
0 100 200 300 400 500 6002
1
0
0 100 200 300 400 500 6000
0.5
0 100 200 300 400 500 600
0
0.5
k
x(k)
K(k)
P(k)
acs kf lqg.19
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
22/37
2.6 Example (continued)
0 100 200 300 400 500 600
2
1
0
0 100 200 300 400 500 6002
1
0
0 100 200 300 400 500 6002
1
0
k
x(k)
x(k)
x(k)
K= 0.01
K=0.05
Optimal gain
acs kf lqg.20
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
23/37
Example To think about
1. What is the problem with a large (small)Kin 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, andR0are often tuning parameters. What are
their influence on the estimate?
acs kf lqg.21
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
24/37
Example To think about (continued)
0 100 200 300 400 500 6002
1
0
0 100 200 300 400 500 600
2
1
0
0 100 200 300 400 500 6002
1
0
k
x(k)
x(k)
x(k)
K=0.01
K=0.05
Optimal gain
Large fixedKrapid initial convergence, but large steady-state varianceSmall fixedKslower convergence, but better performance in steady state
acs kf lqg.22
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
25/37
Steady-state Kalman gain
Recall:
P(k+ 1) = P(k)T +R1
P(k)CT +R12
CP(k)CT +R21
CP(k)T +RT12
Steady-state solution given byP= PT +R1
PCT +R12
CPCT +R2
1CPT +RT
12
is also Riccati equation!
Note: compare with steady-state LQ controller:
S= TS + Q1 TS+ Q12
T(TS+ Q2)
1
TS + QT
12
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
26/37
Duality
Equivalence between LQ control problem and Kalman filter stateestimation problem
LQ Kalmank N k
T
CT
Q0 R0
Q1 R1
Q12 R12
S P
L KT
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
27/37
How to find Kalman filter using matlab?
Command[KEST,L,P] = kalman(SYS,QN,RN,NN)
Calculates (full) Kalman estimatorKESTfor systemSYS
x[n+1] = Ax[n] + Bu[n] + Gw[n] {State equation}y[n] = Cx[n] + Du[n] + Hw[n] + v[n] {Measurements}
with known inputsu, process noisew, measurement noisev, and
noise covariancesE{ww} = QN, E{vv} = RN, E{wv} = NN,
Note: ConstructSYSasSYS=ss(A,[B G],C,[D H],-1)
Also returns steady-state estimator gainLand steady-state errorcovariance
P = E{(x - x[n|n-1])(x - x[n|n-1])} (Riccati solution)
acs kf lqg.23
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
28/37
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
Ex(0)=m0, cov(x(0)) =R0, Ev(k)e(k)v(k)
e(k)T
= R1 R12RT12 R2 find linear control lawy(0),y(1), . . . ,y(k1) u(k)that minimizes
EN1
k=0
xTQ1x + 2x
TQ12u + uTQ2u
+xT(N)Q0x(N)
Solution: Separation principle acs kf lqg.24
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
29/37
3.1 Separation principle
Makes it possible to use control law
u(k) =L x(k|k1)
(sou(k) =Lx(k) +L x(k)) with closed-loop dynamicsx(k+ 1) = x(k)Lx(k) + L x(k) + v(k)
and to view term L x(k)as part of noise
solve LQ problem and estimation problem separately
acs kf lqg.25
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
30/37
Proof of separation principle
Solution of optimal observer design problem does not depend oninputuSo using state feedback does not influence optimality
Kalman filter still optimal
Usingu(k) =L(k) x(k|k1)results in closed-loop system
x(k+ 1|k) = L(k) x(k|k1) + K(k)yCx(k|k1) 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|k1) + K(k)w(k)
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
31/37
Proof of separation principle (continued)
For simplicity we assume Q0= 0
Foru=L x, control design problem in terms of xand xbecomes
minL
EN1
k=0
xTQ1x + 2xTQ12u + uTQ2u
=min
L EN1
k=0 (x
+x
)
TQ1(
x+
x) +
2
(x
+x
)
TQ12L x+
xTLTQ2L xSince it can be shown that E
xTQ x
= 0, we get
minL
EN1
k=0
xTQ1 x + xTQ1 x + 2 x
TQ12L x + xTLTQ2L x
E xTQ xdoes not depend on Land is in fact minimal for Kalman
gainK
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
32/37
Proof of separation principle (continued)
Hence, we get
minL
EN1
k=0
xTQ1 x + 2 xTQ12L x + x
TLTQ2L xsubject to
x(k+ 1|k) = (L(k)) x(k|k1) + K(k)w(k)
= stochastic LQ problem (but with xinstead ofxand
withu=L xalready filled in)
L(k)as computed before still optimal
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
33/37
3.2 LQG problem: Solution
System
State feedback
LQ gain Kalman gain
State estimator+
+
Measurement
Process
L controller
noise
noiseK(k)L(k)
uu
x
y
acs kf lqg.26
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
34/37
Stationary LQG control
Solution:u(k) =L x(k|k1)
with
x(k+ 1|k) = x(k|k1) + u(k) + K(y(k)Cx(k|k1)
Closed-loop dynamics (with error state x(k), see slide acs kf lqg.8):
x(k+ 1) = (L)x(k) + L x(k) + v(k)
x(k+ 1) = (KC) x(k) + v(k)Ke(k)
or
x(k+ 1)x(k+ 1)= L L0 KCx(k)x(k)+IIv(k) + 0Ke(k)dynamics of closed-loop system determined by dynamics of
LQ controller and of optimal filter acs kf lqg.27
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
35/37
How to construct LQG controller using matlab?
Command RLQG = lqgreg(KEST,K)produces LQG controller byconnecting Kalman estimatorKESTdesigned withkalmanandstate-feedback gainKdesigned withdlqr
acs kf lqg.28
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
36/37
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
8/10/2019 Kalman filtering and LQG control --- acs10-12.pdf
37/37
Summary
Kalman filtering
state estimator such that error covariance is minimized
LQG controlseparation principleLQ + Kalman
acs kf lqg.30