linear quadratic gaussian - university of colorado

ECE5530: Multivariable Control Systems II. 6–1

LINEAR QUADRATIC GAUSSIAN

6.1: Deriving LQG via separation principle

■ We will now start to look at the design of controllers for systems

Px.t/ D A.t/x.t/ C Bu.t/u.t/ C Bw.t/w.t/

y.t/ D Cy.t/x.t/ C v.t/:

where w.t/, v.t/ are white, Gaussian, zero-mean, independent.

v.t/ ! N .0; Sv/; w.t/ ! N .0; Sw/; x.0/ ! N .x0; „x;0/:

■ Time-varying plant dynamics can be handled quite easily, but we will

concentrate on time-invariant case.

OBJECTIVE: Design a compensator U D Gc.s/Y so that the closed-loop

system is stable, and system achieves “good performance.”

■ Our goal will be to minimize the cost function

J D E

!

1

2xT .tf /Hx.tf / C

1

2

Z tf

0

xT .t/Qx.t/ C uT .t/Ru.t/ dt

"

;

where H; Q; R > 0 and we measure only the system output.

■ We have seen two versions of this problem already:

1. Noise-free: u.t/ D "K.t/x.t/.

2. Noisy, but with complete state observations: u.t/ D "K.t/x.t/.

Same K.t/ but cost is increased.

3. Now, noisy output observations. We will show that

u.t/ D "K.t/ Ox.t/ where Ox.t/ is the output of a Kalman filter.

Lecture notes prepared by Dr. Gregory L. Plett. Copyright © 2016, 2010, 2008, 2006, 2004, 2002, 2001, 1999, Gregory L. Plett

ECE5530, LINEAR QUADRATIC GAUSSIAN 6–2

■ This is called the separation principle—using estimate Ox.t/ as if it

were the true state.

KEY POINT: This is exactly the same separation operation that we

performed before when we designed state-space controllers via

pole-placement techniques.

➠ Validation of that procedure was based only on the stability of the

closed-loop system.

■ We are now saying that the separation into two independent optimal

sub-problems is actually what optimizes the original objective.

➠ On one hand, the separation is intuitively appealing: Ox.t/ is our

best guess of what is going on inside the system. So, in some sense,

it is clear we should use it in the regulator.

➠ But to extend this intuition further and postulate that it is actually

the optimal procedure to follow is not obvious. (e.g., Noise )

Uncertainty ) Reduced control gains??)

Separation Principle for Optimal Performance

■ Our approach to solving the optimal control problem with finite and

noisy measurements will be

1. Rewrite cost and system in

terms of the estimator states and

dynamics ➠ Can access these.

2. Design a stochastic LQR

controller for this revised system

➠ Full state feedback of Ox.t/.

3. Connection u.t/ D "K.t/ Ox.t/.

Ox.t/

w.t/

u.t/y.t/

v.t/

"K.t/ Kalmanfilter

Plant



■ Start with the cost

EŒxT Qx! D EŒfx " Ox C OxgT Qfx " Ox C Oxg!

D EŒfx " OxgT Qfx " Oxg! C 2EŒfx " OxgT Q Ox! C EŒ OxT Q Ox!

D EŒ QxT Q Qx! C 2EŒ QxT Q Ox! C EŒ OxT Q Ox!:

■ Each term is a scalar, and unchanged by the trace operator,

EŒxT Qx! D trace.EŒ QxT Q Qx!/ C trace.EŒ QxT Q Ox!/ C EŒ OxT Q Ox!

D trace.EŒ Qx QxT Q!/ C trace.EŒ Ox QxT Q!/ C EŒ OxT Q Ox!

D trace.„xQ/ C EŒ OxT Q Ox!

because Ox and Qx are orthogonal if a Kalman filter is used to estimate

x (Proved in text).

■ We can now write the LQG cost as

J D E

!

1

2OxT .tf /H Ox.tf / C

1

2

Z tf

0

OxT .t/Q Ox.t/ C uT .t/Ru.t/ dt

"

C1

2trace

#

„x.tf /H C

Z tf

0

„x.t/Q dt

$

:

■ The terms within the tracefg operator are independent of the control

input u.t/ so J is minimized whenever

J D E

!

1


1

2

Z tf

0


"

is minimized.

■ This looks like a stochastic LQR problem in the (measurable) state Ox.

But, note that the dynamics of Ox are different from the dynamics of x.

Need to show that the assumptions of LQR are met.



