%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%      Doyle-Stein Example      %%%%%%%%%%
%%%%%%%%%%  Modified by Olle Kjellqvist  %%%%%%%%%%
%%%%%%%%%%          2018/07/20           %%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 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];
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);

% Vectorize Pzw and PzuPyw
Pzw_vec = minreal([Pzw(1,1); Pzw(2,1); Pzw(3,1);
	                Pzw(1,2); Pzw(2,2); Pzw(3,2);
						 Pzw(1,3); Pzw(2,3); Pzw(3,3)]);
			
PzuPyw = Pzu*Pyw;
PzuPyw_vec =  minreal([PzuPyw(1,1); PzuPyw(2,1); PzuPyw(3,1);
	                    PzuPyw(1,2); PzuPyw(2,2); PzuPyw(3,2);
						     PzuPyw(1,3); PzuPyw(2,3); PzuPyw(3,3)]);
					 
% Generate a Ritz expansion of Laguerre filters
a = 10;        % Cut-off frequency
N = 10;        % Number of elements in the basis expansion

% This is a little bit of magic to generate the state-space matrices of
% the Ritz expansion.
Dr = kron([-a -2*a*(-1).^(1:(N-1))],ones(N, 1));
Ar = full(spdiags(Dr, 0:N-1, N, N));
Br = eye(N);
Cr = sqrt(2*a)*(-1).^(0:N-1);
Q = ss(Ar,Br,Cr,0);
[A,B,C] = ssdata(PzuPyw_vec*Q);

% Set up H2 cost matrices
Cquad   = B'*lyap(A',C'*C)*B;                                % Quadratic term
Clin    = 2*Pzw_vec.B'*lyap(Pzw_vec.A',A,Pzw_vec.C'*C)*B;    % Linear term

% Compute frequency response of T = -Pyu*Q
w = logspace(-2,3,100);
T_freq = squeeze(freqresp(-Pyu*Q,w))';

% Run CVX
cvx_clear
cvx_begin 
cvx_solver sedumi
variable q(N)

% Formulate quadratic cost
objective = q'*Cquad*q + Clin*q
minimize objective

subject to:
abs(1-T_freq*q) <= 2   % constraint on S = 1-T

cvx_end

% Formulate controller
Q = Q*q;
ctrl_opt = -feedback(Q,Pyu);

% Plot controller Bode diagrams
figure(1)
bode(ctrl_lqg,ctrl_opt,{1e-1,1e3})
legend('lqg', 'Q-design')

% Plot sensitivity functions
figure(2)
bodemag(feedback(1,ctrl_lqg*plant),feedback(1,ctrl_opt*plant),tf(2));
legend('lqg', 'Q-design')

