24. LQG Controllers MAE 546 2018 - Princeton University · Proposition: LQG and LQ robustness would...

Linear-Quadratic-Gaussian Controllers Robert Stengel

Optimal Control and Estimation MAE 546 Princeton University, 2018

Copyright 2018 by Robert Stengel. All rights reserved. For educational use only.http://www.princeton.edu/~stengel/MAE546.html

http://www.princeton.edu/~stengel/OptConEst.html

!  LTI dynamic system!  Certainty Equivalence and the Separation

Theorem!  Asymptotic stability of the constant-gain

LQG regulator!  Coupling due to parameter uncertainty!  Robustness (loop transfer) recovery!  Stochastic Robustness Analysis and Design!  Neighboring-optimal Stochastic Control

1

The Problem: Minimize Cost, Subject to Dynamic Constraint, Uncertain

Disturbances, and Measurement Error

!x t( ) = Fx(t)+Gu(t)+Lw(t), x 0( ) = xoz t( ) = Hx t( ) + n t( )

= 12minuE E xT (t f )S(t f )x(t f ) | ID⎡⎣ ⎤⎦ + E xT (t) uT (t)⎡

⎣⎤⎦Q 00 R

⎡

⎣⎢

⎤

⎦⎥x(t)u(t)

⎡

⎣⎢⎢

⎤

⎦⎥⎥dt

0

t f

∫ ID

⎧⎨⎪

⎩⎪

⎫⎬⎪

⎭⎪

⎧⎨⎪

⎩⎪

⎫⎬⎪

⎭⎪

Dynamic System

Cost Function

2

ID t( ) = x t( ),P t( ),u t( ){ }

minuV to( ) = min

uJ t f( )

Initial Conditions and DimensionsE x 0( )⎡⎣ ⎤⎦ = xo; E x 0( )− x 0( )⎡⎣ ⎤⎦ x 0( )− x 0( )⎡⎣ ⎤⎦

T{ } = P 0( )E w t( )⎡⎣ ⎤⎦ = 0; E w t( )wT τ( )⎡⎣ ⎤⎦ =Wδ t −τ( )E n t( )⎡⎣ ⎤⎦ = 0; E n t( )nT τ( )⎡⎣ ⎤⎦ = Nδ t −τ( )

E w t( )nT τ( )⎡⎣ ⎤⎦ = 0

dim x t( )⎡⎣ ⎤⎦ = n ×1

dim u t( )⎡⎣ ⎤⎦ = m ×1

dim w t( )⎡⎣ ⎤⎦ = s ×1

dim z t( )⎡⎣ ⎤⎦ = dim n t( )⎡⎣ ⎤⎦ = r ×1

Statistics

Dimensions

3

Linear-Quadratic-Gaussian Control

4

Separation Property and Certainty Equivalence

•  Separation Property–  Optimal Control Law and Optimal Estimation Law can be

derived separately–  Their derivations are strictly independent

•  Certainty Equivalence Property–  Separation property plus, ...–  The Stochastic Optimal Control Law and the Deterministic

Optimal Control Law are the same–  The Optimal Estimation Law can be derived separately

•  Linear-quadratic-Gaussian (LQG) control is certainty-equivalent

5

The Equations (Continuous-Time Model)

!x t( ) = F(t)x(t)+G(t)u(t)+L(t)w(t)z t( ) = Hx t( ) + n t( )

u t( ) = −C(t)x(t)+CF (t)yC (t)

!x t( ) = F(t)x(t)+G(t)u(t)+K t( ) z t( )−Hx t( )⎡⎣ ⎤⎦= F(t)−G(t)C t( )−K t( )H⎡⎣ ⎤⎦ x(t)+G t( )CF (t)yC (t)+K t( )z t( )

K t( ) = P(t)HTN−1 t( )!P(t) = F(t)P(t)+ P(t)FT (t)+L t( )W t( )LT t( )− P(t)HTN−1 t( )HP(t)

C t( ) = R−1 t( )GT t( )S t( )!S t( ) = −Q(t)− F(t)T S t( )− S t( )F(t)+ S t( )G(t)R−1(t)GT (t)S t( )

