clear all
close all
% PROG. 2 - Solution of the continuous-time 
%
% Solves the problem:
%   min  tr(E'PE)
%   s.t. [ WA'+AW+Y'B'+BY   WC_z '+Y'D_z']
%        [         *                                -I           ] < 0
%
%           [N    E']
%           [ *    W] > 0

%% System Definition

A=[0 1;-1 2]; B=[1;0]; E=[1;1]; x0=0; Cz=[1 0]; Dz=[2];
nx=size(A,2); nu=size(B,2); nz=size(Cz,1); nw=size(E,2);

%% Decision variables
% YALMIP represents unknowns as "sdpvar" objects, not plain doubles.
W = sdpvar(nx,nx,'symmetric');
Y = sdpvar(nu,nx,'full');
N=sdpvar(nw,nw,'symmetric');

%% Objective function
% We want to minimize x2. trace() is only needed for matrix-valued
% objectives; here x2 is scalar, so trace(x2) is just x2.
Objective = trace(N);
%Objective = 0;   % uncomment to only test feasibility (ignore the objective)

%% Constraints
% YALMIP/LMI solvers work with non-strict inequalities (>=, <=), so strict
% inequalities (>, <) are approximated with a small margin (1e-8).
Constraints = [];

M11 = W*A'+A*W+Y'*B'+B*Y;
M12 = W*Cz'+Y'*Dz';
%M13 = E;
M22 = -eye(nz);
%M23 = zeros(nz,nw);
%M33 = -mu*eye(nw);
Constraints = [Constraints, [M11 M12; M12' M22] <= -1e-8];        % 

O11 = N;
O12 = E';
O22 = W;
Constraints = [Constraints, [O11 O12;O12' O22] >= 1e-8];       % 

%% Solver options
% Selects Mosek as the underlying solver; verbose=0 silences solver output.
Options = sdpsettings('solver', 'mosek', 'verbose', 0);

%% Solve
Solution = optimize(Constraints, Objective, Options);

%% Diagnostics
% Solution.problem == 0 means YALMIP found a feasible/optimal solution;
% any other value flags an issue (infeasible, solver error, etc.).
disp(Solution.problem)
disp(Solution.info)
disp(yalmiperror(Solution.problem))

%% Extract and report the result
if Solution.problem == 0
    Value_Y = value(Y);   % feasible x1
    Value_W=value(W);
    K=Value_Y*inv(Value_W)
    Value_N = sqrt(trace(value(N)))  % minimum x2 that satisfies the constraints
else
    disp('Unfeasible Problem.');
end

s=tf([1 0],1);
Ac=A+B*K;Bc=E;
Cc=Cz+Dz*K; Dc=0;
S=ss(Ac,Bc,Cc,Dc);
[Hn,Hd]=ss2tf(Ac,Bc,Cc,Dc)
step(S)