%%% Doyle--Stein 1979 design example

% Define the LQG problem

A = [-4 -3; 1 0];
B = [1; 0];
N = [-61; 35];
C = [1 2];

Q1 = 80 * [1 sqrt(35); sqrt(35) 35];
Q12 = [0; 0];
Q2 = 1;
R1 = 1;
R2 = 1;

% LQG design (without constraint on M_s)

L = lqr(A,B,Q1,Q2);
K = lqe(A,N,C,R1,R2);
plant = ss(A,B,C,0);
ctrl_lqg = -reg(plant,L,K);

% Define extended plant and Youla parameterization

sqrtQ1 = sqrt([80 80*35; 0 0]);
sqrtQ12 = [0; 0];
sqrtQ2 = 1;
sqrtR1 = [N'; 0 0];
Aext = A;
Bext = [sqrtR1' 0*B B];
Cext = [sqrtQ1; 0*C; C];
Dext = [0*A 0*B sqrtQ12; 0*C 0 sqrtQ2; 0*C sqrt(R2) 0];
Pext = ss(Aext,Bext,Cext,Dext);

Pzw = Pext(1:3,1:3);
Pzu = Pext(1:3,4);
Pyw = Pext(4,1:3);
Pyu = Pext(4,4);

% Define frequency grid for approximation of objective function

n = 50;                  % Number of frequency grid points
w = logspace(-2,3,n).';

% Define Q basis functions

M = 10;                  % Number of terms in Q 
a = 5*bandwidth(Pyu);    % Laguerre basis pole chosen to match plant
s = zpk('s');
Qq = ss(1);
for m = 1:M
  Qq(m) = ss(1/(s/a+1)^m);
end
Q = minreal(Qq);

Q_fr = squeeze(freqresp(Q, w));  % Frequency response of Q

% Trapezoidal weights for approximation of frequency integral 

d = @(x)([x(2:end);x(end)]-[x(1);x(1:end-1)]);
dw = d(w);

% Calculate open-loop plant frequency responses

Pzw_fr = freqresp(Pzw, w);
Pzu_fr = freqresp(Pzu, w);
Pyw_fr = freqresp(Pyw, w);
Pyu_fr = freqresp(Pyu, w);

% Perform the optimization using CVX

cvx_clear
cvx_begin 
cvx_solver sedumi
cvx_precision high
variable q(size(Qq,2))

Qvec = q'*Q_fr;

objective = 0;   % Objective function

sens = [];
for k = 1:n
  % Approximate the objective function by sum over all grid frequencies
  objective = objective + pow_pos(norm(Pzw_fr(:,:,k)-Pzu_fr(:,:,k)*Qvec(k)*Pyw_fr(:,:,k)),2)*dw(k); 
  % Calculate sensitivity contraint over all grid frequencies (S = 1-T)
  sens{k} = 1 - Qvec(k) * Pyu_fr(:,:,k);
end

minimize objective

subject to:
for k = 1:n
  abs(sens{k}) <= 2.0;
end

cvx_end

Q = minreal(Qq*q);                      % Calculate resulting Q
ctrl_opt = minreal(feedback(Q,-Pyu));   % Calculate resulting controller

% Plot controller Bode diagrams
figure(1)
bode(ctrl_lqg,ctrl_opt,{1e-1,1e3})

% Plot sensitivity functions
figure(2)
bodemag(feedback(1,ctrl_lqg*plant),feedback(1,ctrl_opt*plant),tf(2));


