findunc.m

Software for design and tuninig of SISO and MIMO contol systems

function [E,decade,xwc,vub,vlb,vubs,vlbs]=FindUnc(E,fun,decade,Acc,Na,vub,vlb,numpdec,Xo,y,Omega,p,m,q,qd,Mp,inf,Tcanc,xn,mq)

% Default parameter values
options(5)=1;
tole=Acc*Mp;
F=tole+1;
FL=0;
st=1;
Mp=Mp(1);
Ratio=0.999;
if isempty(Xo)
   Xo=xn;
end
n=length(Xo);

fprintf('\n\n       One/two degree of freedom controller');
fprintf('\n       ************************************ \n\n');

%      ----------- ALGORITHM FOR FINDING UNCERTAINTY BOUNDS -----------

Upper=xn+Ratio*xn;
Lower=xn-Ratio*xn;

%      --- Initial guess of frequency

if Omega == 0
 Xo(n+1)=1/E;
elseif Omega > 0
 Xo(n+1)=Omega;
end;


%      --- Minimal Epsilon (Mp condition)
options=optimset('Display','off','LargeScale','off');
clc;
fprintf('\n       Looking for uncertainty so that the maximum peak is %g+-%g \n\n',Mp,tole);
disp(['       Max. peak            Frequency         Ratio ']);
disp(['                         [rad/unit time]    ']);
disp(['       ---------         ---------------      ----- ']);
disp(' ');

while st==1

  if FL==0
   Lower(n+1)=Xo(n+1)/10;
   Upper(n+1)=Xo(n+1)*10;
   [Xo,Func]=fmincon(fun,Xo,[],[],[],[],Lower,Upper,'imc1constr',...
      options,y,E,-1,0,n+1,Mp,p,m,q,qd,Tcanc);
   Func=-Func;

  elseif FL>0
   disp(' ');
   disp(['       Max. peak            Frequency          Ratio ']);
   disp(['                         [rad/unit time]    ']);
   disp(['       ---------         ---------------       ----- ']);
   disp(' ');
   Lower(n+1)=Xo(n+1)/10;
   Upper(n+1)=Xo(n+1)*10;
   [Xo,Func]=fmincon(fun,Xo,[],[],[],[],Lower,Upper,'imc1constr',...
      options,y,E,-1,0,n+1,Mp,p,m,q,qd,Tcanc);
   Func=-Func;
   FL=0;
  end;
  F= Mp-Func;

  fprintf('         %.3f               %.4f            %.3f \n',Func,Xo(n+1),Ratio);

   z=round(log(Xo(n+1))/log(10));
   decade(1)=z-2;
   decade(2)=z+2;
   Omega=Xo(n+1);

  Xopt=Xo;
  if F <= -tole 

    delta = 0.1;
    k=0; Fnext=F; Rnext=Ratio;

    while (F*Fnext > 0) & Rnext > 0
	Ratio = Rnext; F = Fnext;
	Rnext = Ratio - delta;
	if Rnext<=0
	 Rnext=0;
	end
	for i=1:n
	 Upper(i)=xn(i)+Rnext*xn(i);
	 Lower(i)=xn(i)-Rnext*xn(i);
	 if Xopt(i)<Lower(i)
	  Xo(i)=Lower(i);
	 elseif Xopt(i)>Upper(i)
	  Xo(i)=Upper(i);
	 else
	  Xo(i)=Xopt(i);
	 end
	end
	Fv=feval(fun,Xo,y,E,-1,0,n+1,0,p,m,q,qd,Tcanc);
	Fnext= Fv + Mp;
    end

  a=Ratio; b=Rnext;
  Rm=(a+b)/2;
  for i=1:n
   Upper(i)=xn(i)+Rm*xn(i);
   Lower(i)=xn(i)-Rm*xn(i);
   if Xopt(i)<Lower(i) 
    Xo(i)=Lower(i);
   elseif Xopt(i)>Upper(i) 
    Xo(i)=Upper(i);
   else
    Xo(i)=Xopt(i);
   end
  end
  Fv=feval(fun,Xo,y,E,-1,0,n+1,0,p,m,q,qd,Tcanc);
  Fm= Fv + Mp;

      while (abs(Fm) > Acc*.1)
	 Rm=(a+b)/2;
	  for i=1:n
	   Upper(i)=xn(i)+Rm*xn(i);
	   Lower(i)=xn(i)-Rm*xn(i);
	   if Xopt(i)<Lower(i)
	    Xo(i)=Lower(i);
	   elseif Xopt(i)>Upper(i) 
	    Xo(i)=Upper(i);
	   else
	    Xo(i)=Xopt(i);
	   end
	  end
         Fv=feval(fun,Xo,y,E,-1,0,n+1,0,p,m,q,qd,Tcanc);
	 Fm= Fv + Mp;
	 if Fm > 0
	    b = Rm;
	 else
	    a = Rm;
	 end
      end

   Ratio=Rm;

  else   % if F>=-tole