System State and Measurement

Control Law

State Estimate

Estimator Gain and State Covariance Estimate

Control Gain and Adjoint Covariance Estimate

6

Estimator in the Feedback Loop

u t( ) = −C(t)x(t)

x t( ) = Fx(t) +Gu(t) +K t( ) z t( ) −Hx t( )⎡⎣ ⎤⎦

K t( ) = P(t)HTN−1

P(t) = FP(t)+ P(t)FT +LWLT − P(t)HTN−1HP(t)

Linear-Gaussian (LG) state estimator adds dynamics to the feedback signal

Thus, state estimator can be viewed as a control-law “compensator”Bandwidth of the compensation is dependent on the multivariable

signal/noise ratio, [PHT]N–1

7

Stable Scalar LTI Example of Estimator Compensation

!x = − x + K z − Hx( ) = −1− KH( ) x + Kz

Estimator transfer function

Low-pass filter

x s( ) = Ks − −1− KH( )⎡⎣ ⎤⎦

z s( )

s − −1− KH( )⎡⎣ ⎤⎦ x s( ) = Kz s( )Laplace transform of estimator

Estimator differential equation

!x = −x +w; z =Hx +nDynamic system (stable) and measurement

x s( )z s( ) =

Ks − −1− KH( )⎡⎣ ⎤⎦

8

Unstable Scalar LTI Example of Estimator Compensation

x = x + K z − Hx( ) = 1− KH( ) x + Kz

Estimator transfer function

Low-pass filter

x s( ) = Ks − 1− KH( )⎡⎣ ⎤⎦

z s( )

s − 1− KH( )⎡⎣ ⎤⎦ x s( ) = Kz s( )Laplace transform of estimator

Estimator differential equation

!x =+x +w; z =Hx +nDynamic system (unstable) and measurement

x s( )z s( ) =

Ks − 1− KH( )⎡⎣ ⎤⎦

9

Steady-State Scalar Filter Gain

0 = 2P +W −P2H 2

N; P2 −

2NH 2 P −

WNH 2 = 0

Constant, scalar filter gain

P = Signal "Power"= State EstimateVarianceN = Noise "Power"= Measurement Error VarianceH = Projection from Measurement Space to Signal SpaceW = Disturbance Covariance

K =PHN

Algebraic Riccati

equation(unstable case) P =

NH 2 ±

NH 2

⎛⎝⎜

⎞⎠⎟2

+WNH 2 =

NH 2 1± 1+WH

2

N

⎡

⎣⎢⎢

⎤

⎦⎥⎥

10

Steady-State Filter Gain

K =

NH 2 1+ 1+WH

2

N

⎡

⎣⎢⎢

⎤

⎦⎥⎥

⎧⎨⎪

⎩⎪

⎫⎬⎪

⎭⎪H

N=

1H1+ 1+WH

2

N

⎡

⎣⎢⎢

⎤

⎦⎥⎥

⎧⎨⎪

⎩⎪

⎫⎬⎪

⎭⎪

K W >>N⎯ →⎯⎯WN

11

Dynamic Constraint on the Certainty-Equivalent Cost

minuJ = min

uJCE + JS

P(t) is independent of u(t); therefore

JCE is Identical in form to the deterministic cost function

Minimized subject to dynamic constraint based on the state estimate

x t( ) = Fx(t) +Gu(t) +K t( ) z t( ) −Hx t( )⎡⎣ ⎤⎦

12

Kalman-Bucy Filter Provides Estimate of the State Mean Value

Filter residual is a Gaussian process

!x t( ) = Fx(t)+Gu(t)+K t( ) z t( )−Hx t( )⎡⎣ ⎤⎦" Fx(t)+Gu(t)+K t( )ε t( )

Filter equation is analogous to deterministic dynamic constraint on

deterministic cost function

x t( ) = Fx(t) +Gu(t) + Lw(t)13

Control That Minimizes the Certainty-Equivalent Cost

Optimizing control history is generated by a time-varying feedback control law

u t( ) = −C(t)x(t)The control gain is the same as

the deterministic gain

C t( ) = R−1GTS t( )!S t( ) = −Q− FTS t( )− S t( )F + S t( )GR−1GTS t( )

