findone.m

Software for design and tuninig of SISO and MIMO contol systems

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

global xwc_valley

if strcmp(fun,'imc2com')
   confun='imc2constr';
elseif strcmp(fun,'Usercas')
   confun='Usercascon';
else
	confun='imc1constr';
end
% Default parameter values
tole=Acc*Mp;
F=tole+1;

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

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


%      --- Initial guess of frequency
n=length(Xo)+1;
Xo(n)=1;

%      --- 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(' ');
options=optimset('Display','off','LargeScale','off');
freq_window=10;
vlb(n)=Xo(n)/freq_window;
vub(n)=Xo(n)*freq_window;
continue=1;
while continue
global_optimal=0;
while ~global_optimal  
   [Xo,Func]=fmincon(fun,Xo,[],[],[],[],vlb,vub,confun,options,...
      y,Epsilon,-1,0,n,Mp,p,m,q,qf,qd,pd);
   if Func < -2*Mp
      [Epsilon,err]=adjust(Epsilon,1.5*Mp,fun,Xo,y,n,p,m,q,qf,qd,pd,0.1); 
   else % global check
      grid_x=zeros(3,n);
      for j=1:3
         grid_freq(j)=Xo(n)*(j-0.8)*3;
         vlb(n)=grid_freq(j)/freq_window;
         vub(n)=grid_freq(j)*freq_window;
         [grid_x(j,1:n-1)]=fmincon(fun,Xo(1:n-1),[],[],[],[],vlb(1:n-1),vub(1:n-1),...
            [],options,y,Epsilon,-1,grid_freq(j),0,Mp,p,m,q,qf,qd,pd);
         grid_x(j,n)=grid_freq(j);
         [grid_x(j,:),grid_Func(j)]=fmincon(fun,grid_x(j,:),[],[],[],[],vlb,vub,...
            [],options,y,Epsilon,-1,0,n,Mp,p,m,q,qf,qd,pd);
      end
      [temp,temp1]=min(grid_Func);
   if (temp<Func)
      Xo=grid_x(temp1,:);
      vlb(n)=grid_freq(temp1)/freq_window;
      vub(n)=grid_freq(temp1)*freq_window;
   else
      global_optimal=1;
   end
   end
end
fprintf('         %.3f               %.4f            %.3f \n',-Func,Xo(n),Epsilon(1));
if abs(Mp+Func)<tole
   continue=0;
else
   [Epsilon,err]=adjust(Epsilon,Mp,fun,Xo,y,n,p,m,q,qf,qd,pd,tole/10); 
end
end

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


  if err
   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 ');
   continue=0;
  end
Epsilon
[Results,refreq]=optfunf(Epsilon,fun,decade,numpdec,vub,vlb,Xo,y,-1,p,m,q,qf,...
   qd,pd,Mp,0.01,Acc,Tcanc,order,mq);
   [FL,b]=max(Results(:,2));
   Results
   [row,col]=size(Results);
     for i=3:col
      Xo(i-2)=Results(b,i);
     end
    Xo(col-1)=Results(b,1);


      fprintf('\n      Results: \n\n ');
      fprintf('\n      Filter time constant: %g ',Epsilon(1));
      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



% --- 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(1));
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 oscillatory output response  \n');
   fprintf('          of any process in this control system!!\n\n\n');
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)];
	%
   % Search for Epsilon using Qubic spline interpolation
   %
   [x_spike,spk_val(j,1)]=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)=-spk_val(j,1);
   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(1);
   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(1),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(1); E_ub=Inf; 
       Epsilon(1)=Epsilon(1)+Epsilon(1)/5;
   end
while stop==0
   index=j-4*fix(j/4)+1;
   j=j+1;
   [x_spike,spk_val(index,1)]=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)=-spk_val(index,1);
   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(1);
   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(1),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(1);
      if j >= 4
         Epsilon(1) = spline(spk_val(:,1)-spk_val(:,2),spk_val(:,3),Mp2);
      else
         Epsilon(1) = spline(spk_val(1:j,1)-spk_val(1:j,2),spk_val(1:j,3),Mp2);
      end
       if Epsilon(1) > E_ub | Epsilon(1) < E_lb
          Epsilon(1)=(E_lb+E_ub)/2;
       end
   elseif temp < -Mp2/50 %then decrease Epsilon
      	E_ub=Epsilon(1);
      if j >= 4
         Epsilon(1) = spline(spk_val(:,1)-spk_val(:,2),spk_val(:,3),Mp2);
      else
         Epsilon(1) = spline(spk_val(1:j,1)-spk_val(1:j,2),spk_val(1:j,3),Mp2);
      end
       if Epsilon(1) > E_ub | Epsilon(1) < E_lb
          Epsilon(1)=(E_lb+E_ub)/2;
       end
 	else
    stop=1;
 	end %if
end % while (stop == 0)
fprintf('  -------------------------------------------------------------------------\n');

      fprintf('\n\n\n      Results: \n\n ');
      fprintf('\n      Filter time constant                 : %g ',Epsilon(1));
      fprintf('\nThe most oscillatory peak magnitude        : %g ',spk_val(index,1));
      fprintf('\n      At the frequency                     : %g ',x_spike(n));
      fprintf('\n peak high (relative to the adjoin 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
end %if (temp < Mp2/2)
 disp(' ');
return;

%------------------------
% Sub Function definition
% to bring the peak down
% -----------------------
function [Epsilon,err]=adjust(Epsilon,Mp,fun,Xo,y,n,p,m,q,qf,qd,pd,Acc)
E=Epsilon(1);
err=0;
F=feval(fun,Xo,y,Epsilon,-1,0,n,0,p,m,q,qf,qd,pd)+Mp;
k=0; Fnext=F; Enext=E;
while (F*Fnext > 0)
   k = k + 1;
   E = Enext; F = Fnext;
   Enext = E+E;
   if Enext > 1e3
      err=1;
      return
   end
   Epsilon(1)=Enext;
   Fnext=feval(fun,Xo,y,Epsilon,-1,0,n,0,p,m,q,qf,qd,pd)+Mp;
end
a=E; b=Enext;
Em=(a+b)/2;
Epsilon(1)=Em;
Fv=feval(fun,Xo,y,Epsilon,-1,0,n,0,p,m,q,qf,qd,pd);
Fm= Fv + Mp;
while (abs(Fm) > Acc)
   Em=(a+b)/2;
   Epsilon(1)=Em;
   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