■ Estimator dynamics:

POx.t/ D A Ox.t/ C Buu.t/ C L.t/%

y.t/ " Cy Ox.t/&

:

KEY FACT: The innovations process r.t/ D y.t/ " Cy Ox.t/ is white,

Gaussian ! N .0; Sv/. (Shown in section A12 of book appendix).

■ So, LQG problem is to minimize

J D E

!

1


1

2

Z tf

0


"

C const

subject to the dynamics

POx.t/ D A Ox.t/ C Buu.t/ C L.t/r.t/

L.t/ known;

r.t/ ! N .0; Sv/:

■ This is a (strange looking) stochastic LQR problem.

➠ Solution independent of the white driving noise.

➠ Optimal solution a linear feedback law u.t/ D "K.t/ Ox.t/.

➠ K.t/ is from solution of LQR with data (A, B, Q, R) . . . same.

➠ Therefore, LQG D combination of LQR/LQE is

PERFORMANCE OPTIMAL.



6.2: Satellite-tracking example

EXAMPLE: A satellite tracking antenna, subject to random wind torques,

can be modeled as"

P".t/R".t/

#

D

"

0 1

0 "0:1

# "

".t/P".t/

#

C

"

0

0:001

#

u.t/ C

"

0

0:001

#

w.t/

y.t/ Dh

1 0i

"

".t/P".t/

#

C v.t/;

where

■ ".t/ is the pointing error of the antenna in degrees,

■ u.t/ is the control torque in N m, and

■ w.t/ is the wind torque (disturbance input) in N m.

■ The wind torque is assumed to be white noise with a spectral density

of Sw D 5000 N2 m2 Hz"1.

■ The measurement noise is white with a spectral density

Sv D 1 deg2 Hz"1.

■ The cost function to be minimized is

J D E

!Z 100

0

q"2.t/ C ru2.t/ dt

"

:

■ The simulation is initialized with

Ox.0/ D

"

0

0

#

; „x.0/ D

"

0 0

0 0

#

:

■ The MATLAB code is from Burl. Results follow.



■ First, the baseline example with: q D 180; r D 1; Sw D 5000; Sv D 1.

0 20 40 60 80 1000

20

40

60

80

100

Time (s)

Feed

back

gai

ns

0 20 40 60 80 1000

0.1

0.2

0.3

0.4

Time (s)

Kalm

an g

ains

0 20 40 60 80 100−2

−1

0

1

2

3

4

Time (sec)

Angl

e er

ror (

deg)

ActualEstimated

0 20 40 60 80 100−100

−50

0

50

Time (s)

Con

trol (

N-m

)

■ Increased regulation requirement: q D 18000; r D 1; Sw D 5000;

Sv D 1. State feedback gains are larger and approach steady state

more rapidly, but Kalman gains remain the same. Larger control

inputs and better control performance.

0 20 40 60 80 1000

200

400

600

800

1000

Time (s)

Feed

back

gai

ns

0 20 40 60 80 1000

0.1

0.2

0.3

0.4

Time (s)

Kalm

an g

ains

0 20 40 60 80 100−1

−0.5

0

0.5

1

1.5

Time (sec)

Angl

e er

ror (

deg)

ActualEstimated

0 20 40 60 80 100−400

−200

0

200

400

Time (s)

Con

trol (

N-m

)



■ Increased control-effort penalty: q D 180; r D 100; Sw D 5000; Sv D 1.

State feedback gains smaller, converge more slowly. Kalman gains

remain the same. Smaller control inputs, increased angle errors.

0 20 40 60 80 1000

2

4

6

8

10

12

Time (s)

Feed

back

gai

ns

0 20 40 60 80 1000

0.1

0.2

0.3

0.4

Time (s)

Kalm

an g

ains

0 20 40 60 80 100−15

−10

−5

0

5

Time (sec)

Angl

e er

ror (

deg)

ActualEstimated

0 20 40 60 80 100−2

0

2

4

6

8

10

Time (s)

Con

trol (

N-m

)

■ High disturbance: q D 180; r D 1; Sw D 500000; Sv D 1. Kalman gains

larger, converge much more rapidly. State feedback gains remain the

same. Large gains make estimator faster but larger plant noise

means that estimate is less accurate. Control input also increased

due to larger gains and increased plant noise. Angle error increased

by roughly 10, which is square-root of increase in spectral density.



0 20 40 60 80 1000

20

40

60

