Kalman filtering and LQG control --- acs10-12.pdf

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


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


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


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


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


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


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/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


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.


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


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


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


13/37


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


14/37


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


15/37


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


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


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


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


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


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


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


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


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


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


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


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


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


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


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)


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


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


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


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


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


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


37/37

Summary

Kalman filtering

state estimator such that error covariance is minimized

LQG controlseparation principleLQ + Kalman

acs kf lqg.30

Documents

Kalman filtering and LQG control --- acs10-12.pdf