% For process G=(1.239Z(-1)-0.9282Z(-2))/(1-1.607Z(-1)+0.6086Z(-2)) Shi Jia's AIChE 2006
% With input delay and parameter uncertainties
clear all
clc;
% system matrices
% A=[1.607 -0.6086;1 0];
% B=[1;0];C=[1.2390,-0.9282];
% %--------------------------------------------------------------------------
A=[1.607 1;-0.6086 0];B=[1.2390;-0.9282];C=[1 0];
E=eye(2);Fa=1*[0.04 0;0.04 0];Fb=0.5*[0.10;0.14];
% %--------------------------------------------------------------------------
d=1; % constant delay (the maximum delay is 5 after 21 iterations)
% structure uncertainty
% E=1*eye(2);Fa=1*[0.02 0.02;0 0];Fb=0*[0.1;0.14];
% augmented system matrices
A1=[A zeros(2);C*A zeros(1,2);zeros(1,2) [1 1]]; % 4*4
A2=[zeros(2) zeros(2);zeros(1,2) [1 0];zeros(1,2) [0 0]];
G=[-C eye(1) 0;zeros(1,2) [1 0];zeros(1,2) [1 1]];
L=[0 0 1 0]; Dg=[eye(2);C;zeros(1,2)];
Bg=[B;C*B;zeros(1,1)];
% augmentd system uncertainty matrices
bar_E=[E;C*E;zeros(1,size(E,2))];
bar_Fa=[Fa zeros(2)];
% obtain system dimension
n=size(A1,1);m=size(Bg,2);l=size(G,1);
% set tuning parameter to adjust closed-loop poles
alpha=0;
% LMI solver
setlmis([])
% LMI variables
% P>0
[P1,P1n,sP1]=lmivar(1,[n,1]);
[P3,P3n,sP3]=lmivar(1,[n,1]);
[P2,P2n,sP2]=lmivar(2,[n,n]);
P=lmivar(3,[sP1,sP2;sP2',sP3]);
% T>0 T=inv(P)
[T1,T1n,sT1]=lmivar(1,[n,1]);
[T3,T3n,sT3]=lmivar(1,[n,1]);
[T2,T2n,sT2]=lmivar(2,[n,n]);
T=lmivar(3,[sT1,sT2;sT2',sT3]);
% P0>0
[P01,P01n,sP01]=lmivar(1,[n,1]);
[P03,P03n,sP03]=lmivar(1,[n,1]);
[P02,P02n,sP02]=lmivar(2,[n,n]);
P0=lmivar(3,[sP01,sP02;sP02',sP03]);
% R>0
[R1,R1n,sR1]=lmivar(1,[n,1]);
[R3,R3n,sR3]=lmivar(1,[n,1]);
[R2,R2n,sR2]=lmivar(2,[n,n]);
R=lmivar(3,[sR1,sR2;sR2',sR3]);
% S>0 S=inv(R)
[S1,S1n,sS1]=lmivar(1,[n,1]);
[S3,S3n,sS3]=lmivar(1,[n,1]);
[S2,S2n,sS2]=lmivar(2,[n,n]);
S=lmivar(3,[sS1,sS2;sS2',sS3]);
% Q>0
[Q1,Q1n,sQ1]=lmivar(1,[n,1]);
[Q3,Q3n,sQ3]=lmivar(1,[n,1]);
[Q2,Q2n,sQ2]=lmivar(2,[n,n]);
Q=lmivar(3,[sQ1,sQ2;sQ2',sQ3]);
% M1
[M11,M11n,sM11]=lmivar(2,[n,n]);
[M12,M12n,sM12]=lmivar(2,[n,n]);
[M13,M13n,sM13]=lmivar(2,[n,n]);
[M14,M14n,sM14]=lmivar(2,[n,n]);
M1=lmivar(3,[sM11,sM12;sM13,sM14]);
% M2
[M21,M21n,sM21]=lmivar(2,[n,n]);
[M22,M22n,sM22]=lmivar(2,[n,n]);
[M23,M23n,sM23]=lmivar(2,[n,n]);
[M24,M24n,sM24]=lmivar(2,[n,n]);
M2=lmivar(3,[sM21,sM22;sM23,sM24]);
% M3
[M31,M31n,sM31]=lmivar(2,[n,n]);
[M32,M32n,sM32]=lmivar(2,[n,n]);
[M33,M33n,sM33]=lmivar(2,[n,n]);
[M34,M34n,sM34]=lmivar(2,[n,n]);
M3=lmivar(3,[sM31,sM32;sM33,sM34]);
K1=lmivar(2,[m,n]);
K2=lmivar(2,[m,n]);
L1=lmivar(2,[n,l]);
L2=lmivar(2,[n,l]);
eps=lmivar(1,[1,1]);
% LMI terms
SYS_lmi1=newlmi;
lmiterm([SYS_lmi1,1,1,P01],2*alpha-1,1);
lmiterm([SYS_lmi1,1,1,Q1],1,1);
lmiterm([SYS_lmi1,1,1,M11],1,1,'s');
lmiterm([SYS_lmi1,1,2,P02],2*alpha-1,1);
lmiterm([SYS_lmi1,1,2,Q2],1,1);
lmiterm([SYS_lmi1,1,2,M12],1,1);
lmiterm([SYS_lmi1,1,2,-M13],1,1);
lmiterm([SYS_lmi1,2,2,P03],2*alpha-1,1);
lmiterm([SYS_lmi1,2,2,Q3],1,1);
lmiterm([SYS_lmi1,2,2,M14],1,1,'s');
lmiterm([SYS_lmi1,1,3,-M21],1,1);
lmiterm([SYS_lmi1,1,4,-M23],1,1);
lmiterm([SYS_lmi1,2,3,-M22],1,1);
lmiterm([SYS_lmi1,2,4,-M24],1,1);
lmiterm([SYS_lmi1,1,5,M11],-1,1);
lmiterm([SYS_lmi1,1,5,-M31],1,1);
lmiterm([SYS_lmi1,1,6,M12],-1,1);
lmiterm([SYS_lmi1,1,6,-M33],1,1);
lmiterm([SYS_lmi1,2,5,M13],-1,1);
lmiterm([SYS_lmi1,2,5,-M32],1,1);
lmiterm([SYS_lmi1,2,6,M14],-1,1);
lmiterm([SYS_lmi1,2,6,-M34],1,1);
lmiterm([SYS_lmi1,1,7,M11],-d,1);
lmiterm([SYS_lmi1,1,8,M12],-d,1);
lmiterm([SYS_lmi1,2,7,M13],-d,1);
lmiterm([SYS_lmi1,2,8,M14],-d,1);
lmiterm([SYS_lmi1,1,9,0],A1');
lmiterm([SYS_lmi1,1,9,-K1],1,Bg');
lmiterm([SYS_lmi1,2,10,0],A1');
lmiterm([SYS_lmi1,1,11,0],d*(A1'-eye(n)));
lmiterm([SYS_lmi1,1,11,-K1],d,Bg');
lmiterm([SYS_lmi1,2,12,0],d*(A1'-eye(n)));
lmiterm([SYS_lmi1,1,13,0],-bar_Fa');
lmiterm([SYS_lmi1,1,13,-K1],1,-Fb');
lmiterm([SYS_lmi1,2,13,0],bar_Fa');
lmiterm([SYS_lmi1,3,3,P1],-1,1);
lmiterm([SYS_lmi1,3,3,P01],1,1);
lmiterm([SYS_lmi1,3,4,P2],-1,1);
lmiterm([SYS_lmi1,3,4,P02],1,1);
lmiterm([SYS_lmi1,4,4,P3],-1,1);
lmiterm([SYS_lmi1,4,4,P03],1,1);
lmiterm([SYS_lmi1,3,5,M21],-1,1);
lmiterm([SYS_lmi1,3,6,M22],-1,1);
lmiterm([SYS_lmi1,4,5,M23],-1,1);
lmiterm([SYS_lmi1,4,6,M24],-1,1);
lmiterm([SYS_lmi1,3,7,M21],-d,1);
lmiterm([SYS_lmi1,3,8,M22],-d,1);
lmiterm([SYS_lmi1,4,7,M23],-d,1);
lmiterm([SYS_lmi1,4,8,M24],-d,1);
lmiterm([SYS_lmi1,3,9,0],A2');
lmiterm([SYS_lmi1,3,9,-K2],1,Bg');
lmiterm([SYS_lmi1,4,9,-L2],G',1);
lmiterm([SYS_lmi1,4,10,0],A2');
lmiterm([SYS_lmi1,4,10,-L2],G',1);
lmiterm([SYS_lmi1,3,11,0],d*A2');
lmiterm([SYS_lmi1,3,11,-K2],d,Bg');
lmiterm([SYS_lmi1,4,11,-L2],G',d);
lmiterm([SYS_lmi1,4,12,0],d*A2');
lmiterm([SYS_lmi1,4,12,-L2],G',d);
lmiterm([SYS_lmi1,3,13,-K2],-1,Fb');
lmiterm([SYS_lmi1,5,5,Q1],-1,1);
lmiterm([SYS_lmi1,5,5,M31],-1,1,'s');
lmiterm([SYS_lmi1,5,6,Q2],-1,1);
lmiterm([SYS_lmi1,5,6,M32],-1,1);
lmiterm([SYS_lmi1,5,6,-M33],-1,1);
lmiterm([SYS_lmi1,6,6,Q3],-1,1);
lmiterm([SYS_lmi1,6,6,M34],-1,1,'s');
lmiterm([SYS_lmi1,5,7,M31],-d,1);
lmiterm([SYS_lmi1,5,8,M32],-d,1);
lmiterm([SYS_lmi1,6,7,M33],-d,1);
lmiterm([SYS_lmi1,6,8,M34],-d,1);
lmiterm([SYS_lmi1,6,9,-L1],G',1);
lmiterm([SYS_lmi1,6,10,-L1],G',1);
lmiterm([SYS_lmi1,6,11,-L1],G',d);
lmiterm([SYS_lmi1,6,12,-L1],G',d);
lmiterm([SYS_lmi1,7,7,R1],-d,1);
lmiterm([SYS_lmi1,7,8,R2],-d,1);
lmiterm([SYS_lmi1,8,8,R3],-d,1);
lmiterm([SYS_lmi1,9,9,T1],-1,1);
lmiterm([SYS_lmi1,9,10,T2],-1,1);
lmiterm([SYS_lmi1,10,10,T3],-1,1);
lmiterm([SYS_lmi1,10,10,eps],1,bar_E*bar_E');
lmiterm([SYS_lmi1,10,12,eps],d,bar_E*bar_E');
lmiterm([SYS_lmi1,11,11,S1],-d,1);
lmiterm([SYS_lmi1,11,12,S2],-d,1);
lmiterm([SYS_lmi1,12,12,S3],-d,1);
lmiterm([SYS_lmi1,12,12,eps],d^2,bar_E*bar_E');
lmiterm([SYS_lmi1,13,13,eps],-1,1);
SYS_lmi2=newlmi;
lmiterm([-SYS_lmi2,1,1,P],1,1);
lmiterm([-SYS_lmi2,1,2,0],1);
lmiterm([-SYS_lmi2,2,2,T],1,1);
SYS_lmi3=newlmi;
lmiterm([-SYS_lmi3,1,1,R],1,1);
lmiterm([-SYS_lmi3,1,2,0],1);
lmiterm([-SYS_lmi3,2,2,S],1,1);
SYS_lmi4=newlmi;
lmiterm([-SYS_lmi4,1,1,P0],1,1);
SYS_lmi5=newlmi;
lmiterm([-SYS_lmi5,1,1,Q],1,1);
lmisys = getlmis;
[tm,xf]=feasp(lmisys);
Pf=dec2mat(lmisys,xf,P);
Tf=dec2mat(lmisys,xf,T);
Rf=dec2mat(lmisys,xf,R);
Sf=dec2mat(lmisys,xf,S);
% K1=dec2mat(lmisys,xf,K1);
% K2=dec2mat(lmisys,xf,K2);
% L1=dec2mat(lmisys,xf,L1);
% L2=dec2mat(lmisys,xf,L2);
% eps=dec2mat(lmisys,xf,eps);
k=0; % iterative number
tmin=1;
iterate_no=500;
tol=1;
while (tmin>0) && (k<iterate_no)
[Pk,Tk,Rk,Sk,K1,K2,L1,L2,copt]=Direct_LMI_SSP_mincx(Pf,Tf,Rf,Sf,A1,A2,Bg,bar_E,bar_Fa,Fb,G,d,n,m,l,alpha);
% check the feasibility with given K1,K2,L1,L2
[P,R,Q,P0,tmin]=Direct_LMI_SSP_stability_test(A1,A2,Bg,G,bar_E,bar_Fa,Fb,d,K1,K2,L1,L2,alpha);
if tmin<0
disp('The controller gains K1 and K2 are :')
K1
K2
disp('The observer gains L1 and L2 are :')
L1
L2
IterN=k
else
Pf=Pk;
Tf=Tk;
Rf=Rk;
Sf=Sk;
k=k+1;
end
end
% test the feasibility
% hat_A1=blkdiag(bar_A1+bar_Bg*K1,bar_A1);
% eig(hat_A1);
%
% hat_A2=[bar_A2+bar_Bg*K2,L2*bar_G;zeros(size(A2,1)),bar_A2+L2*bar_G];
% eig(hat_A2);
%
% A11=[bar_A1+bar_Bg*K1,L1*bar_G;zeros(size(A2,1)),bar_A1+L1*bar_G];
% eig(A11);