80

100

Time (s)

Feed

back

gai

ns

0 20 40 60 80 1000

0.5

1

1.5

Time (s)

Kalm

an g

ains

0 20 40 60 80 100−20

−10

0

10

20

Time (sec)

Angl

e er

ror (

deg)

ActualEstimated

0 20 40 60 80 100−400

−200

0

200

400

Time (s)C

ontro

l (N

-m)

■ Large sensor noise: q D 180; r D 1; Sw D 5000; Sv D 100. Kalman

gains smaller, converge more slowly. State feedback gains remain

same. Smaller Kalman gains make estimator slower and larger

measurement noise makes estimates less accurate. Control is poorer

due to increased measurement noise.

0 20 40 60 80 1000

20

40

60

80

100

Time (s)

Feed

back

gai

ns

0 20 40 60 80 1000

0.01

0.02

0.03

0.04

0.05

0.06

Time (s)

Kalm

an g

ains

0 20 40 60 80 100−4

−2

0

2

4

6

Time (sec)

Angl

e er

ror (

deg)

ActualEstimated

0 20 40 60 80 100−40

−20

0

20

40

Time (s)

Con

trol (

N-m

)



6.3: Steady-state LQG control and the compensator

■ Assume time-invariant plant dynamics.

■ Use steady-state cost. Analysis (Stengel) shows (where

´.t/ D C´x.t/)

JLQG D limt!1

EŒ´T .t/Q´´.t/ C uT .t/Ru.t/!

D trace

#

†x;ss LssSvLTss C „x;ss C T

´ Q´C´

$

D tracen

†x;ss BwSwBTw C „x;ss KT

ssRKss

o

where terms are labeled regulator or estimator and

AT †x;ss C †x;ssA C C T´ Q´C´ " †x;ssBuR"1BT

u †x;ss D 0

A„x;ss C „x;ssAT C BwSwBT

w " „x;ssCTy S"1

v Cy„x;ss D 0

and Lss D „x;ssCTy S"1

v ; Kss D R"1BTu †x;ss.

■ Can evaluate steady-state cost from the solution of two Riccati

equations. More complicated than the stochastic LQR result

JLQR D trace˚

.Q C KTssRKss/†x;ss!

'

:

■ Reason: JLQG must also account for cost increase because of

estimation error.

■ In LQG, Ox ¤ x in general. Concepts of (1) Regulation error (x ¤ 0)

and (2) Estimation error ( Ox ¤ x). Both contribute to the cost.

Further interpretations

FAST REGULATOR: To see that both errors contribute, consider the result

of a very fast regulator ➠ Control effort cheap, R ! 0.



■ Precise analysis requires we find impact of R ! 0 on †x;ss but

JLQG D tracen

†x;ssLssSvLTss C „x;ssC

T´ QĆ´

„ ƒ‚ …

not fn of R

o

# trace˚

„x;ssCT´ QĆ´

'

for all R

In particular limR!0

JLQG # trace˚

„x;ssCT´ QĆ´

'

.

■ Even in the limit of no control penalty, the cost is lower-bounded by a

term associated with the estimation error in the state „x;ss.

FAST ESTIMATOR: Low sensor noise and fast estimation, Sv ! 0.

■ Use second identity to show limSv!0

JLQG # trace˚

†x;ssBwSwBTw

'

.

■ Regulation error remains: lower bound on performance with fast

estimator.

■ Both cases illustrate that it is futile to make either the estimator or the

regulator much faster than the other.

■ Ultimate performance limited and quickly reach point of diminishing

returns.

RULE OF THUMB: Select R, Sv so that estimator and regulator poles are

roughly the same (order of magnitude) distance to origin ➠ Similar

levels of authority. Since Sv is generally physical, this places a design

constraint on R.

The compensator

■ Classical control system design techniques create a compensator

D.s/ to control a plant G.s/.

■ State-space methods use state feedback and estimation.



■ Can combine the state-feedback law and estimation law to derive a

compensator. Usually called K.s/.

REGULATOR: u.t/ D "K.t/ Ox.t/ where

K.t/ D R"1BTu P.t/

" PP .t/ D AT P.t/ C P.t/A C C T´ Q´C´ " P.t/BuR"1BT

u P.t/I P.tf / D H:

ESTIMATOR: Dynamics

POx.t/ D A Ox.t/ C Buu.t/ C L.t/%

y.t/ " Cy Ox.t/&

