University of Pennsylvania

ESE 500: Linear Systems Theory

Homework V
MODULE V: Optimal Control & Estimation

Due: on Thursday 12/10 at 23:59 on Gradescope

Instructions
Read the following instructions carefully before beginning to work on the homework.

• You must submit your solutions on Gradescope. It must be submitted as a single PDF
file, compiled in LATEX or written by hand and scanned into images included in your
pdf. It must be readable by our staff to be graded.

• Please start a new problem on a fresh page and mark on Gradescope all the pages corre-
sponding to each problem. Failure to do so may result in your work not graded completely.

• Clearly indicate the name and Penn email of all your collaborators on your submitted
solutions. You may discuss the problems but you cannot share solutions and whatever
you submit should be your own work. Failure to do so will result in penalties according
to Penn’s Code of Conduct policies.

• Regrades are handled via Gradescope within a week from the releasing of HW grades.
After that no regrade request is admissible. When requesting a regrade, the entire sub-
mission is regraded.

• Please submit only one pdf. If you wrote any code, you have to submit that as well.

Problem 1 (15 points). Consider the following system.

   
2 −4 3 1
ẋ = 1 −3 −1 x + 0  u
  
1 −2 −2 0

a) (3 points) Decompose the system into controllable and uncontrollable subsystems.

b) (2 points) Justify whether or not the system is stabilizable.

c) (4 points) Design a feedback gain K̂ for the transformed system, you obtained in a, to place
the poles to −1, −2, −3.

d) (3 points) Compute feedback gain K corresponding to K̂ to be applied to the original system

in order to have the same pole placement as in c.

e) (3 points) Justify whether or not it is possible to place the poles to −2, −2, −3.

1
Solution 1. a) Consider the controllability matrix which is
 
1 2 3
C = B AB A2 B =  0 1 −2  ⇒ rank(C) = 2
 

0 1 −2
   
 1 2 
⇒ R(C) = span  0 , 1 
 
0 1
 

Now, we extend the basis of R(C) to a basis for R3 and put the elements together to obtain
 
1 2 0
P = 0 1 0 
0 1 1
Now, we compute the transformation x = P x̃ to obtain the controllable-uncontrollable de-
composition. Applying x = P x̃, we have

x̃˙ = P −1 AP x̃ + P −1 Bu

Computing values results in

   
0 7 5 1
x̃˙ =  1 −2 −1  x̃ +  0  u
0 0 −1 0
 
0 7
Therefore, the controllable part is Ac = and the uncontrollable part is Ac̄ = [−1]
1 −2
with eigenvalue −1.

b) The eigenvalue of uncontrollable part Ac̄ = [−1] is −1 which has negative real part. Thus,
Ac̄ is stable and as a result, the system is stabilizable.
 
c) Let K̂ = k1 k2 k3 . Note that u = K̂ x̃ should be applied to the transformed system.
We have
     
0 7 5 1  k1 7 + k2 5 + k3
x̃˙ =  1 −2 −1  x̃ +  0  k1 k2 k3 x̃ =  1

−2 −1  x̃
0 0 −1 0 0 0 −1

The poles should be placed to −1, −2, −3. Note that we already have the pole −1 (as an
uncontrollable pole) and as it can be seen, the feedback does not affect this pole. Hence, we
should place two poles of the controllable part to −2, −3. We can simply solve
  
k1 7 + k2
(s + 2)(s + 3) = det sI −
1 −2
or equivalently write
 
k1 7 + k2
(−2) + (−3) = trace = k1 − 2
1 −2
 
k1 7 + k2
(−2)(−3) = det = −2k1 − k2 − 7
1 −2
which gives k1 = −3, k2 = −7. Note that k3 does not matter and any value works for k3 .
This makes sense because
 k3 is corresponding
 to x3 which is the uncontrollable part of the
state. Hence, K̂ = −3 −7 0 .

2
d) u = K̂ x̃ = K̂P −1 x. Therefore, K = K̂P −1 .
 
1 −2 0
K = K̂P −1 =
   
−3 −1 0  0 1 0  = −3 −1 0
0 −1 1

e) As discussed above, the uncontrollable part has eigenvalue −1 which cannot be changed. As
a result, it is not possible to have the poles on −2, −3, −5.

Problem 2 (10 points). Consider the discrete-time system

xk+1 = Axk + Buk

where    
0 −1 1
A= B=
−1 0 0
and the cost function
∞
X
J= kxk k22 + u2k
k=0

Note that here Q = I and R = 1. Design an LQR feedback gain Klqr for the system using the
following steps.