S t f( ) given

14

Optimal Cost for the Continuous-Time LQG Controller

Certainty-equivalent cost

JCE =12Tr S(0)E x 0( ) xT 0( )⎡⎣ ⎤⎦ + S t( )K t( )NKT t( )dt

0

t f

∫⎡

⎣⎢⎢

⎤

⎦⎥⎥

J = JCE + JS

= 12Tr S(0)E x 0( ) xT 0( )⎡⎣ ⎤⎦ + S t( )K t( )NKT t( )dt

0

t f

∫⎧⎨⎪

⎩⎪

⎫⎬⎪

⎭⎪

+ 12Tr S(t f )P(t f )+ QP t( )dt

0

t f

∫⎡

⎣⎢⎢

⎤

⎦⎥⎥

Total cost

15

Discrete-Time LQG ControllerKalman filter produces state estimate

xk −( ) = Φxk−1 +( ) + ΓCk−1xk−1 +( )

xk+1 = Φxk − ΓCkxk +( )

xk +( ) = xk −( ) +Kk zk −Hxk −( )⎡⎣ ⎤⎦Closed-loop system uses state estimate for feedback control

16

Response of Discrete-Time 1st-Order System to Disturbance

Kalman Filter Estimate from Noisy Measurement

Propagation of Uncertainty Kalman Filter, Uncontrolled System

17

Comparison of 1st-Order Discrete-Time LQ and LQG Control Response

Linear-Quadratic Control with Noise-free Measurement

Linear-Quadratic-Gaussian Control with Noisy

Measurement

18

Asymptotic Stability of the LQG Regulator

19

System Equations with LQG Control

ε t( ) = x t( ) − x t( )

x t( ) = Fx(t)+Gu(t)+Lw(t)x t( ) = Fx(t)+Gu(t)+K t( ) z t( )−Hx t( )⎡⎣ ⎤⎦

ε t( ) = F −KH( )ε t( ) + Lw t( ) −Kn t( )

With perfect knowledge of the system

State estimate error

State estimate error dynamics

20

Control-Loop and Estimator Eigenvalues are Uncoupled

!x t( )!ε t( )

⎡

⎣⎢⎢

⎤

⎦⎥⎥=

F −GC( ) GC0 F −KH( )

⎡

⎣⎢⎢

⎤

⎦⎥⎥

x t( )ε t( )

⎡

⎣⎢⎢

⎤

⎦⎥⎥+ L 0

L −K⎡

⎣⎢

⎤

⎦⎥w t( )n t( )

⎡

⎣⎢⎢

⎤

⎦⎥⎥

Upper-block-triangular stability matrixLQG system is stable because

(F – GC) is stable(F – KH) is stable

Estimate error affects state response

Actual state does not affect error responseDisturbance affects both equally

x t( ) = F −GC( )x t( ) +GCε t( ) + Lw t( )

21

Parameter Uncertainty Introduces Coupling

22

Coupling Due To Parameter Uncertainty

!x t( )!ε t( )

⎡

⎣⎢⎢

⎤

⎦⎥⎥=

FA −GAC( ) GAC

FA − F( )− GA −G( )C−K HA −H( )⎡⎣ ⎤⎦ F + GA −G( )C−KH⎡⎣ ⎤⎦

⎡

⎣

⎢⎢

⎤

⎦

⎥⎥x t( )ε t( )

⎡

⎣⎢⎢

⎤

⎦⎥⎥+"

Actual System: FA ,GA ,HA{ }Assumed System: F,G,H{ }

x t( ) = FAx(t)+GAu(t)+Lw(t)x t( ) = Fx(t)+Gu(t)+K t( ) z t( )−Hx t( )⎡⎣ ⎤⎦

Closed-loop control and estimator responses are coupled

z t( ) = HAx(t)+ n t( )u(t) = −C t( ) x t( )

23

Effects of Parameter Uncertainty on Closed-Loop Stability

sI2n − FCL =

sIn − FA −GAC( )⎡⎣ ⎤⎦ −GAC

