%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%       DC-motor Example        %%%%%%%%%%
%%%%%%%%%%  Modified by Olle Kjellqvist  %%%%%%%%%%
%%%%%%%%%%          2018/07/20           %%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Define the LQG problem
A = [0 0; 1 -1];
B = [20; 0];
N = B;
C = [0 1];
Q1 = C'*C;
Q2 = 1;
R1 = 1;
R2 = 1;

% LQG design (without time-domain constraints)
L = lqr(A,B,Q1,Q2);
K = lqe(A,N,C,R1,R2);
P = ss(A,B,C,0);
ctrl_lqg = -reg(P,L,K);

% Define extended plant and Youla parameterization
Popen = minreal([P 0 P; 1 0 1; P 1 P]);

% Stabilize the open-loop system using some controller C0
C0 = -0.01;                       % Stabilizing controller
Pext = feedback(Popen,C0,3,3,1);  % Resulting stabilized system

% Recover partial systems
Pzw = Pext(1:2,1:2);
Pzu = Pext(1:2,3);
Pyw = Pext(3,1:2);
Pyu = Pext(3,3);

% Vectorize Pzw and PzuPyw
Pzw_vec = minreal([Pzw(1,1); Pzw(2,1); Pzw(1,2); Pzw(2,2)]);
PzuPyw_vec = minreal([Pzu(1)*Pyw(1); Pzu(2)*Pyw(1); Pzu(1)*Pyw(2); Pzu(2)*Pyw(2)]);

% Generate a Ritz expansion of Laguerre filters
a = 3;         % Cut-off frequency
N = 12;        % 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 step responses
t = 0:0.01:4;
Pzw_step = step(Pzw_vec,t);
PzuPywQ_step = step(PzuPyw_vec*Q,t);

% 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:
-0.4 <= Pzw_step(:,4) + squeeze(PzuPywQ_step(:,4,:))*q <= 0.4               % meas dist -> ctrl signal
-0.2 <= Pzw_step(301:401,1) + squeeze(PzuPywQ_step(301:401,1,:))*q <= 0.2   % load dist -> output (at 3...4)

cvx_end

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

% Plot step responses output dist -> ctrl
figure(1)
step(feedback(ctrl_lqg,P),feedback(ctrl_opt,P),tf(0.4))
legend('lqg', 'Q-design')

% Plot step responses load dist -> output
figure(2)
step(feedback(P,ctrl_lqg),feedback(P,ctrl_opt),tf(0.2),tf(-0.2),5)
legend('lqg', 'Q-design')

% Plot controller bode diagrams
figure(3)
bode(ctrl_lqg,ctrl_opt)
legend('lqg', 'Q-design')