close all; clear; clc;
format long
xi=0.3; %Decay rate
h=0.1; %Sampling period
%Proposed system
A=[1.38 -0.2077 6.715 -5.676;
-0.5814 -4.29 0 0.675;
1.067 4.273 -6.654 5.893;
0.048 4.273 1.343 -2.104];
B=[0 0; 5.679 0; 1.136 -3.146; 1.136 0];
% C=[1 0 0 -1; 0 1 0 0];
n=size(A,1);
m=size(B,2);
%Solver Margins
margen=1e-3*eye(n);
Margen=1e-3*eye(9*n);
%% We do define Matrices
P1=sdpvar(n,n);
Q1=sdpvar(n,n);
Z1=sdpvar(n,n);
P2=sdpvar(n,n);
Q2=sdpvar(n,n);
Z2=sdpvar(n,n);
N=sdpvar(n,n,'full','real');
V=sdpvar(m,n,'full','real');
E1=sdpvar(n,n,'full','real');
E2=sdpvar(n,n,'full','real');
F1=sdpvar(n,n,'full','real');
F2=sdpvar(n,n,'full','real');
G1=sdpvar(n,n,'full','real');
G2=sdpvar(n,n,'full','real');
W1=sdpvar(n,n,'full','real');
W2=sdpvar(n,n,'full','real');
M1=sdpvar(n,n);
M2=sdpvar(n,n);
M3=sdpvar(n,n);
M4=sdpvar(n,n);
M5=sdpvar(n,n);
Y11=sdpvar(n,n,'full','real');
Y12=sdpvar(n,n,'full','real');
Y22=sdpvar(n,n,'full','real');
U11=sdpvar(n,n,'full','real');
U12=sdpvar(n,n,'full','real');
U22=sdpvar(n,n,'full','real');
%% Set the Matrices
%=====C1=====%
%PI
pi11=Q1+G1+G1'+A*N'+N*A'+h*Y11+xi*P1;
pi12=F1-G1+G2'+h*Y12+B*V+N*A';
pi13=-F1;
pi14=P1-N'+N*A';
pi15=B*V;
pi21=F1'-G1'+G2+h*Y12'+V'*B'+A*N';
pi22=F2+F2'-G2-G2'+h*Y22+B*V+V'*B'+M2;
pi23=-F2;
pi24=-N'+V'*B';
pi25=B*V;
pi31=-F1';
pi32=-F2';
pi33=-exp(-xi*h)*Q1;
pi34=zeros(n);
pi35=zeros(n);
pi41=P1-N+A*N';
pi42=-N+B*V;
pi43=zeros(n);
pi44=(h^2)*Z1-N-N';
pi45=B*V;
pi51=V'*B';
pi52=V'*B';
pi53=zeros(n);
pi54=V'*B';
pi55=-M1;
PI=[pi11 pi12 pi13 pi14 pi15;
pi21 pi22 pi23 pi24 pi25;
pi31 pi32 pi33 pi34 pi35;
pi41 pi42 pi43 pi44 pi45;
pi51 pi52 pi53 pi54 pi55];
%PHI
phi11=N*A';
phi12=zeros(n);
phi13=zeros(n);
phi14=N*A';
phi21=M4'+V'*B';
phi22=M4';
phi23=zeros(n);
phi24=M4'+V'*B';
phi31=zeros(n);
phi32=zeros(n);
phi33=zeros(n);
phi34=zeros(n);
phi41=-N;
phi42=zeros(n);
phi43=zeros(n);
phi44=-N;
phi51=M5'+V'*B';
phi52=M5';
phi53=zeros(n);
phi54=M5'+V'*B';
PHI=[phi11 phi12 phi13 phi14;
phi21 phi22 phi23 phi24;
phi31 phi32 phi33 phi34;
phi41 phi42 phi43 phi44;
phi51 phi52 phi53 phi54];
%PSI
psi11=Q2+E1+E1'+h*U11-N-N'+xi*P2;
psi12=W1-E1+E2'+h*U12-N;
psi13=-W1;
psi14=P2-N'-N;
psi21=W1'-E1'+E2+h*U12'-N';
psi22=W2+W2'-E2-E2'+h*U22+M3;
psi23=-W2;
psi24=-N';
psi31=-W1';
psi32=-W2';
psi33=-exp(-xi*h)*Q2;
psi34=zeros(n);
psi41=P2-N-N';
psi42=-N;
psi43=zeros(n);
psi44=(h^2)*Z2-N-N';
PSI=[psi11 psi12 psi13 psi14;
psi21 psi22 psi23 psi24;
psi31 psi32 psi33 psi34;
psi41 psi42 psi43 psi44];
C1=[PI PHI;PHI' PSI];
%=====C2=====%
Y=[Y11 Y12;Y12' Y22];
F=[F1' F2']';
C2=[Y F;F' h*exp(-xi*h)*Z1];
%=====C3=====%
G=[G1' G2']';
C3=[Y G;G' h*exp(-xi*h)*Z1];
%=====C4=====%
U=[U11 U12;U12' U22];
W=[W1' W2']';
C4=[U W;W' h*exp(-xi*h)*Z2];
%=====C5=====%
E=[E1' E2']';
C5=[U E;E' h*exp(-xi*h)*Z2];
%% We solve the Feasibility problem
const=[P1-margen>=0, P2-margen>=0, Q1-margen>=0, Q2-margen>=0, Z1-margen>=0, Z2-margen>=0,...
M1-margen>=0, M2-margen>=0, M3-margen>=0, M4-margen>=0, M5-margen>=0, -C1-Margen>=0, C2>=0, C3>=0,...
C4>=0, C5>=0]; %Constraints
ops=sdpsettings('solver','SDPT3');
optimize(const,[],ops);
% solvesdp(const)
%% To Double
P_1=double(P1);
P_2=double(P2);
Q_1=double(Q1);
Q_2=double(Q2);
Z_1=double(Z1);
Z_2=double(Z2);
Nb=double(N);
Vb=double(V);
M_1=double(M1);
M_2=double(M2);
M_3=double(M3);
M_4=double(M4);
M_5=double(M5);
C_1=double(C1);
C_2=double(C2);
C_3=double(C3);
C_4=double(C4);
C_5=double(C5);
%% Calculate Design Matrices for ETM
K=Vb*(inv(Nb))';
S1=(inv(Nb))*M_1*(inv(Nb))';
S2=(inv(Nb))*M_2*(inv(Nb))';
S3=(inv(Nb))*M_3*(inv(Nb))';
S4=M_4*(inv(Nb))';
S5=M_5*(inv(Nb))';
%We further check the definiteness of some matrices involved in the Theorem
%1 conditions (P_{1,2}>0,Q_{1,2}>0,Z_{1,2}>0,S_{1-3}>0,C_1<0,C_{2-5}>=0), by checking its eigenvalues.
indices=0;
if min(eig(P_1))<=0
indices=indices+1;
disp('Fails P1')
end
if min(eig(P_2))<=0
indices=indices+1;
disp('Fails P2')
end
if min(eig(Q_1))<=0
indices=indices+1;
disp('Fails Q1')
end
if min(eig(Q_2))<=0
indices=indices+1;
disp('Fails Q2')
end
if min(eig(Z_1))<=0
indices=indices+1;
disp('Fails Z1')
end
if min(eig(Z_2))<=0
indices=indices+1;
disp('Fails Z2')
end
%
if min(eig(M_1))<=0
indices=indices+1;
disp('Fails M1')
end
if min(eig(M_2))<=0
indices=indices+1;
disp('Fails M2')
end
if min(eig(M_3))<=0
indices=indices+1;
disp('Fails M3')
end
if min(eig(M_4))<=0
indices=indices+1;
disp('Fails M4')
end
if min(eig(M_5))<=0
indices=indices+1;
disp('Fails M5')
end
%
if max(eig(C_1))>=0
indices=indices+1;
disp('Fails C1')
end
if min(eig(C_2))<=0
indices=indices+1;
disp('Fails C2')
end
if min(eig(C_3))<=0
indices=indices+1;
disp('Fails C3')
end
if min(eig(C_4))<=0
indices=indices+1;
disp('Fails C4')
end
if min(eig(C_5))<=0
indices=indices+1;
disp('Fails C5')
end
%
if min(eig(S1))<=0
indices=indices+1;
disp('Fails S1')
end
if min(eig(S2))<=0
indices=indices+1;
disp('Fails S2')
end
if min(eig(S3))<=0
indices=indices+1;
disp('Fails S3')
end
disp('****************')
car=['Fail ',num2str(indices), ' out of ',num2str(19)];
disp(car)