5

Optimal Linear Quadratic Gaussian (LQG) Control

Problem:
Given the LTI system
x = Ax + Bu u + Bw w,
y = Cy x + Dyw w,
z = Cz x + Dzuu,
and the observer-based controller
x = A
x + Bu u + F (
y y),
y = Cy x,
u = K x.
compute (K, F ) that stabilize the closed loop system and minimize the cost
function


J := lim E z(t)T z(t) .
(3)
t

Assumptions:

1. (A, Bu) stabilizable,

2. (A, Cy ) detectable,
3. w(t) is a Gaussian zero mean white noise with variance W 0,
T
Dzu 0,
4. CzT Dzu = 0 and Dzu
T
T
5. Bw W Dyw
= 0 and Dyw W Dyw
0.

Remark 1: 4 and 5 can be relaxed.

Remark 2: 4 implies that


J = lim E x(t)T Qx(t) + u(t)T Ru(t) ,
t

where

Q = CzT Cz ,

MAE 280 B

T
R = Dzu
Dzu 0.

65

Maurcio de Oliveira

5.1

The Closed Loop System

The closed loop system is

  
  

x
A
Bu K
x
Bw
=
+
w,
F Cy A + BuK + F Cy x
F Dyw
x
 

 x
z = Cz DzuK
x

(4)

As in the estimation problem, we write the dynamics in terms of e := x x.

Note that
  
 
  
 
e
I I
x
x
I I
e
=
,
=
,
x
0 I
x
x
0 I
x
and

5.2

  
  

e
A + F Cy
0
e
Bw + F Dyw
=
+
w,
F Dyw
F Cy A + Bu K
x
x
 

 e
z = Cz Cz + DzuK
x

(5)

Stability of the Closed Loop System

First fantastic result:

The closed loop system (4) is stable
if and only if
K is a stabilizing state feedback gain and
F is a stabilizing state estimation gain.
Proof: Systems (4) and (5) are similar, therefore the closed loop poles are the
roots of the polyomial equation


sI A
BuK
0 = det
F Cy sI (A + Bu K + F Cy )


sI (A + F Cy )
0
= det
F Cy
sI (A + BuK)
= det [sI (A + F Cy )] det [sI (A + Bu K)]

MAE 280 B

66

Maurcio de Oliveira

5.3

The Cost Function

Now write the cost function (1) in terms of system (5)



J = lim E z(t)T z(t) .
t

Computing J using the Controllability Gramian of system (5) we have






 Y1 Y2
CzT
,
J = trace Cz Cz + DzuK
T
Y2T Y3 CzT + K T Dzu

(6)

where Y1 , Y2 and Y3 satisfy the Lyapunov equation


 


0 0
A + F Cy
0
Y1 Y2
=
0 0
F Cy A + Bu K Y2T Y3


T
Y1 Y2 A + F C y
0
+
Y2T Y3
F Cy A + Bu K




Bw + F Dyw
+
W (Bw + F Dyw )T (F Dyw )T
F Dyw
Expanding these equations we have
0 = (A + F Cy )Y1 + Y1(A + F Cy )T + (Bw + F Dyw )W (Bw + F Dyw )T ,

(7)

T
0 = (A + F Cy )Y2 + Y2(A + BuK)T Y1 CyT F T F Dyw W Dyw
FT,

(8)

Y
0 = (A + BuK)Y3 + Y3(A + BuK)T + F Dyw W Dyw
FT

F Cy Y2 Y2T CyT F T .

(9)

T
In (8) we have used the assumption that Bw W Dyw
= 0.

MAE 280 B

67

Maurcio de Oliveira

5.4

The Optimal Estimator Gain

The closed loop is stable if and only if F is stabilizing. Furthermore

