findtwo.m

Software for design and tuninig of SISO and MIMO contol systems

function [Epsilon,decade,xwc,qd,err]=FindTwo(Epsilon,fun,decade,Acc,Na,vub,vlb,...
   numpdec,Xo,y,p,m,q,qf,qd,pd,Mp,Mp2,inf,Tcanc,mq,order)
global xwc_valley

E=Epsilon(2);
% Default parameter values
tole=Acc*Mp;
F=tole+1;
FL=0;
st=1;
imag=sqrt(-1);
Xorg=Xo;
Uorg=vub;
Lorg=vlb;
% err
err=0;

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

%      ----------- ALGORITHM FOR FINDING FILTER TIME CONSTANT -----------

options=optimset('LargeScale','off','Display','off');

%   ----- initial guess epsilon
Epsilon(2)=E;

%      --- Initial guess of frequency

 n=length(Xo)+1;
 Xo(n)=1/E;

%      --- Minimal Epsilon (Mp condition)

fprintf('\n       Looking for epsilon so that the maximum peak is %g+-%g \n\n',Mp,tole);
disp(['       Max. peak            Frequency         Epsilon ']);
disp(['                         [rad/unit time]    ']);
disp(['       ---------         ---------------      ------- ']);
disp(' ');
skip=0;
while st==1

  if FL==0
   vlb(n)=Xo(n)/10;
   vub(n)=Xo(n)*10;
   Epsilon(2)=E;
   if strcmp(qd,'1')
       qdnum=1; qdden=1;
   else
       [qd,qdnum,qdden]=qd_mat(Tcanc(2,:),mq,Epsilon(2),order,y);
   end
   s=imag;
   e=Epsilon(2);
   if angle(eval(qd))< 0
      fprintf('\n\n\n !! qd = %s is lag. at e = %g !!\n\n',qd,Epsilon(2));
      disp(['Try reducing either the order of controllers q and/or qd. If the process and model are stable,']);
      disp(['and IMCTUNE still can not find a filter time constant after qd is set to 1 (by setting the ']);
      disp(['model disturbance lag to 1), then there is probably a problem with the input data. ']);
      disp(['If the model and process are unstable, then find the filter time constant that minimizes the ']);
      disp(['sensitivity function. This calculation must be done manually using the upper bound frequency ']);
      disp(['response calculation.']);
      disp([' ']);
      disp(['Tuning stopped ...']);
      xwc=Xo(1:n-1);
      Omega=Xo(n);
      err=1;
      return;
   end %if
   [Xo,Func]=fmincon(fun,Xo,[],[],[],[],vlb,vub,'imc2pseucon',options,y,Epsilon,-1,0,n,Mp,p,m,q,qf,qd,pd);
   Func=-Func;

  elseif FL>0
   disp(' ');
   disp(['       Max. peak            Frequency          Epsilon ']);
   disp(['                         [rad/unit time]    ']);
   disp(['       ---------         ---------------       ------- ']);
   disp(' ');
   vlb(n)=Xo(n)/10;
   vub(n)=Xo(n)*10;
   
      if strcmp(qd,'1')
       qdnum=1; qdden=1;
      else
       [qd,qdnum,qdden]=qd_mat(Tcanc(2,:),mq,Epsilon(2),order,y);
      end

   [Xo,Func]=fmincon(fun,Xo,[],[],[],[],vlb,vub,'imc2pseucon',options,y,Epsilon,-1,0,n,Mp,p,m,q,qf,qd,pd);
   Func=-Func;
   FL=0;
  end;
  F= Mp-Func;

  fprintf('         %.3f               %.4f            %.3f \n',Func,Xo(n),E);

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

  if F <= -tole 
    if E>0
     delta = abs(E)*(.5);
    elseif E == 0
     delta = 0.1;
    end
    k=0; Fnext=F; Enext=E; skip =0;

    while (F*Fnext > 0)
	 k = k + 1;
	 E = Enext; F = Fnext;
	 Enext = E + 2^k*delta;
    Epsilon(2)=Enext;
    if strcmp(qd,'1')
       qdnum=1; qdden=1;
    else
       [qd,qdnum,qdden]=qd_mat(Tcanc(2,:),mq,Epsilon(2),order,y);
    end
         Fv=feval(fun,Xo,y,Epsilon,-1,0,n,0,p,m,q,qf,qd,pd);
	 Fnext= Fv + Mp;
    	if Fnext < F
          skip=1;
          st=0;
          break
    	end
    end

  a=E; b=Enext;
  Em=(a+b)/2;
  Epsilon(2)=Em;
   if strcmp(qd,'1')
     qdnum=1; qdden=1;
   else
    [qd,qdnum,qdden]=qd_mat(Tcanc(2,:),mq,Epsilon(2),order,y);
   end