a) (4 points) Find the 2 × 2 symmetric and positive definite matrix P which satisfies the Alge-
braic Riccati equation (ARE) for discrete case, which is

P = Q + AT P A − AT P B(R + B T P B)−1 B T P A

b) (2 points) Compute feedback gain Klqr = −(R + B T P B)−1 B T P A.

c) (4 points) Compute the closed-loop system using the feedback uk = Klqr xk .

 
a b
Solution 2. a) Consider a general 2 × 2 symmetric matrix P = . Then by replacing
b c
other values, the right hand side of ARE gives
" #
b2 ab
− + c + 1 b −
Q + AT P A − AT P B(R + B T P B)−1 B T P A = a+1
ab
a+1
a2
b − a+1 a − a+1 +1

Therefore, we need to solve

" 2
#
  b ab
a b − a+1 +c+1 b − a+1
= ab a2
b c b − a+1 a − a+1 +1
√ √ √
which results in a = 1 + 3, b = 0, c = 3. (Note that the solution a = 0, b = ± 2, c = 1
does not result in a positive definite P ) Hence,
 √ 
1 + 3 √0
P =
0 3

b) h √ i
Klqr = −(R + B T P B)−1 B T P A = 0 1+√3
2+ 3

3
c) " √ #
1+√3
0 −1 + 2+
A + BKlqr = 3
−1 0

Problem 3 (50 points). Make sure you include the Matlab code.

Figure 1: A UAV moves in the z-axis direction under the gravitational force and the actuator
force F . The control objective is to hover at the height z = zd .

Consider a one dimensional UAV of Figure 1, which is moving only in the z−axis direction.
Its state comprises of the height z and the velocity ż. Hence the equations of motion are:

mz̈ = F − mg

where F is the actuator input, m is the mass, and g is the gravitational acceleration. The initial
height is z(0) = 0 and the initial velocity ż(0) = 0. In this problem, we will study state space
methods for control design. In particular, the control objective for the UAV is to hover at the
desired height zd , with zero velocity.

1. (8 points) Discretization Prior to controlling the system, we need to discretize the

continuous-time dynamics given above, using sampling period Ts . Assume that F is piece-
wise constant in the intervals [kTs , (k + 1)Ts ]. Prove that the discretized system satisfies
the difference equation:
   2  
1 Ts Ts /2 F (kTs )
xk+1 = xk + − g , x0 = [0, 0]T (1)
0 1 Ts m

where xk = [z(kTs ), ż(kTs )]T . Now, assume that Ts = 1, m = 1, g = 10 and that we apply
control F (t) = mg + muk for t ∈ [kTs , (k + 1)Ts ) to obtain

xk+1 = Axk + Buk , x0 = [0, 0]T (2)

Find the numerical values of A, B. Using that, we will design controllers that enable the
UAV to hover around the desired height.

2. (30 points) Open-loop vs Closed-loop control For the following questions keep the
assumption that Ts = 1, m = 1, g = 10, i.e. use system (2).

(a) (7 points) Verify that (2) is controllable. Find the minimum norm control sequence :
49
X
min u2k s.t. x50 = [2, 0]T
u0 ,...,u49
k=0

This is an open-loop controller.

4
 (b) (7 points) Prove that a feedback law

uk = K(xk − xd ) (3)

where xd = [2, 0]T , forces the system (2) to xd , provided that the poles of A + BK
are strictly inside the unit circle. Then, design the feedback controller (i.e. find K)
so that the closed-loop poles are 0.8, 0.8.
(c) (8 points) For k = 0, .., 50, simulate system (2) under the open-loop controller 2a.
Repeat using the closed loop controller 2b. Plot the states evolution under each control
method on the same plot (create one plot for height and one for velocity). Compare
the two system responses.
(d) (8 points) Assume that due to wind disturbances, there is some noise that is added
to the velocity dynamics at each time k:
 
0
xk+1 = Axk + Buk + w x0 = [0, 0]T (4)
0.01 k

where wk ∼ N (0, 1). For k = 0, ..., 50, simulate system 4 and plot on the same graph
the UAV’s height (only the first component of the state vector) when under open-loop
and closed-loop control. Which one is more robust? Make sure you use the same
noise sequences for the open-loop and the closed-loop. Use the command
rng(0) before starting generating the random sequence. (useful function normrnd)

3. (12 points) Optimal Control The LQR control offers an effective way to tune the feedback
control law. By carefully selecting matrices Q, R, we can control the tradeoff between how
small the state is and how large the control effort is without damaging stability. Here, we
will study the infinite-horizon LQR control with cost:
∞
X
J= (x − xd )T Q(x − xd ) + uTk Ruk (5)
k=0