J = J1 (Y1) + J2 (Y2, Y3, K)
where
J1 (Y1) = trace(Cz Y1 CzT ),
J2(Y2, Y3, K) = trace([Cz + DzuK]Y3[Cz + DzuK]T )
+ 2 trace(Cz Y2[Cz + DzuK]T ).
Note that Y1 is determined by solving (7)
(A + F Cy )Y1 + Y1 (A + F Cy )T + (Bw + F Dyw )W (Bw + F Dyw )T = 0,
which does not depend on K. Therefore the cost J is minimum if J1(Y1) is also
minimum. But this is the case if Y1 is minimum in the sense of the Comparison
Lemma1. That is, with Y1 = Y satisfying the ARE
T 1
) Cy Y + Bw W BwT = 0.
AY + Y AT Y CyT (Dyw W Dyw

(10)

The optimal observer gain is then

T 1
F = Y CyT (Dyw W Dyw
) .

5.5

A Temporary Solution For Y2

Knowing that, we rewrite (8) as

T 1
0 = AY2 + Y2 (A + Bu K)T Y CyT (Dyw W Dyw
) C y Y2 .

Note that the above equation is trivially satisfied with Y2 = 0. Assume this
choice of Y2 as a matter of faith for the moment.

See section What if Y2 6= 0 for a formal proof of a similar fact.

MAE 280 B

68

Maurcio de Oliveira

5.6

The Optimal Control Gain

With Y1 = Y and Y2 = Y2 = 0, the cost function (20) becomes

h
i


T
T
J = trace Cz Y Cz + trace (Cz + DzuK) Y3 (Cz + Dzu K) .
In this form, it is clear that the optimal K is the one that minimizes
i
h
T
J2 = trace (Cz + DzuK) Y3 (Cz + DzuK) .

subject to equation (9), which for Y2 = 0 is simply

T 1
(A + Bu K)Y3 + Y3 (A + Bu K)T + Y CyT (Dyw W Dyw
) Cy Y = 0.

(11)

Using our previous duality results, the optimal K is the one that minimizes
 T

T 1/2
XB
,
:= Y CyT (Dyw W Dyw
J2 = trace B
B
)

where

0 = (A + Bu K)T X + X(A + Bu K) + (Cz + DzuK)T (Cz + DzuK)

T
DzuK.
= (A + Bu K)T X + X(A + Bu K) + CzT Cz + K T Dzu

Using what we know about the LQR problem, the solution is

T
Dzu)1BuT X ,
K = (Dzu

where X satisfies the ARE

T
AT X + X A X Bu(Dzu
Dzu)1BuT X + CzT Cz = 0.

Matrix Y3 can be computed from (9) upon substitution of the optimal K .

MAE 280 B

69

Maurcio de Oliveira

5.7

Summary of LQG Control

Given the LTI system

x = Ax + Bu u + Bw w,
y = Cy x + Dyw w,
z = Cz x + Dzuu,
and the observer-based controller
x = A
x + Bu u + F (
y y),
y = Cy x,
u = K x,
if
1. (A, Bu) stabilizable,
2. (A, Cy ) detectable,
3. w(t) is a Gaussian zero mean white noise with variance W 0,
T
4. CzT Dzu = 0 and Dzu
Dzu 0,
T
T
0,
= 0 and Dyw W Dyw
5. Bw W Dyw

then the choice of (K, F ) that stabilize the closed loop system and minimize
the cost function


J := lim E z(t)T z(t) .
t

is given by

T
K = (Dzu
Dzu)1BuT X ,

where X satisfies the ARE

T
AT X + X A X Bu(Dzu
Dzu)1BuT X + CzT Cz = 0,

and
T 1
F = Y CyT (Dyw W Dyw
) ,

where Y satisfies the ARE

T 1
AY + Y AT Y CyT (Dyw W Dyw
) Cy Y + Bw W BwT = 0.

MAE 280 B

70

Maurcio de Oliveira

5.8

What if Y2 6= 0?

The optimal covariance matrix has the form


 

Y1 Y2
Y
0
0
=
0 Y3
Y2T Y3
We claim that this matrix is minimal in the sense of the comparison lemma.
Proof: Assume it is not. Then there exists

 

Y
0
Y1 Y2

0
Y2T Y3
0 Y3
This implies that


Y Y1
Y2
0
(12)
Y2T Y3 Y3
 


I
we obtain that
Multiplying (12) on the left by I 0 and on the right by
0
Y  Y1 .

But since we have already proved that Y is minimal

Y1  Y .
Therefore, Y1 = Y . Using this fact (12) becomes


0
Y2
 0.
Y2T Y3 Y3
 
0
Multiplying (12) on the left by 0 I and on the right by
we conclude that
I


Y3  Y3 .

Now, matrix (12) is positive semidefinite if and only if

 T 
 
x
0
Y2
x
= 2xT Y2 y + y T (Y3 Y3)y 0,
T

Y2 Y3 Y3 y
y

x, y

Because, Y3 Y3  0, this is certainly true for Y2 = 0. On the other hand, if

Y2 6= 0, all vectors in the form x = Y2 y are such that
y T (Y3 Y3 )y 2y T Y2T Y2 y 0,

y.

As Y2T Y2  0, for any y such that Y2y 6= 0 there is a sufficiently big > 0 such
that the above inequality is violated, proving that Y2 must indeed be zero.
MAE 280 B

71

Maurcio de Oliveira

5.9

Example: controlling a satellite in circular orbit

u2
r
1111
0000
0000
1111
0000
1111
0000
1111
0000
1111
0000
1111
0000
1111

u1
m

Satellite of mass m with thrust in the radial direction u1 and in the tangential
direction u2. Continuing...
km
m(
r r2 ) = u1 2 + w1,
r

m(2r + r) = u2 + w2 ,
where w1 and w2 are independent white noise disturbances with variances 1
and 2 .
As before, putting in state space and linearizing

0 0
1
0
0
0
0
0
 
 
0 0
0
u1
0
w1
0
1
0
0
x+

x =
+
3
1/m
2 0
0
2 r
1/m
0 u2
0 w2
0 0 2
/
r 0
0 1/(m
r)
0 1/(m
r)
Consider that you have a noisy measurement of (x2)


y = 0 1 0 0 +v

where Evv T = 3.
Problem: Given

m = 100 kg,

k = GM

r = R + 300 km,

where G 6.673 1011 N m2/kg2 is the universal gravitational constant, and

M 5.98 1024 kg and R 6.37 103 km are the mass and radius of the
earth. If the variances 1 = 2 = 3 = 0.1N find solutions to the LQR control
problem where
Q = I,

R = I,

using u2 only first, then using u1 and u2.

MAE 280 B

72

Maurcio de Oliveira

%
%
%
%
%
m
r
R
G
M
k
w
v

MAE 280 B - Linear Control Design

Mauricio de Oliveira
LQG Control - Part II
=
=
=
=
=
=
=
=

100;
300E3;
6.37E6;
6.673E-11;
5.98E24;
G * M;
sqrt(k/((R+r)3));
w * (R + r);

% linearized system
A = [0 0 1 0; 0 0 0
Bu = [0 0; 0 0; 1/m
Bw = [0 0; 0 0; 1/m

%
%
%
%
%
%
%
%

100 kg
300 km
6.37 103 km
6.673 N m2/kg2
5.98 1024 kg
gravitational force constant
angular velocity (rad/s)
"ground" velocity (m/s)

matrices
1; 3*w2 0 0 2*(r+R)*w; 0 0 -2*w/(r+R) 0];
0; 0 1/(m*r)];
0; 0 1/(m*r)];

% noise variances
W = 0.1 * eye(2)
W =
1.0000e-01
0
0
1.0000e-01
% scale
T = diag([1 r 1 r])
T =
1
0
0
300000
0
0
0
0

0
0
1
0

0
0
0
300000

% similarity transformation
At = T * A / T
At =
0
0
1.0000e+00
0
0
0
4.0343e-06
0
0
0
0 -1.0432e-04
But = T * Bu
But =
0
0
0
0
1.0000e-02
0
0
1.0000e-02
Bwt = T * Bw
Bwt =
0
0
0
0
1.0000e-02
0
0
1.0000e-02

MAE 280 B

73

0
1.0000e+00
5.1565e-02
0

Maurcio de Oliveira

% measuring x2 (theta)
Cy = [0 1 0 0];
Cyt = Cy / T
Cyt =
0
3.3333e-06
% augment noise matrices
Bwt = [Bwt zeros(4,1)]
Bwt =
0
0
0
0
1.0000e-02
0
0
1.0000e-02
Dywt = [zeros(1,2) 1]
Dywt =
0
0
1
Ww = W
Ww =
1.0000e-01
0
Wv = 0.1
Wv =
1.0000e-01

0
0
0
0

0
1.0000e-01

Wt = [Ww zeros(2, 1); zeros(1, 2) Wv]

Wt =
1.0000e-01
0
0
0
1.0000e-01
0
0
0
1.0000e-01
% optimal state feedback control (using u2 only)
rho = 1
rho =
1
Czt = [eye(4); zeros(1,4)];
Dzut = sqrt(rho)*[zeros(4,1); eye(1)];
Czt*Dzut
ans =
0
0
0
0
[K,X,S] = lqr(At, But(:,2), Czt*Czt, Dzut*Dzut);
K = - K
K =
-1.0055e+00
1.0000e+00 -5.2522e+01 -1.8511e+01
Acl = At + But(:,2)*K;
eig(Acl)
ans =
-6.7473e-02 + 7.5883e-02i
-6.7473e-02 - 7.5883e-02i
-5.0086e-02

MAE 280 B

74

Maurcio de Oliveira

-7.8118e-05
% optimal state estimation
[F,X,S] = lqr(At, Cyt, Bwt * Wt * Bwt, Dywt * Wt * Dywt);
F = - F
F =
5.7737e+02
-1.7360e+02
-3.8374e-01
-5.0229e-02
Acl = At + F*Cy;
eig(Acl)
ans =
-2.1170e-04
-3.8819e-05 + 1.0470e-03i
-3.8819e-05 - 1.0470e-03i
-1.7360e+02
% optimal controller
ctr1 = ss(At + But(:,2) * K + F * Cyt, -F, K, 0)
a =
x1
x2
x3
x4

x1
0
0
4.034e-06
-0.01006

x2
0.001925
-0.0005787
-1.279e-06
0.01

x3
1
0
0
-0.5253

x4
0
1
0.05157
-0.1851

b =
x1
x2
x3
x4

u1
-577.4
173.6
0.3837
0.05023

y1

x1
-1.006

y1

u1
0

c =
x2
1

x3
-52.52

x4
-18.51

d =

Continuous-time model.
% controller transfer-function
zpk(ctr1)
Zero/pole/gain:
733.09 (s+5.507e-05) (s2 - 0.0005306s + 9.993e-07)
---------------------------------------------------(s+0.05429) (s+0.0006114) (s2 + 0.1308s + 0.009978)

MAE 280 B

75

Maurcio de Oliveira

% compute using matlabs lqg

sys = ss(At, But(:,2), Cyt, 0);
ctr2 = lqg(sys, [Czt Dzut] * [Czt Dzut], ...
[Bwt; Dywt] * Wt * [Bwt; Dywt])
a =
x1_e
0
0
4.034e-06
-0.01006

x1_e
x2_e
x3_e
x4_e

x2_e
0.001925
-0.0005787
-1.279e-06
0.01

x3_e
1
0
0
-0.5253

x4_e
0
1
0.05157
-0.1851

b =
x1_e
x2_e
x3_e
x4_e

y1
-577.4
173.6
0.3837
0.05023

c =
u1

x1_e
-1.006

u1

y1
0

x2_e
1

x3_e
-52.52

x4_e
-18.51

d =

Input groups:
Name
Measurement
Output groups:
Name
Controls

Channels
1

Channels
1

Continuous-time model.
% compute closed loop system
Acl = [At But(:,2)*K; -F*Cyt, At+But(:,2)*K+F*Cyt];
Bcl = [Bwt; -F*Dywt];
Ccl = [Czt; Dzut*K];
eig(Acl)
ans =
-6.7473e-02
-6.7473e-02
-5.0086e-02
-7.8118e-05
-2.3280e-04
-2.3280e-04
-5.6530e-05
-5.6530e-05

MAE 280 B

+ 7.5883e-02i
- 7.5883e-02i

+
+
-

2.1126e-04i
2.1126e-04i
1.1651e-03i
1.1651e-03i

76

Maurcio de Oliveira

% state variance
Y = lyap(Acl, Bcl * Wt * Bcl)
Y =
Columns 1 through 6
2.4528e+08
2.4049e+01 -2.0592e-06
2.4022e+01
2.4091e+07
4.1881e+04
2.4808e-06
4.1881e+04
1.1701e+03
-4.1881e+04 -1.0982e-06 -9.6965e-05
1.8471e+07
1.7321e+07 -1.6668e+04
1.7321e+07
1.8883e+07
3.0368e+04
-1.6668e+04
3.0368e+04
9.9675e+02
-2.0345e+04 -1.5069e+03
1.1612e+00
Columns 7 through 8
-1.6668e+04 -2.0345e+04
3.0368e+04 -1.5069e+03
9.9675e+02
1.1612e+00
1.1612e+00
1.0205e+03
-1.6668e+04 -2.0345e+04
3.0368e+04 -1.5069e+03
9.9675e+02
1.1612e+00
1.1612e+00
1.0205e+03
sqrt(trace(Y))
ans =
1.7514e+04
sqrt(diag(Y))
ans =
1.5661e+04
4.9082e+03
3.4206e+01
3.1977e+01
4.2978e+03
4.3454e+03
3.1571e+01
3.1945e+01

-4.1881e+04
1.1101e-06
-9.6965e-05
1.0226e+03
-2.0345e+04
-1.5069e+03
1.1612e+00
1.0205e+03

1.8471e+07
1.7321e+07
-1.6668e+04
-2.0345e+04
1.8471e+07
1.7321e+07
-1.6668e+04
-2.0345e+04

1.7321e+07
1.8883e+07
3.0368e+04
-1.5069e+03
1.7321e+07
1.8883e+07
3.0368e+04
-1.5069e+03

% optimal state feedback control (using u1 and u2)

rho = 1
rho =
1
Czt = [eye(4); zeros(2,4)];
Dzut = sqrt(rho)*[zeros(4,2); eye(2)];
Czt*Dzut
ans =
0
0
0
0
0
0
0
0
[K,X,S] = lqr(At, But, Czt*Czt, Dzut*Dzut);
K = - K
K =
-9.8444e-01
1.7795e-01 -1.3843e+01 -2.4919e+00
-1.7795e-01 -9.8404e-01 -2.4919e+00 -1.4741e+01
Acl = At + But*K;

MAE 280 B

77

Maurcio de Oliveira

eig(Acl)
ans =
-7.1524e-02
-7.1524e-02
-7.1395e-02
-7.1395e-02

+
+
-

8.2854e-02i
8.2854e-02i
5.7005e-02i
5.7005e-02i

% optimal state estimation

[F,X,S] = lqr(At, Cyt, Bwt * Wt * Bwt, Dywt * Wt * Dywt);
F = - F
F =
5.7737e+02
-1.7360e+02
-3.8374e-01
-5.0229e-02
Acl = At + F*Cy;
eig(Acl)
ans =
-2.1170e-04
-3.8819e-05 + 1.0470e-03i
-3.8819e-05 - 1.0470e-03i
-1.7360e+02
% optimal controller
ctr1 = ss(At + But * K + F * Cyt, -F, K, 0)
a =
x1
0
0
-0.00984
-0.00178

x1
x2
x3
x4

x2
0.001925
-0.0005787
0.001778
-0.009841

x3
1
0
-0.1384
-0.02502

x4
0
1
0.02665
-0.1474

b =
x1
x2
x3
x4

u1
-577.4
173.6
0.3837
0.05023

y1
y2

x1
-0.9844
-0.178

y1
y2

u1
0
0

c =
x2
0.178
-0.984

x3
-13.84
-2.492

x4
-2.492
-14.74

d =

Continuous-time model.
% controller transfer-function

MAE 280 B

78

Maurcio de Oliveira

zpk(ctr1)
Zero/pole/gain from input to output...
593.8432 (s+0.01501) (s2 + 0.1347s + 0.009646)
#1: --------------------------------------------------(s2 + 0.144s + 0.008393) (s2 + 0.1424s + 0.01202)

#2:

-69.7822 (s+0.2538) (s+0.0984) (s-5.737e-05)

--------------------------------------------------(s2 + 0.144s + 0.008393) (s2 + 0.1424s + 0.01202)

% compute using matlabs lqg

sys = ss(At, But, Cyt, 0);
ctr2 = lqg(sys, [Czt Dzut] * [Czt Dzut], ...
[Bwt; Dywt] * Wt * [Bwt; Dywt])
a =
x1_e
0
0
-0.00984
-0.00178

x1_e
x2_e
x3_e
x4_e

x2_e
0.001925
-0.0005787
0.001778
-0.009841

x3_e
1
0
-0.1384
-0.02502

x4_e
0
1
0.02665
-0.1474

b =
x1_e
x2_e
x3_e
x4_e

y1
-577.4
173.6
0.3837
0.05023

c =
u1
u2

x1_e
-0.9844
-0.178

u1
u2

y1
0
0

x2_e
0.178
-0.984

x3_e
-13.84
-2.492

x4_e
-2.492
-14.74

d =

Input groups:
Name
Measurement
Output groups:
Name
Controls

Channels
1

Channels
1,2

Continuous-time model.
% compute closed loop system
Acl = [At But*K; -F*Cyt, At+But*K+F*Cyt];
Bcl = [Bwt; -F*Dywt];

MAE 280 B

79

Maurcio de Oliveira

Ccl = [Czt; Dzut*K];

eig(Acl)
ans =
-5.6530e-05
-5.6530e-05
-2.3280e-04
-2.3280e-04
-7.1524e-02
-7.1524e-02
-7.1395e-02
-7.1395e-02

+
+
+
+
-

1.1651e-03i
1.1651e-03i
2.1126e-04i
2.1126e-04i
8.2854e-02i
8.2854e-02i
5.7005e-02i
5.7005e-02i

% state variance
Y = lyap(Acl, Bcl * Wt * Bcl)
Y =
Columns 1 through 6
2.2715e+08 -1.7437e+07
1.3780e-06
-1.7437e+07
5.2476e+06
1.7501e+04
-2.0926e-06
1.7501e+04
1.3688e+03
-1.7501e+04 -6.8376e-09 -3.4619e+02
3.4373e+05 -1.1556e+05 -1.6668e+04
-1.1556e+05
3.9555e+04
5.9891e+03
-1.6668e+04
5.9891e+03
1.1955e+03
4.0341e+03 -1.5069e+03 -3.4503e+02
Columns 7 through 8
-1.6668e+04
4.0341e+03
5.9891e+03 -1.5069e+03
1.1955e+03 -3.4503e+02
-3.4503e+02
1.1046e+02
-1.6668e+04
4.0341e+03
5.9891e+03 -1.5069e+03
1.1955e+03 -3.4503e+02
-3.4503e+02
1.1046e+02
sqrt(trace(Y))
ans =
1.5257e+04
sqrt(diag(Y))
ans =
1.5071e+04
2.2908e+03
3.6997e+01
1.0608e+01
5.8629e+02
1.9889e+02
3.4576e+01
1.0510e+01

-1.7501e+04
7.0774e-09
-3.4619e+02
1.1253e+02
4.0341e+03
-1.5069e+03
-3.4503e+02
1.1046e+02

3.4373e+05
-1.1556e+05
-1.6668e+04
4.0341e+03
3.4373e+05
-1.1556e+05
-1.6668e+04
4.0341e+03

-1.1556e+05
3.9555e+04
5.9891e+03
-1.5069e+03
-1.1556e+05
3.9555e+04
5.9891e+03
-1.5069e+03

diary off

MAE 280 B

80

Maurcio de Oliveira