; Ox.0/ D EŒx0!

L.t/ D „x.t/C Ty S"1

v

P„x.t/ D A„x.t/ C „x.t/AT C BwSwBTw " „x.t/C T

y S"1v Cy„x.t/I „x.0/ D „x;0:

COMPENSATOR: Combine the above to get (where xc D Ox)

Pxc.t/ D .A " BuK.t/ " L.t/Cy/xc.t/ C L.t/y.t/

u.t/ D "K.t/xc.t/:

■ In steady-state, L.t/ D Lss and K.t/ D Kss and we get

Pxc.t/ D .A " BuKss " LssCy/xc.t/ C Lssy.t/

u.t/ D "Kssxc.t/:

■ We can then generate a transfer function for the compensator

K.s/ D NC .sI " NA/"1 NB:

(what are NA, NB, and NC ?)

r.t/y.t/ y.t/u.t/

: : : or : : :K.s/ "K.s/

G.s/ G.s/



Closed-loop poles (steady-state)

■ The regulator has poles at det.sI " A C BuKss/ D 0;

■ The estimator has poles at det.sI " A C LssCy/ D 0;

■ The closed-loop poles are the combination of regulator and estimator

poles (via the separation principle).

■ The compensator poles are at det.sI " A C BuKss C LssCy/ D 0 which

are neither the estimator nor the regulator poles. In particular, the

compensator might be unstable!



6.4: Reference tracking

■ So far, we have discussed only regulator design—forcing the system

state to zero as quickly as possible and keeping it there.

■ This is the same as “tracking” the reference input 0.

■ What if we want to track a more complicated reference signal?

Tracking constant reference inputs

TRACKING VIA COORDINATE TRANSLATION: A constant plant output can

be generated if the plant state is constant.

■ Generate the desired constant plant output with a specific state xd

and regulate around that state.

u.t/ D "KŒx.t/ " xd .t/!:

■ The desired state is chosen so that Cyxd D r .

■ Often, many different xd will work in this equation.

■ There is usually steady-state error when using this reference-tracking

method. Form Nx.t/ D x.t/ " xd.t/.

■ Then,

PNx.t/ D Px.t/ D AŒ Nx.t/ C xd ! C BuŒ"K Nx.t/! D .A " BuK/ Nx.t/ C Axd:

■ Notice that this differential equation is not homogeneous—it has input

Axd . So, in general, Nx.t/ 6! 0.

■ We have an additional constraint Axd D 0. This can be met only if xd

is in the null-space of A.



■ In order for A to have a null-space, it must be singular—have an

eigenvalue of 0. An integrator! (More on this later if A does not have

an integrator).

■ In general, A must have as many 0 eigenvalues as there are

reference inputs.

■ If Cyxd D r and Axd D 0 then the plant model becomes

PNx.t/ D A Nx.t/ C Buu.t/

and standard LQR theory can be applied to generate u.t/.

FEEDFORWARD CONTROL: An alternate approach, which works even

when A does not contain integrators, is

u.t/ D "Kx.t/ C Krr

where Krr is feedforward control (like the NN approach in ECE5520).

The closed-loop system is

Px.t/ D .A " BuK/x.t/ C BuKrr

y.t/ D Cyx.t/:

■ Assuming stability, using the final value theorem,

y.1/ D "Cy.A " BuK/"1BuKrr:

The inverse exists because the closed-loop Acl matrix is stable, so

has no eigenvalues at 0.

■ Set r D y.1/ and solve for Kr . For a square system,

Kr D "ŒCy.A " BuK/"1Bu!"1:

■ Kr is the inverse of the dc-gain of the system.



■ Not a robust method. Feed-forward control is more sensitive to plant

modeling errors than feedback control.

EXAMPLE: Consider the state equation

Px.t/ D

"

0 1

"10 "1

#

x.t/ C

"

0

1

#

u.t/

y.t/ Dh

1 0i

x.t/

and the LQR cost function

J D

Z 1

0

y2.t/ C 0:001u2.t/ dt:

■ Using feedforward control,

u.t/ D "h

23 5:9i

x.t/ C 33r:

0 0.5 1 1.5 20

0.2

0.4

0.6

0.8

1

1.2

1.4

Time (sec)

y(t)

0 0.5 1 1.5 20

5

10

15

20

25

30

35

Time (sec)

Con

trol t

orqu

e

■ Zero steady-state error has been achieved since our model is

“perfect”.

