%%% DC-servo design example

% 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
s = tf('s');
C0 = ss(0.01/(s+1));            % Strictly proper stabilizing controller
Pext = feedback(Popen,C0,3,3);  % Resulting stabilized system

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

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

% Compute step responses
t = 0:0.1:4;
cl_stepresp = step(Pzw,t);
for k=1:M
    cl_stepresp = cl_stepresp - step(Pzu * Qq(k) * Pyw, t) * q(k);
end

objective = 0;   % Objective function

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); 
end

minimize objective

subject to:
-0.4 <= cl_stepresp(:,2,2) <= 0.4        % meas dist -> ctrl signal
-0.2 <= cl_stepresp(31:41,1,1) <= 0.2  % load dist -> output (at 3...4)

cvx_end

Q = minreal(Qq*q);                         % Calculate resulting Q

ctrl_opt = C0 + minreal(feedback(Q,-Pyu)); % Calculate resulting controller

% Plot step responses meas dist -> ctrl signal
figure(1)
step(feedback(ctrl_lqg,P),feedback(ctrl_opt,P),tf(0.4))

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

% Plot controller bode diagrams
figure(3)
bode(ctrl_lqg,ctrl_opt)