− FA − F( )− GA −G( )C−K HA −H( )⎡⎣ ⎤⎦ sIn − F + GA −G( )C−KH⎡⎣ ⎤⎦{ }= ΔCL s( ) = 0

!  Uncertain parameters affect closed-loop eigenvalues!  Coupling can lead to instability for numerous reasons

!  Improper control gain!  Control effect on estimator block!  Redistribution of damping

24

Doyle’s Counter-Example of LQG Robustness (1978)

x1x2

⎡

⎣⎢⎢

⎤

⎦⎥⎥= 1 1

0 1⎡

⎣⎢

⎤

⎦⎥

x1x2

⎡

⎣⎢⎢

⎤

⎦⎥⎥+ 0

1⎡

⎣⎢

⎤

⎦⎥u +

11

⎡

⎣⎢

⎤

⎦⎥w

z = 1 0⎡⎣ ⎤⎦x1x2

⎡

⎣⎢⎢

⎤

⎦⎥⎥+ n

Q = Q 1 11 1

⎡

⎣⎢

⎤

⎦⎥; R = 1; W =W 1 1

1 1⎡

⎣⎢

⎤

⎦⎥; N = 1

C = 2 + 4 +Q( ) 1 1⎡⎣ ⎤⎦ = c c⎡⎣ ⎤⎦

K = 2 + 4 +W( ) 11

⎡

⎣⎢

⎤

⎦⎥ =

kk

⎡

⎣⎢

⎤

⎦⎥

Unstable Plant

Design Matrices

Control and Estimator Gains

25

Uncertainty in the Control Effect

s −1( ) −1 0 0

0 s −1( ) µc µc

−k 0 s −1+ k( ) −1

−k 0 c + k( ) s −1+ c( )

= 0

FA = F; GA =0µ

⎡

⎣⎢⎢

⎤

⎦⎥⎥; HA = H

System Matrices

Characteristic Equation

s4 + a3s3 + a2s

2 + a1s + a0 = ΔCL s( ) = 026

Stability Effect of Parameter Variation

a1 = k + c − 4 + 2 µ −1( )cka0 = 1+ 1− µ( )ck

Routh's Stability Criterion (necessary condition)• All coefficients of Δ(s) must be positive for stability

• Arbitrarily small uncertainty, µ = 1 + ε , could cause unstability• Not surprising: uncertainty is in the control effect

• µ is nominally equal to 1• µ can force a0 and a1 to change sign• Result is dependent on magnitude of ck

27

The Counter-Example Raises a FlagSolution

Choose Q and W to be small, increasing allowable range of μ

!  However, .... The counter-example is irrelevant because it does not satisfy the requirements for LQ and LG stability!  The open-loop system is unstable, so it requires feedback

control to restore stability!  To guarantee stability, Q and W must be positive definite, but

Q =Q 1 11 1

⎡

⎣⎢

⎤

⎦⎥; hence, Q = 0

W =W 1 11 1

⎡

⎣⎢

⎤

⎦⎥; hence, W = 0

28

Restoring Robustness (Loop Transfer Recovery)

29

Loop-Transfer Recovery (Doyle and Stein, 1979)

!  Proposition: LQG and LQ robustness would be the same if the control vector had the same effect on the state and its estimate

Cx t( ) and Cx t( )produce same expected value of control, E u t( )⎡⎣ ⎤⎦

!  Therefore, restoring the correct mean value from z(t) restores closed-loop robustness

!  Solution: Increase the assumed “process noise” for estimator design as follows (see text for details)

W =Wo + k2GGT

30

but not the same

E uLQ t( )− uLQG t( )⎡⎣ ⎤⎦ uLQ t( )− uLQG t( )⎡⎣ ⎤⎦T{ }

as x t( ) contains measurement errors but x t( ) does not

!  Analogous solution in Kwakernaak, H., and Sivan, R., Linear Optimal Control Systems, 1972

Stochastic Robustness Analysis and Design

31

Expression of Uncertainty in the System Model

•  Variation may be–  Deterministic, e.g.,

•  Upper/lower bounds (“worst-case”)–  Probabilistic, e.g.,

•  Gaussian distribution•  Bounded variation is equivalent to probabilistic