INTEGRAL CONTROL: We saw in the first method that constant reference

tracking could be achieved if the plant contained integrator(s).

■ If the plant does not contain integrators, we can add them:

u.t/ xI .t/

r.t/

y.t/ e.t/Plant

1

s



■ The integrator error equation is

PxI .t/ D e.t/ D r.t/ " y.t/

D r.t/ " Cyx.t/:

Note that there are as many integrators as there are reference inputs.

■ We can include the integral state into our normal state-space form by

augmenting the system dynamics

"

Px.t/

PxI .t/

#

D

"

A 0

"Cy 0

# "

x.t/

xI .t/

#

C

"

Bu

0

#

u.t/ C

"

0

I

#

r.t/:

■ Note that the new “A” matrix has an open-loop eigenvalue at the

origin. This corresponds to increasing the system type, and integrates

out steady-state error.

■ LQR may be used to generate a state feedback for the augmented

plant. Use a cost function that penalizes the integral of the error

J D

Z tf

0

xTI .t/xI .t/ C uT .t/Ru.t/ dt

D

Z tf

0

h

xT .t/ xTI .t/

i"

0 0

0 I

# "

x.t/

xI .t/

#

C uT .t/Ru.t/ dt:

■ The control weighting matrix R is generated by trial-and-error since

the physical meaning of the cost function is nebulous.

■ The control law is,

u.t/ D "K.t/

"

x.t/

xI .t/

#

D "h

Kx KI

i"

x.t/

xI .t/

#

:



■ Generally, integral control is more robust and precise than the other

two methods.

EXAMPLE: For the same system considered in the example for

feedforward control, use integral control instead.

■ The augmented system becomes

"

Px.t/

PxI .t/

#

D

2

64

0 1 0

"10 "1 0

"1 0 0

3

75

"

x.t/

xI .t/

#

C

2

64

0

1

0

3

75 u.t/ C

2

64

0

0

1

3

75 r.t/:

■ Ignoring the reference input and applying LQR gives

u.t/ D "h

31 7i

x.t/ C 100

Z t

0

fr " y.t/g dt:

The control weighting R D 0:0001 was chosen by trial and error to

yield desirable pole locations.

0 0.5 1 1.5 2 2.5 30

0.2

0.4

0.6

0.8

1

1.2

1.4

Time (sec)

y(t)

0 0.5 1 1.5 2 2.5 30

2

4

6

8

10

12

Time (sec)

Con

trol t

orqu

e

Tracking time-varying reference inputs

■ The integration method of eliminating steady-state error may be

generalized in order to track more complicated reference-inputs.

■ Integration works with dc signals since it puts an open-loop pole at dc.



■ Generally, increasing the open-loop gain in a certain band of

frequencies will improve closed-loop tracking of those frequencies.

■ The system dynamics are augmented by a filter:

u.t/ xf .t/

r.t/

y.t/ e.t/Plant Filter

■ Zero steady-state tracking error is obtained when the poles in the

Laplace transform of the reference input are also poles of the loop

transfer function.

■ This is called the internal (signal) model principle.



6.5: Designing for disturbance rejection

■ LQG is designed to reject white-noise disturbances, but may be

suboptimal when applied to nonwhite disturbances.

■ Can modify LQG to reject nonwhite disturbance.

Feedforward disturbance cancellation with full information

■ Assume that the state and disturbance are measured exactly.

Px.t/ D Ax.t/ C Buu.t/ C Bwowo

y.t/ D Cyx.t/

where wo is a constant disturbance input.

■ Use LQR to generate a state-feedback K matrix, ignoring disturbance

input.

■ Use

u.t/ D "Kx.t/ C Kww0:

■ The closed-loop state equation is

Px.t/ D .A " BuK/x.t/ C BuKww0 C Bw0w0:

■ The steady-state output due to disturbance input is

yw.1/ D "Cy.A " BuK/"1Bw0w0

and the steady-state output due to the feedforward control is

yu.1/ D "Cy.A " BuK/"1BuKww0:

■ These two terms should cancel to eliminate steady-state error due to

disturbance



"Cy.A " BuK/"1Bw0w0 " Cy.A " BuK/"1BuKww0 D 0

which gives

Kw D "fCy.A " BuK/"1Bug"1Cy.A " BuK/"1Bw0:

■ As with reference tracking, feedforward disturbance canceling is not

very robust.

EXAMPLE: The angular velocity of a field-controlled dc motor is required