where xd = [2, 0]T (remember we want the system to hover żd = 0 at the desire height
zd = 2).

(a) (7 points) Simulate system (2) under the infinite-horizon LQR control (5), with
Q = I, R = 1 for k = 0, ..., 15 (useful function dlqr, be careful of the sign). Repeat
for Q = 100I and R = 1. Plot the states xk and the input uk for both cases. Compare
the plots and explain any differences.
(b) (5 points) Assume that the actuator cannot achieve forces larger than 10.5. What
LQR parameters should you tune to limit the control effort? Try to tune them such
that uk ≤ 0.5 ∀k ∈ [0, .., 15]. Report Q, R and plot the states xk and input uk .
This is a classic Model Predictive Control problem, i.e. a constrained optimal control
problem. You will find more about those topics in ESE-619.

Solution 3. 1. To translate the initial equation into space-state form, denote x = [x1 , x2 ]> =
>
[z, ż] . Therefore, ẋ = [ż, z̈] = [x2 , F/m − g] then
      
0 1 0 F 0
ẋ = x+ − g , x0 =
0 0 1 m 0

We compute the discretization,

   
F (kTs ) 0
xk+1 = Ad xk + Bd −g , x0 =
m 0

5
 Using the exact discretization method, we compute:
 
1 1 Ts
Ad = eATs = I + ATs + A2 Ts2 + . . . = I + ATs =
2 0 1

Then for Bd :
Z Ts Z Ts     " Ts2
#  " 2#
Ts
Aτ 1 τ 0 1 = Ts 0
Bd = e dτ B = dτ = 2 = 2
0 0 0 1 1 0 Ts 1 Ts

We proved that the discretized system satisfies the difference equation in the problem for-
mulation. Now we can substitute Ts = 1, m = 1, g = 10, F (t) = 10 + uk , then the system
becomes      
1 1 0.5 0
xk+1 = + uk , x0 =
0 1 1 0

2. (a) We compute the controllability matrix:

 
0.5 1.5
C = [B, AB] =
1 1

It is a full rank matrix, so the discrete-time system is controllable. In order to fin

the minimum norm control such that x1 (50) = 2 with zero velocity we formulate this
open loop control problem using least squares:

min u> u
u0 ,...,u49
 
2
s.t. x50 =
0

where x(50) can be expressed as

 
u0
x50 = A49 Bu0 + . . . Bu49 = [A49 B, . . . , B]  ...  = M U
 

u49
 
u0
49  .. 
where M = [A B, . . . , B] and U =  . . This is a minimum norm least squares
u49
problem with solution  
2
U ∗ = M > (M M > )−1 .
0
See also attached code for implementation.
(b) Method A:
     
2 2 0
Define zk = xk − . Then, xk converges to if and only if zk converges to .
0 0 0
But if we compute the state equation for zk , we find:
      
2 2 2
zk+1 = xk+1 − = A zk + + BKzk − = (A + BK)zk
0 0 0
      
2 1 1 2 2
where we used A = = . If the poles of A + BK are strictly inside
0 0 1 0 0
 
2
the unit circle, then zk will converge to zero. Thus, xk will converge to .
0

6
 Method B:
Because the poles of A + BK are strictly inside the unit circle, the system is asymp-
totically stable. Thus it is also BIBO stable. BIBO stable systems will converge to
some steady-state solution under step inputs. Denote the steady state by xss . Then,
we have:

xss = Axss + Bu
 
2
xss = (A + BK)xss − BK
0
 
2
(I − A − BK)xss = −BK
0
 
2
xss = −(I − A − BK)−1 BK
0

where since the poles of A + BK are inside the unit circle, matrix I − A − BK is
invertible. But
     
−1 2 −1 2 2
xss = −(I − A − BK) BK = −(I − A − BK) (BK + A − I − A + I) = .
0 0 0

Pole placement
We seek to place the poles of the matrix A+BK at 0.8, 0.8. This results in a charac-
teristic polynomial given by (s − 0.8)2 = s2 − 85 + 16
25 . We calculate the characteristic
polynomial of the matrix:

1 + k21 1 + k22
  
k1 k1
|sI − (A + BK)| = det sI − = s2 − s(2 + + k2 ) + (k2 − + 1)
k1 1 + k2 2 2

We equate the coefficients:

k1 8
2+ + k2 =
2 5
k1 16
k2 − +1=
2 25
1
Solving the system, we get k1 = − 25 and k2 = − 19
50 .
(c) See attached code at the end of the file for the remaining parts.
(d) See attached code at the end of the file for the remaining parts.

3. See attached code at the end of the file for the remaining parts.

(a) See attached code at the end of the file for the remaining parts.
(b) See attached code at the end of the file for the remaining parts.

Problem 4 (Robust Observers, 25 points). Suppose the linear, time-invariant system

ẋ(t) = Ax(t) + Bu(t) + w(t)

y(t) = Cx(t)

is observable and kw(t)k2 ≤ , for some  > 0. Note that the actual value of w(t) is unknown.

1. (2 points) For a given matrix L, derive the observer equations for the system.

7
 2. (3 points) Compute the error dynamics for the system, where the error is defined as
e = x̂ − x. We call this system the error system.
3. (20 points) Assume that the matrix L is such that the system (A + LC) is asymptotically
stable. Prove that there exists a real constant α such that if kek > α then the error system
is asymptotically stable. Note that α should depend on . (Hint: Determine a suitable
Lyapunov Equation and find α such that V̇ (e) < 0 whenever kek2 > α.)
Solution 4. 1. The dynamics for our observer are
x̂˙ = Ax̂ + Bu + L(ŷ − y)
= Ax̂ + Bu + LC x̂ − Ly
= (A + LC)x̂ + Bu − Ly

So our observer has dynamics

x̂˙ = (A + LC)x̂ + Bu − Ly
ŷ = C x̂

2. Now define e = x̂ − x. So we have that

ė = x̂˙ − ẋ = (A + LC)x̂ + Bu − Ly − Ax − Bu − w(t)

= (A + LC)x̂ − LCx − Ax − w(t)
= (A + LC)x̂ − (A + LC)x − w(t)
= (A + LC)e − w(t)

3. Since (A + LC) is asymptotically stable, there exists a matrix P > 0 such that (A +
LC)T P + P (A + LC) < −µI for some µ > 0. The Lyapunov function is V (e) = eT P e.
So

V̇ (e) = (eT ˙P e)
= ėT P e + eT P ė
= [eT (A + LC)T − w(t)T ]P e + eT P [(A + LC)e − w(t)]
= eT (ATc P + P Ac )e − 2w(t)T P e, where Ac = A + LC
≤ eT (ATc P + P Ac )e + 2||w(t)||2 ||P e||2 , from Cauchy Schwartz
T
≤e (ATc P
+ P Ac )e + 2||P e||2
1
= eT (ATc P + P Ac )e + 2 ||P e||22
||P e||
1
= eT (ATc P + P Ac )e + 2 eT P T P e
||P e||
1
= eT (ATc P + P Ac + 2 P 2 )e
||P e||

For the system to be stable, we need ATc P +P Ac +2 ||P1e|| P 2 ≺ 0. Recall that (A+LC)T P +
P (A + LC) ≺ −µI. So ATc P + P Ac + 2 ||P1e|| P 2 ≺ 0 ⇐⇒ −µI + 2 ||P1e|| P 2 ≺ 0. Define
λP to be the largest eigenvalue of P . From Rayleigh-Ritz inequality we have
1 1
eT (−µI + 2 P 2 )e ≤ (2 λ2 − µ)eT e
kP ek kP ek P

8
So if (2 kP1ek λ2P − µ) < 0 ⇔ 2 kP1ek λ2P < µ ⇔ 2 2
µ λP < ||P e|| our observer will be asymptot-
ically stable. Finally, note that kP ek ≤ kP k kek = λP kek. So we arrive at the conclusion
that our observer error is asymptotically stable if ||e|| > 2 µ λP = α. That means the 2-
norm of the error of our observer is guaranteed to decay to at least 2 µ λP , but we cannot
guarantee that the error goes to 0.

9
 Table of Contents
........................................................................................................................................ 1
5.2 Feedback ...................................................................................................................... 1
5.2.4) Pole placement .......................................................................................................... 1
5.2.5 Comparison ................................................................................................................ 2
5.2.3 Pole Placement ........................................................................................................... 3
5.2.6 ................................................................................................................................. 3
5.3 LQR ............................................................................................................................ 5
5.3.1 ................................................................................................................................. 5
5.3.2 ................................................................................................................................. 7

% Problem 5
clear all;
close all;

5.2 Feedback
%5.2.2) Least squares
A = [1 1; 0 1];
B = [.5;1];
M = B;
y = [2;0]; % Desired state
for i = 1:49
M = [A*M B];
end
u = M'*inv(M*M')*y;
norm_u = norm(u)