Fv=feval(fun,Xo,y,Epsilon,-1,0,n,0,p,m,q,qf,qd,pd);
  Fm= Fv + Mp;
  
  if ~skip
      while (abs(Fm) > Acc*.1)
	 Em=(a+b)/2;
    Epsilon(2)=Em;
   if strcmp(qd,'1')
       qdnum=1; qdden=1;
   else
        [qd,qdnum,qdden]=qd_mat(Tcanc(2,:),mq,Epsilon(2),order,y);
    end
    Fv=feval(fun,Xo,y,Epsilon,-1,0,n,0,p,m,q,qf,qd,pd);
	 Fm= Fv + Mp;
	 if Fm > 0
	    b = Em;
	 else
	    a = Em;
	 end
      end
   end
   E=Em;


  else   % if F>=-tole


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

     x=vub;
	Epsilon(2)=E;
   if strcmp(qd,'1')
       qdnum=1; qdden=1;
   else
     [qd,qdnum,qdden]=qd_mat(Tcanc(2,:),mq,Epsilon(2),order,y);
    end
 [x,temp]=fmincon(fun,x,[],[],[],[],vlb,vub,'imc2pseucon',options,y,Epsilon,-1,inf,0,0,p,m,q,qf,qd,pd);

      Fv=feval(fun,x,y,Epsilon,1,inf,0,0,p,m,q,qf,qd,pd);
      x(n)=Fv*inf;

     [x,temp]=fmincon(fun,x,[],[],[],[],vlb,vub,'imc2pseucon',options,y,Epsilon,-1,0,n,Mp,p,m,q,qf,qd,pd);
     FL=-temp;

  if FL>Mp+tole

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

  else   % if FL>Mp+tole