to equal a set point regardless of the load torque. The state equation

of the motor is"

P!.t/

P#M .t/

#

D

"

"1 1

0 "0:1

# "

!.t/

#M .t/

#

C

"

0

0:1

#

u.t/ C

"

1

0

#

#L.t/

y.t/ Dh

1 0i

x.t/;

where ! is the angular velocity; #M is the motor torque and #L is the

load torque.

■ The LQR cost function is

J D

Z 1

0

!2.t/ C 0:01u2.t/ dt:

■ We get

u.t/ D "h

14:6 16:1i

x.t/ " 17:1#L.t/:

■ Simulation of step load torque with set-point of zero:

0 5 10 15 20−0.05

0

0.05

0.1

0.15

0.2

0.25

0.3

Time (s)

Angu

lar v

eloc

ity (r

ad/s

)

0 5 10 15 20−20

−15

−10

−5

0

Time (s)

Fiel

d vo

ltage

(V)



■ Simulation output “perfect” due to exact model used.

Feedforward disturbance cancellation with partial information

■ Usually we have only noisy partial state measurements.

■ Estimate both the plant state and disturbance using a Kalman filter.

■ To estimate disturbance, the plant model must be augmented with a

filter that generates a nearly-constant disturbance.

wo.t/

u.t/w.t/ y.t/

v.t/

Plant

1

s

■ The integral has an artificial noise input that allows the disturbance

input to change slowly.

■ The plant state model for designing the Kalman filter is then

"

Px.t/

Pw0.t/

#

D

"

A Bw0

0 0

# "

x.t/

w0.t/

#

C

"

Bu

0

#

u.t/ C

"

0

I

#

w.t/

y.t/ Dh

Cy 0i

"

x.t/

w0.t/

#

C v.t/

■ The disturbance-canceling controller that uses the noisy partial

measurements is then

u.t/ D "K Ox.t/ " Kw Ow0.t/;

where K is calculated via LQR and Kw is calculated as before.

Integral control for disturbance canceling

■ Integral control may also be used to eliminate steady-state errors due

to step-like disturbances.



■ The Kalman filter must

estimate both the plant

state and the disturbance

input; if it estimates only the

plant state it will be biased

by the disturbance (the

assumptions made when

deriving the Kalman filter

are violated by non

white-noise disturbance).

Ox.t/

w.t/

O#L.t/

u.t/y.t/

v.t/#L.t/

Oe

r.t/

"Kx

"Kw Cy

Kalmanfilter

Plant

1=s

EXAMPLE: The disturbance canceling example from before."

P!.t/

P#M .t/

#

D

"

"1 1

0 "0:1

# "

!.t/

#M .t/

#

C

"

0

0:1

#

u.t/

C

"

1

0

#

#L.t/ C

"

0

1

#

w.t/

y.t/ Dh

1 0i

x.t/ C v.t/;

■ The augmented state for the purpose of designing a Kalman filter is2

64

P!.t/

P#M .t/

P#L.t/

3

75 D

2

64

"1 1 1

0 "0:1 0

0 0 0

3

75

2

64

!.t/

#M .t/

#L.t/

3

75 C

2

64

0

0:1

0

3

75 u.t/

C

2

64

0 0

1 0

0 1

3

75

"

w.t/

wT .t/

#

y.t/ Dh

1 0 0i

x.t/ C v.t/;



where wT .t/ is the white noise that drives changes in the load torque.

The noise spectral densities are

S2

64

w

wT

3

75

D

"

0:001 0

0 0:01

#

I Sv D 0:001;

where the spectral density for wT was selected by trial and error.

■ The resulting steady-state Kalman gain matrix is

L Dh

1:8 0:1 3:2iT

:

■ Incorporating this information, we have so far the dynamics of the

estimator:

POx.t/ D A Ox.t/ C Bu.t/ C L.y.t/ " Oy.t//

D .A " LC / Ox.t/ C Bu.t/ C Ly.t/2

64

PO!.t/PO#M .t/PO#L.t/

3

75 D

2

64

"2:8 1 1

"0:1 "0:1 0

"3:2 0 0

3

75

2

64

O!.t/

O#M .t/

O#L.t/

3

75 C

2

64

0

0:1

0

3

75 u.t/ C

2

64

1:8

0:1

3:2

3

75 y.t/:

■ For LQR integral state feedback, the augmented state equation is2

64

P!.t/

P#M .t/

PxI .t/

3