variation with uniform distribution

System uncertainty may be expressed as• Elements of F

• Coefficients of Δ(s)• Eigenvalues, λ

• Frequency response/singular values/time response, A(jω ), σ (jω ), x(t)

32

Stochastic Root Locus:Uncertain Damping Ratio and

Natural Frequency

Uniform Distribution of

Eigenvalues

Gaussian Distribution of

Eigenvalues

33

Probability of Instability

•  Nonlinear mapping from probability density functions (pdf) of uncertain parameters to pdf of roots

•  Finite probability of instability with Gaussian (unbounded) distribution

•  Zero probability of instability for some uniform distributions

34

Probabilistic Control Design•  Design constant-parameter controller (CPC) for satisfactory

stability and performance in an uncertain environment•  Monte Carlo Evaluation of simulated system response with

–  competing CPC designs [Design parameters = d]–  given statistical model of uncertainty in the plant [Uncertain plant

parameters = v]

•  Search for best CPC–  Exhaustive search–  Random search–  Multivariate line search–  Genetic algorithm–  Simulated annealing

35

Design Outcome Follows Binomial Distribution

•  Binomial distribution: Satisfactory/Unsatisfactory•  Confidence intervals of probability estimate are functions of

–  Actual probability–  Number of trials

1e+01

1e+03

1e+05

1e+07

1e+09

1e-06 1e-05 1e-04 0.001 0.01 0.1 1

2%5%10%20%

100%

Num

ber

of E

valu

atio

ns

0.5

IntervalWidth

Probability or (1 - Probability)Binomial Distribution

Maximum Information Entropy when Pr = 0.5

Number of Evaluations

Confidence Interval

36

Example: Probability of Stable Control of an Unstable Plant

F =

−2gf11 /V ρV 2 f12 / 2 ρVf13 −g

−45 /V 2 ρVf22 / 2 1 0

0 ρV 2 f32 / 2 ρVf33 00 0 1 0

⎡

⎣

⎢⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥⎥

Longitudinal dynamics for a Forward-Swept-Wing AirplaneX-29 Aircraft

37

G =

g11 g12

0 0g31 g32

0 0

⎡

⎣

⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥

; x =

V , Airspeedα , Angle of attack

q, Pitch rateθ , Pitch angle

⎡

⎣

⎢⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥⎥

Ray, Stengel, 1991

Example: Probability of Stable Control of an Unstable Plant

p = ρ V f11 f12 f13 f22 f32 f33 g11 g12 g31 g32⎡⎣

⎤⎦T

λ1−4 = −0.1± 0.057 j, − 5.15, 3.35

Nominal eigenvalues (one unstable)

Environment Uncontrolled Dynamics Control Effect

Air density and airspeed, ρ and V , have uniform distributions(±30%)

10 coefficients have Gaussian distributions (σ = 30%)

38

LQ Regulators for the ExampleCase a) LQR with low control weighting

Case b) LQR with high control weighting

Q = diag 1,1,1,0( ); R = 1,1( ); λ1−4nominal = –35,–5.1,–3.3,–.02

C = 0.17 130 33 0.360.98 −11 −3 −1.1

⎡

⎣⎢

⎤

⎦⎥

Q = diag 1,1,1,0( ); R = 1000,1000( ); λ1−4nominal = −5.2,−3.4,−1.1,−.02

C = 0.03 83 21 −0.060.01 −63 −16 −1.9

⎡

⎣⎢

⎤

⎦⎥

λ1−4nominal = −32,–5.2,–3.4,–0.01

C = 0.13 413 105 −0.320.05 −313 −81 −1.1− 9.5

⎡

⎣⎢

⎤

⎦⎥

Case c) Case b with gains multiplied by 5 for bandwidth (loop-transfer) recovery

Three stabilizing feedback control laws

39

Stochastic Root Loci!  Distribution of closed-loop roots with

!  Gaussian uncertainty in 10 parameters!  Uniform uncertainty in velocity and air

density!  25,000 Monte Carlo evaluations

Stochastic Root Locus

!  Probability of instability!  a) Pr = 0.072!  b) Pr = 0.021!  c) Pr = 0.0076