norm_u =

0.0196

5.2.4) Pole placement

K = [-1/25, -19/50]; % Computed analytically
eig(A+B*K)
x_k = [0;0];
X_cl =[0;0];
X_ol = [0;0];

for i = 1:50
u_k = K*(X_cl(:,i) - [2;0]);

X_cl = [X_cl A*X_cl(:,i) + B*u_k];

X_ol = [X_ol A*X_ol(:,i) + B*u(i)];

1
 end

ans =

0.8000
0.8000

5.2.5 Comparison
figure(1)
hold on
title('Open vs Closed loop Position')
plot(0:50, X_ol(1,:))
plot(0:50, X_cl(1,:))
legend('Open loop', 'Closed loop')

figure(2)
hold on
title('Open vs Closed loop Velocity')
plot(0:50, X_ol(2,:))
plot(0:50, X_cl(2,:))
legend('Open loop', 'Closed loop')

2
5.2.3 Pole Placement
x_ss = -inv(eye(2,2) - A - B*K)*B*K*[2;0]
Q = -inv(eye(2,2) - A - B*K)*B*K

x_ss =

2
0

Q =

1.0000 9.5000
0 0

5.2.6
x_k = [0;0];
X_cl = [0;0];
X_ol = [0;0];

for i = 1:50

3
 u_k = K*(X_cl(:,i) - [2;0]);
% Add noise
wk = [0; .01] * normrnd(0,1); % Note here that we add the same
noise to both systems.
X_cl = [X_cl A*X_cl(:,i) + B*u_k + wk];
X_ol = [X_ol A*X_ol(:,i) + B*u(i) + wk];
end

figure(3)
hold on
title('Open vs Closed loop Position')
plot(0:50, X_ol(1,:))
plot(0:50, X_cl(1,:))
legend('Open loop', 'Closed loop')

figure(4)
hold on
title('Open vs Closed loop Velocity')
plot(0:50, X_ol(2,:))
plot(0:50, X_cl(2,:))
legend('Open loop', 'Closed loop')

4
5.3 LQR
5.3.1
Q = eye(2,2);
R = 1;
K_s = -dlqr(A,B,Q,R,0);
K_l= -dlqr(A,B,100*Q,R,0);
X_s = [0;0];
X_l = [0;0];
% Simulate system

for i = 1:15
u_s = K_s*(X_s(:,i) - [2;0]);
u_l = K_l*(X_l(:,i) - [2;0]);

X_s = [X_s A*X_s(:,i) + B*u_s];

X_l = [X_l A*X_l(:,i) + B*u_l];
end

figure(5)
hold on
title('LQR small Q vs large Q')
plot(0:15, X_s(1,:))
plot(0:15, X_l(1,:))

5
legend('Small Q', 'large Q')

figure(6)
hold on
title('LQR small Q vs large Q')
plot(0:15, X_s(2,:))
plot(0:15, X_l(2,:))
legend('Small Q', 'large Q')

6
5.3.2
We need to penalize u, to keep the control signal below 0.5. The values below are not optimized, but do
achieve the desired behavior.

R = 10; % Try R = 10 for the small Q

K_s = -dlqr(A,B,Q,R,0);
R = 650; % Try R = 650 for large Q
K_l= -dlqr(A,B,100*Q,R,0);
X_s = [0;0];
X_l = [0;0];
% Simulate system
U_s = zeros(15,1);
U_l = zeros(15,1);
for i = 1:15
U_s(i) = K_s*(X_s(:,i) - [2;0]);
U_l(i) = K_l*(X_l(:,i) - [2;0]);

X_s = [X_s A*X_s(:,i) + B*U_s(i)];

X_l = [X_l A*X_l(:,i) + B*U_l(i)];

% Save controls for plotting

end

figure(7)

7
hold on
title('Position (small Q vs large Q)')
plot(0:15, X_s(1,:))
plot(0:15, X_l(1,:))
legend('Small Q', 'large Q')

figure(8)
hold on
title('Velocity (small Q vs large Q)')
plot(0:15, X_s(2,:))
plot(0:15, X_l(2,:))
legend('Small Q', 'large Q')

figure(9)
hold on
title('Control (small Q vs large Q)')
plot(0:14, U_s)
plot(0:14, U_l)
legend('Small Q', 'large Q')

8
9
These values seem to keep the control below 0.5. But these Q,R values may not be robust in the case that
we decide to move to a different height.

Published with MATLAB® R2016b

10