%    --- Globality check
%    --- High frequency worst case parameters

x=Upper;
x(n+1)=Omega;
Lower(n+1)=x(n+1)/10;
Upper(n+1)=x(n+1)*10;

[x]=fmincon(fun,x,[],[],[],[],Lower,Upper,[],...
   options,y,E,-1,inf,0,0,p,m,q,qd,Tcanc);

Fv=feval(fun,x,y,E,1,inf,0,0,p,m,q,qd,Tcanc);

x(n+1)=Fv*inf;
[x,FL]=fmincon(fun,x,[],[],[],[],Lower,Upper,[],...
   options,y,E,-1,0,n+1,Mp,p,m,q,qd,Tcanc);

FL=-FL;

  if FL>Mp+tole

   fprintf('\n       Globality check \n\n');
   Xo=x;

  else   % if FL>Mp+tole
   Upper=[];
   Lower=[];
   for i=1:n
    Upper(i)=xn(i)+Ratio*xn(i);
    Lower(i)=xn(i)-Ratio*xn(i);
   end
   [Results,refreq]=optfunf(E,fun,decade,numpdec,Upper,Lower,Upper,y,-1,p,m,q,q,...
      '1','1',Mp,0.01,Acc,Tcanc,inf,mq);
   [FL,b]=max(Results(:,2));
   [row,col]=size(Results);
   if FL > Mp+tole 
     for i=3:col
      Xo(i-2)=Results(b,i);
     end
    Xo(col-1)=Results(b,1);
   else
    st=0;
   end

   end  %if FL>Mp+tole
  end  %if F<-tole

  if Ratio<Acc
   fprintf('\n      !!! Warning !!! \n ');
   fprintf('\n      The uncertainty bounds close to a perfect model \n ');
   fprintf('     while the reqiured Maximum peak was not achieved. \n ');
   fprintf('     Choose a Filter time constant larger than displayed below.  \n\n ');
   st=0;
  end

 end       %while

if FL > Func
 Func=FL;
 for i=3:col
  Xo(i-2)=Results(b,i);
 end
end
      fprintf('\n      Results: \n\n ');
      fprintf('\n      Filter time constant: %g ',E);
      fprintf('\n      Maximum peak        : %g ',Func);
      fprintf('\n      At frequency        : %g \n\n',Omega);
      fprintf('      The plant parameters are:\n\n');
      xwc=[];
      vubs=[];
      vlbs=[];
      for k=1:n
	fprintf('           %.3f   <=  x(%.0f) = %.3f  <= %.3f  \n',Lower(k),k,Xo(k),Upper(k));
        xwc(k)=Xo(k);
	vub(k)=Upper(k);
	vlb(k)=Lower(k);
	vubs=[vubs ' ' num2str(Upper(k))];
	vlbs=[vlbs ' ' num2str(Lower(k))];
      end
      fprintf('\n\n      The nominal parameters are:\n\n');
      for k=1:n
	 fprintf('                       xn(%.0f) = %.3f   \n',k,xn(k));
      end

save wcp E Omega xwc
 disp(' ');
return;