40

Case a Case b

Case c

Ray, Stengel, 1991

Probabilities of Instability for the Three Cases

41

Stochastic Root Loci for the Three Cases

Case a: Low LQ Control Weights

Case b: High LQ Control Weights

Case c: Bandwidth Recovery

with Gaussian Aerodynamic Uncertainty

•  Probabilities of instability with 30% uniform aerodynamic uncertainty–  Case a: 3.4 x 10-4–  Case b: 0–  Case c: 0

42

American Control Conference Benchmark Control Problem, 1991•  Parameters of 4th-order mass-spring system

–  Uniform probability density functions for •  0.5 < m1, m2 < 1.5 •  0.5 < k < 2

•  Probability of Instability, Pi–  mi = 1 (unstable) or 0 (stable)

•  Probability of Settling Time Exceedance, Pts –  mts = 1 (exceeded) or 0 (not exceeded)

•  Probability of Control Limit Exceedance, Pu –  mu = 1 (exceeded) or 0 (not exceeded)

•  Design Cost Function•  10 controllers designed for the competition

J = aPi2 + bPts

2 + cPu2

43

Stochastic LQG Design for Benchmark Control Problem

•  SISO Linear-Quadratic-Gaussian Regulators (Marrison)–  Implicit model following with control-rate weighting and

scalar output (5th order)–  Kalman filter with single measurement (4th order)–  Design parameters

•  Control cost function weights•  Springs and masses in ideal model•  Estimator weights

–  Search•  Multivariate line search•  Genetic algorithm

44

Comparison of Design Costs for Benchmark Control Problem

Cost Emphasizes Instability Cost Emphasizes

Excess ControlCost Emphasizes

Settling-Time Exceedance

Competition Designs

Four SRAD LQG

Designs

J = Pi2 + 0.01Pts

2 + 0.01Pu2 J = 0.01Pi

2 + 0.01Pts2 + Pu

2 J = 0.01Pi2 + Pts

2 + 0.01Pu2

Stochastic LQG controller more robust in 39 of 40 benchmark controller comparisons45

Marrison, Stengel, 1995

Estimation of Minimum Design Cost Using Jackknife/Bootstrapping

Evaluation

2.24 2.25 2.26 2.27 2.28 2.29 2.3 2.31 2.320

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Design Cost Estimate x 103

Pr(J < J0)

Genetic Algorithm Results

Corresponding Weibull Distribution

46Marrison, Stengel, 1995

Neighboring-Optimal Control with Uncertain Disturbance, Measurement,

and Initial Condition

47

Immune Response Example!  Optimal open-loop drug therapy (control)

!  Assumptions !  Initial condition known without error!  No disturbance

!  Optimal closed-loop therapy!  Assumptions

!  Small error in initial condition!  Small disturbance!  Perfect measurement of state

!  Stochastic optimal closed-loop therapy!  Assumptions

!  Small error in initial condition!  Small disturbance!  Imperfect measurement!  Certainty-equivalence applies to

perturbation control48

Immune Response Example with Optimal

Feedback ControlOpen-Loop Optimal Control for Lethal Initial Condition

Open- and Closed-Loop Optimal Control for 150% Lethal Initial Condition

49Ghigliazza, Kulkarni, Stengel, 2002

Immune Response with Full-State Stochastic Optimal Feedback Control

(Random Disturbance and Measurement Error Not Simulated)

Low-Bandwidth Estimator (|W| < |N|)

High-Bandwidth Estimator (|W| > |N|)

!  Initial control too sluggish to prevent divergence

!  Quick initial control prevents divergence

50Ghigliazza, Stengel, 2004

W = I4N = I2 / 20

Stochastic-Optimal Control (u1) with Two Measurements (x1, x3)

51

Immune Response to Random Disturbance with Two-Measurement

Stochastic Neighboring-Optimal Control•  Disturbance due to

–  Re-infection–  Sequestered “pockets”

of pathogen•  Noisy measurements•  Closed-loop therapy is

robust •  ... but not robust enough:

–  Organ death occurs in one case

•  Probability of satisfactory therapy can be maximized by stochastic redesign of controller

52

The End!

53