
clear
%To compute the upper bound and lower bound of E(z)
%with E(z)=(A,B,C,D)
%for k=4:0.1:6

H0 = fir1(23,1/6);
%k
%H0=firpm(23,[0 .1 .3 1],[1 1 0 0]);
den1 = [1 zeros(1,23)];
Vz0 = [H0(1) 0 0 0;0 H0(2) 0 0; 0 0 H0(3) 0; 0 0 0 H0(4); zeros(2,4)]; %coefficients of z^0
Vz1 = [0 0 H0(7) 0; 0 0 0 H0(8); 0 0 0 0;0 0 0 0;H0(5) 0 0 0;0 H0(6) 0 0];%coefficients of z^-1
Vz2 = [0 0 0 0;0 0 0 0;H0(9) 0 0 0;0 H0(10) 0 0;0 0 H0(11) 0;0 0 0 H0(12)];
Vz3 = [H0(13) 0 0 0;0 H0(14) 0 0; 0 0 H0(15) 0; 0 0 0 H0(16); zeros(2,4)];
Vz4 = [0 0 H0(19) 0; 0 0 0 H0(20); 0 0 0 0;0 0 0 0;H0(17) 0 0 0;0 H0(18) 0 0];
Vz5 = [0 0 0 0;0 0 0 0;H0(21) 0 0 0;0 H0(22) 0 0;0 0 H0(23) 0;0 0 0 H0(24)];


sys1 = tf(1,[1 0],-1);
sys2 = tf(1,[1 0 0],-1);
sys3 = tf(1,[1 0 0 0],-1);
sys4 = tf(1,[1 0 0 0 0],-1);
sys5 = tf(1,[1 0 0 0 0],-1);
syssys = Vz0+sys1*Vz1+sys2*Vz2+sys3*Vz3+sys4*Vz4+sys5*Vz5;
[syssys,g] = balreal(syssys);
   % compute balanced realization
       elim = (g<1e-5);          % small entries of g -> negligible states
       rsys = modred(syssys,elim);


[A,B,C,D] = ssdata(rsys);

 feasptest = 0; % 1 = test, others = no test
 qr = 1;  % qr = 1/0 dis/en-able QR algorithm in mincx
 target = 1;	% termination target of mincx
% 
 if feasptest == 1
    %load xinit
    xinit=[];
 else   
    xinit=[];
 end


setlmis([]);
    %P1= lmivar(1,[1 0]);
    %P2= lmivar(1,[1 0]);
    %P3= lmivar(1,[1 0]);
    %P4= lmivar(1,[1 0]);
    %P5= lmivar(1,[1 0]);
    %P6= lmivar(1,[1 0]);
    P = lmivar(1,[size(A,1) 1]);
    beta = lmivar(1,[1 0]);
    
%    lmiterm([1 1 1 X],1,A_M,'s')
%    lmiterm([1 2 1 X],B_M',1)
%    lmiterm([1 2 2 gamma],1,1)
%    lmiterm([1 1 1 P],M',M)
%    lmiterm([1 1 1 gamma],-1,1)

    lmiterm([1 1 1 P],A',A)
    lmiterm([1 1 1 P],-1,1)
    lmiterm([1 1 1 0],C'*C)
    
    lmiterm([1 2 1 P],B',A)
    lmiterm([1 2 1 0],D'*C)

    lmiterm([1 2 2 P],B',B)
    lmiterm([1 2 2 beta],-1,1)
    lmiterm([1 2 2 0],D'*D) 

hflmi=getlmis; 
    
   if feasptest == 1
      [tmin xinit] = feasp(hflmi);
   end
    
	n = decnbr(hflmi);
	weight = zeros(n,1);
	for j = 1:n
	   [gj] = defcx(hflmi,j,beta);
	   weight(j) =gj;
	end

	% LMI optimization
    option = [1e-9 0 0 qr 0];


    [copt,xopt] = mincx(hflmi,weight,option,xinit,target); 

	P = dec2mat(hflmi,xopt,P);
    upper_bound = sqrt(dec2mat(hflmi,xopt,beta));
    %upper_bound

    
setlmis([]);

    Q = lmivar(1,[size(A,1) 1]);
    theta = lmivar(1,[1 0]);
    
    lmiterm([1 1 1 Q],theta*A',A)
    lmiterm([1 1 1 Q],-theta,1)
    lmiterm([1 1 1 theta],1,-C'*C)
    
    lmiterm([1 2 1 Q],theta*B',A)
    lmiterm([1 2 1 theta],1,-D'*C)

    lmiterm([1 2 2 Q],theta*B',B)
    lmiterm([1 2 2 0],1)
    lmiterm([1 2 2 theta],1,-D'*D) 
    
    lmiterm([-2 1 1 theta],1,1)

hflmi=getlmis; 
    
   if feasptest == 1
      [tmin xinit] = feasp(hflmi);
   end
    
	n = decnbr(hflmi);
	weight = zeros(n,1);
	for j = 1:n
	   [gj] = defcx(hflmi,j,theta);
	   weight(j) =(gj);
	end


    option = [1e-9 0 0 qr 0];


    [copt,xopt] = mincx(hflmi,weight,option,xinit,target); 


	Q = dec2mat(hflmi,xopt,Q);
    lower_bound = sqrt(1/dec2mat(hflmi,xopt,theta));
    upper_bound
    lower_bound
    ratio = upper_bound/lower_bound