Epsilon
   [Results,refreq]=optfunf(Epsilon,fun,decade,numpdec,Uorg,Lorg,Xorg,y,-1,p,m,q,qf,qd,pd,Mp,0.01,Acc,Tcanc,order,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 E>inf
   fprintf('\n      !!! Warning !!! \n ');
   fprintf('\n      The filter time constant reached infinity \n ');
   fprintf('     while the reqiured Maximum peak was not achieved. \n ');
   fprintf('     Choose a Maximum peak larger than displayed below.  \n\n ');
   st=0;
  end

 end       %while
if skip
   temp='The function Mp=Mp(Epsilon) is concave';
   temp=strvcat(temp,' and has minimum Mp higher than required Mp.');
   temp=strvcat(temp,' The resulting Epsilon is the approximated ');
   temp=strvcat(temp,' Epsilon which give the minimum Mp. ');
   msgbox(temp,'Warning');
   return
end

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=[];
      for k=1:n-1
	 fprintf('                           x(%.0f) = %.2f   \n',k,Xo(k));
         xwc(k)=Xo(k);
      end
 if E>inf
    return;
 end % if

Epsilon(2)=E;


% --- Find Epsilon (The peak with Mp2 higher than its adjoining valley)

x=Xo(1:n-1);
vlb=vlb(1:n-1);
vub=vub(1:n-1);
npoints=length(Results);

fprintf('\n\n  Peak Height Tunning ...');
fprintf('\n\n  Epsilon = %g',Epsilon(2));
fprintf('\n\n  Iteration     Frequency    Magnitude \n');
fprintf('   --------    ---------     --------- ');

nspikes=1; i_valley=[npoints]; i_spike=[npoints]; spike_up=1;
for i=npoints:-1:1
if Results(i,2) >= Results(i_spike(nspikes),2)
 	if Results(min([i+1 npoints]),2)==Results(i_valley(max([nspikes-1 1])),2) & ~spike_up
       spike_up=1;
	   fprintf([' *valley(' num2str(max([nspikes-1 1])) ')']);
   end
   i_spike(nspikes)=i;
   i_valley(nspikes)=i;
elseif Results(i,2) < Results(i_valley(nspikes),2)
 	if Results(i+1,2)==Results(i_spike(nspikes),2) & spike_up
	   fprintf([' *peak(' num2str(nspikes) ')']);
   	spike_up=0;
   	nspikes=nspikes+1;
 		i_valley(nspikes)=i;
 	end
   i_spike(nspikes)=i;
 	i_valley(nspikes-1)=i;
 end %if 
 fprintf('\n    %3.0f         %.4f        %.5f',npoints-i+1,Results(i,1),Results(i,2));
end  %for

   
% Find the maximum of all peak height
[temp,temp1]=max(Results(i_spike,2)-Results(i_valley,2));

fprintf('\n\n                                         Magnitude\n');
fprintf('                       ---------------------------------------------------\n');
fprintf('  Iter_n    Epsilon    spike     freq.     valley     freq.    spike-valley\n');
fprintf('  ------    -------    -----     -----     ------     -----    ------------\n\n');

if (temp < Mp2/2)
   fprintf('       !!There is no peak in the closed loop frequency response  \n');
   spk_val=[0 0];
   index=1;
   x_spike=1.0;
   n=1;
else
	stop=0; j=1; index=j; spk_val=zeros(4,3);
   freq=(Results(i_spike(temp1),1)-Results(i_valley(temp1),1))/2;
   vlb_spike=[vlb (Results(i_spike(temp1),1)-freq)];
   vub_spike=[vub (Results(i_spike(temp1),1)+freq)];
	x_spike=[Results(i_spike(temp1),3:end) Results(i_spike(temp1),1)];
   
   % change frequency range to plot
   z=round(log(x_spike(n))/log(10));
   decade(1)=z-2;
   decade(2)=z+2;
	%
   % Search for Epsilon using Qubic spline interpolation
   %
   [x_spike,temp]=fmincon(fun,x_spike,[],[],[],[],vlb_spike,vub_spike,[],options,y,...
      Epsilon,-1,0,n,0,p,m,q,qf,qd,pd);
   spk_val(j,1)=-temp;
   xwc=x_spike(1:n-1);
   [Omega_valley,spk_val(j,2)]=fminbnd('max_val',Results(i_spike(temp1+1),1),x_spike(n),...
      options,fun,xwc,Epsilon,vlb,vub,y,p,m,q,qf,qd,pd);
   spk_val(j,3)=Epsilon(2);
   temp=spk_val(j,1)-spk_val(j,2);
   fprintf('   %3.0f      %3.5f    %1.5f   %3.5f   %1.5f   %3.5f   %1.5f \n'...
      ,j,Epsilon(2),spk_val(j,1),x_spike(n),spk_val(j,2),Omega_valley,temp);
   
   if temp <= Mp2
    fprintf('\n\n The oscillatory response of previous Mp tuning is allready acceptable !! \n\n');
    stop=1;
 	else
       E_lb=Epsilon(2); E_ub=Inf; 
       Epsilon(2)=Epsilon(2)+Epsilon(2)/5;
   end
while stop==0
   index=j-4*fix(j/4)+1;
   j=j+1;
   if strcmp(qd,'1')
       qdnum=1; qdden=1;
   else
    [qd,qdnum,qdden]=qd_mat(Tcanc(2,:),mq,Epsilon(2),order,y);
    end
[x_spike,temp]=fmincon(fun,x_spike,[],[],[],[],vlb_spike,vub_spike,[],options,y,...
      Epsilon,-1,0,n,0,p,m,q,qf,qd,pd);
   spk_val(index,1)=-temp;
   xwc=x_spike(1:n-1);
   
   [Omega_valley,spk_val(index,2)]=fminbnd('max_val',Results(i_spike(temp1+1),1),x_spike(n),...
      options,fun,xwc,Epsilon,vlb,vub,y,p,m,q,qf,qd,pd);
   spk_val(index,3)=Epsilon(2);
   temp=spk_val(index,1)-spk_val(index,2);
   
   fprintf('   %3.0f      %3.5f    %1.5f   %3.5f   %1.5f   %3.5f   %1.5f \n'...
      ,j,Epsilon(2),spk_val(index,1),x_spike(n),spk_val(index,2),Omega_valley,temp);
   temp=temp-Mp2;
   if temp > Mp2/50 % then increase Epsilon
         E_lb=Epsilon(2);
      if j >= 4
         Epsilon(2) = spline(spk_val(:,1)-spk_val(:,2),spk_val(:,3),Mp2);
      else
         Epsilon(2) = spline(spk_val(1:j,1)-spk_val(1:j,2),spk_val(1:j,3),Mp2);
      end
       if Epsilon(2) > E_ub | Epsilon(2) < E_lb
          Epsilon(2)=(E_lb+E_ub)/2;
       end
   elseif temp < -Mp2/50 %then decrease Epsilon
      	E_ub=Epsilon(2);
      if j >= 4
         Epsilon(2) = spline(spk_val(:,1)-spk_val(:,2),spk_val(:,3),Mp2);
      else
         Epsilon(2) = spline(spk_val(1:j,1)-spk_val(1:j,2),spk_val(1:j,3),Mp2);
      end
       if Epsilon(2) > E_ub | Epsilon(2) < E_lb
          Epsilon(2)=(E_lb+E_ub)/2;
       end
 	else
    stop=1;
 	end %if
end % while (stop == 0)
end %if (temp < Mp2/2)
fprintf('  -------------------------------------------------------------------------\n');

      fprintf('\n\n\n      Results: \n\n ');
      fprintf('\n      Filter time constant          : %g ',Epsilon(2));
      fprintf('\nThe most oscillatory peak magnitude : %g ',spk_val(index,1));
      fprintf('\n      At the frequency              : %g ',x_spike(n));
      fprintf('\n        peak - valley               : %g \n\n',...
         spk_val(index,1)-spk_val(index,2));
      fprintf('  The plant parameters at that peak are:\n\n');
      for k=1:n-1
	 fprintf('                           x(%.0f) = %.2f   \n\n',k,xwc(k));
      end

 disp(' ');