clc
clear all

warning('off','YALMIP:strict')
%defining state matrices of TS fuzzy system

m11 = 0.3;
m12 = 0.54;   % m_h = 0.4;
m_h = 0.4;
m2 = 0.01;
m3 = 0.078;

c1 = 1.00;
c2 = 36.00;
c3 = 1.53;

k1 = 0.01;
k2 = 3667;
k3 = 2152;


A1=[0 0 0 1 0 0;
    0 0 0 0 1 0;
    0 0 0 0 0 1;
    -(k1+k2)/m11   k2/m11       0    -(c1+c2)/m11   c2/m11         0;
        k2/m2   -(k2+k3)/m2   k3/m2    c2/m2      -(c2+c3)/m2    c3/m2;
          0         k3/m3    -k3/m3       0         c3/m3       -c3/m3];

A2=[0 0 0 1 0 0;
    0 0 0 0 1 0;
    0 0 0 0 0 1;
    -(k1+k2)/m12 k2/m12 0 -(c1+c2)/m12 c2/m12 0;
    k2/m2 -(k2+k3)/m2 k3/m2 c2/m2 -(c2+c3)/m2 c3/m2;
    0 k3/m3 -k3/m3 0 c3/m3 -c3/m3];

B1=[0; 0; 0; 0; +1/m2; -1/m3];

E1=[ 0;0;0;0;0;1/m3];

B2=B1; 

E2=E1;

C1=[1 0 0 0 0 0];
C2=C1;

I=eye(1);
Z=0;%
%%
%For feasibility problem we assume
gamma=0.01;
u_max=5000;
%defining LMI variables
X=sdpvar(6);
M1=sdpvar(1,6);
M2=sdpvar(1,6);

%definning LMI terms

%proposition 1:
lmi1=(X>0);

lmi2=([-0.5*(X*A1'- M1'*B1'+ A1*X - B1*M1 + X*A1' - M1'*B1' + A1*X - B1*M1) -0.5*(E1+E1) -0.5*X*(C1+C1)';
    -0.5*(E1+E1)' gamma^2*I Z;
    -0.5*(C1+C1)*X Z I]>=0);

lmi3=([-0.5*(X*A1'-M2'*B1'+A1*X-B1*M2+X*A2'-M1'*B2'+A2*X-B2*M1) -0.5*(E1+E2) -0.5*X*(C1+C2)';
    -0.5*(E1+E2)' gamma^2*I Z;
    -0.5*(C1+C2)*X Z I]>=0);

lmi4=([-0.5*(X*A2'-M2'*B2'+A2*X-B2*M2+X*A2'-M2'*B2'+A2*X-B2*M2) -0.5*(E2+E2) -0.5*X*(C2+C2)';
    -0.5*(E2+E2)' gamma^2*I Z;
    -0.5*(C2+C2)*X Z I]>=0);

lmi5=([X M1';M1 u_max^2*I]>=0)
lmi6=([X M2';M2 u_max^2*I]>=0)

LMI=lmi1+lmi2+lmi3+lmi4+lmi5+lmi6;

%Solver options
opts=sdpsettings;
opts.solver='mosek';

%Solve LMI for feasibilitX
solvesdp(LMI,[],opts);

%Check for feasibilitX <=======> "primal residuals" all have to be positive
checkset(LMI)

%Returning the value of LMIs
X=double(X);
M1=double(M1);
M2=double(M2);


%Parameter of Controller
P=X^-1
F1=M1*P
F2=M2*P


eig(P)