75 D

2

64

"1 1 0

0 "0:1 0

"1 0 0

3

75

2

64

!.t/

#M .t/

xI .t/

3

75 C

2

64

0

0:1

0

3

75 u.t/ C

2

64

0

0

1

3

75 r.t/;

and the cost function is

J D

Z 1

0

x2I .t/ C 0:0001u2.t/ dt:

■ For this cost function, we find that

K Dh

60 34 "100i

:



■ Note that the integrator state implements POxI .t/ D r.t/ " O!.t/ so we

can augment the estimator dynamics:

2

66664

PO!.t/PO#M .t/PO#L.t/POxI .t/

3

77775

D

2

66664

"2:8 1 1 0

"0:1 "0:1 0 0

"3:2 0 0 0

"1 0 0 0

3

77775

2

66664

O!.t/

O#M .t/

O#L.t/

OxI .t/

3

77775

C

2

66664

1:8

0:1

3:2

0

3

77775

y.t/ C

2

66664

0

0

0

1

3

77775

r.t/

C

2

66664

0

0:1

0

0

3

77775

h

"60 "34 0 100i

2

66664

O!.t/

O#M .t/

O#L.t/

OxI .t/

3

77775

2

66664

PO!.t/PO#M .t/PO#L.t/POxI .t/

3

77775

D

2

66664

"2:8 1 1 0

"6:1 "3:5 0 10

"3:2 0 0 0

"1 0 0 0

3

77775

2

66664

O!.t/

O#M .t/

O#L.t/

OxI .t/

3

77775

C

2

66664

1:8

0:1

3:2

0

3

77775

y.t/ C

2

66664

0

0

0

1

3

77775

r.t/

u.t/ Dh

"60 "34 0 100i

2

66664

O!.t/

O#M .t/

O#L.t/

OxI .t/

3

77775

:

■ Response to step load torque.



0 5 10 15 20−0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

Time (s)

Angu

lar v

eloc

ity (r

ad/s

)

0 5 10 15 20−12

−10

−8

−6

−4

−2

0

2

Time (s)

Fiel

d vo

ltage

(V)

Frequency-shaped LQG

■ One final “trick” we look at is used to shape the frequency response

of the LQG system.

■ For example, a system may have a resonance frequency we do not

wish to excite. Standard control design would place a notch filter at

this frequency.

■ To perform frequency-shaped LQG design, we place a bandpass filter

in parallel with the plant dynamics.

u.t/ y.t/

xf .t/

Plant

Bandpassfilter

Augmentedplant

■ This adds states that we wish to penalize.

■ When designing a Kalman filter, we need only to estimate the true

plant states as we have access to the filter states.



Appendix: Steady-state LQG cost

■ We earlier stated (where ´.t/ D C´x.t/)

J D limt!1

EŒ´T .t/Q´´.t/ C uT .t/Ru.t/!

D trace

#

†x;ss LssSvLTss C „x;ss C T

´ QĆ´

$

D tracen

†x;ss BwSwBTw C „x;ss KT

ssRKss

o

:

■ Here, we show that the second and third lines are the same (not yet

sure how to show that the first and second are the same).

■ Start with the second line:

trace˚


T´ QĆ´

'

D trace

8

ˆ<

ˆ:

†x

0

B@ „xC T

y S"1v Cy„x

„ ƒ‚ …

A„xC„xAT CBwSwBTw

1

CA C „xC T

´ QĆ´

9

>=

>;

■ But,

C T´ QĆ´ D †xBuR"1BT

u †x„ ƒ‚ …

KT RK

"AT †x " †xA

so

trace˚


T´ QĆ´

'

D trace˚

†xBwSwBTw C †x.A„x C „xAT / C „x

(

KT RK " AT †x " †xA)'

D trace˚

†xBwSwBTw C „xKT RK

'

C trace˚

†x.A„x C „xAT /'

" trace˚

„x.AT †x C †xA/'

„ ƒ‚ …

equal

D trace˚

†xBwSwBTw C „xKT RK

'



where we used the knowledge that

AT †x;ss C †x;ssA C C T´ Q´C´ " †x;ssBuR"1BT

u †x;ss D 0

A„x;ss C „x;ssAT C BwSwBT

w " „x;ssCTy S"1

v Cy„x;ss D 0

and Lss D „x;ssCTy S"1

v ; Kss D R"1BTu †x;ss.


linear quadratic gaussian - university of colorado

Documents