tune_solve1.asv

内模控制器（IMC）工具箱。包括参数整定、PID控制器参数转换等

function [Epsilon,xwc]=tune_solve1(Epsilon,fun,xwc,Mp,vub,vlb,y,ps,ms,qs,order,input_delay,pd_prime,pds,mq,E_handle,tole)
global Xwc_ub_w % The variable that contain information of the upper bound.
tic
n=length(order);
ln=length(vub)+1;
option=optimset('LargeScale','off','Display','off');
switch fun
case 'mimo1com'
   constr='mimo1com_con';
   enorm_fun='enorm';
   enorm_con='enorm_constr';
   E_step=Epsilon{1};
case 'mimo2com'
   constr='mimo2com_con';
   enorm_fun='enorm2';
   enorm_con='enorm2_constr';
   E_step=Epsilon{1};
case 'mimopartial'
   constr='mimopartial_con';
   enorm_fun='epartial';
   enorm_con='epartial_constr';
   E_step=Epsilon{2};
end
for i=1:n % determine the worse case parameter from the existing upper bounde results
   if ~isempty(Xwc_ub_w{i,i}) & ~all(Xwc_ub_w{i,i}(:,3:end)==0)
      [temp,temp1]=max(Xwc_ub_w{i,i}(:,1));
      xwc(i,:)=[Xwc_ub_w{i,i}(temp1,3:end) ...
            Xwc_ub_w{i,i}(temp1,2)];
   else 
      xwc(i,:)=[xwc(i,2:end) xwc(i,1)];
   end
end
if strcmp(fun,'mimopartial')| strcmp(fun,'mimo2com')
  for i=1:n
    [qd{i,i},qdnum{i,i},qdden{i,i}]=qd_mat(pd_prime{i,i},mq{i},Epsilon{2}(i),order(i),y);
    qd{i,i}=strrep(qd{i,i},'e',['e(' num2str(i) ')']);
  end
  qds=cell2str(qd);
end    
for i=1:n
vub(ln)=max([0.01 5*xwc(ln)]); % Moving the frequency limits while optimizing
vlb(ln)=max([0.004 0.2*xwc(ln)]);
xwctemp=[];temp=[];
[xwctemp(1,:),temp(1)]=fmincon(fun,xwc(i,:),[],[],[],[],vlb,vub,constr,option,...
   [],Epsilon,-1,0,ps,ms,qs,order,i,i,input_delay,pds,qds);
vub(ln)= vub(ln)*10;
vlb(ln)= vlb(ln)*10;
[xwctemp(2,:),temp(2)]=fmincon(fun,vub,[],[],[],[],vlb,vub,constr,option,...
   [],Epsilon,-1,0,ps,ms,qs,order,i,i,input_delay,pds,qds);
vub(ln)= vub(ln)*10;
vlb(ln)= vlb(ln)*10;
[xwctemp(3,:),temp(3)]=fmincon(fun,vlb,[],[],[],[],vlb,vub,constr,option,...
   [],Epsilon,-1,0,ps,ms,qs,order,i,i,input_delay,pds,qds);
[fval(i),index]=min(temp);
xwc(i,:)=xwctemp(index,:);
end
fval
iter=1;
disp('Iteration : Filter time constants --> ');
if strcmp(fun,'mimo1com')| strcmp(fun,'mimo2com')
   disp([iter Epsilon{1}]);
else
   disp([iter Epsilon{2}]);
end
previous_Epsilon=Epsilon;
max_iter=30;
lampda=1;
while iter < max_iter & any(abs(fval+Mp) > tole) 
  % previous_norm=current_norm;
   iter=iter+1;
   if any(fval > -Mp+tole) % If the magnitude is too small decrease the filter time constants.
      tmp=find(fval > -Mp+tole);
      if strcmp(fun,'mimo1com')| strcmp(fun,'mimo2com')
         if previous_Epsilon{1}==Epsilon{1}
            Epsilon{1}(tmp)=0.5*Epsilon{1}(tmp);
         else
            Epsilon{1}(tmp)=(previous_Epsilon{1}(tmp)+Epsilon{1}(tmp))/2;
         end
      else
         if previous_Epsilon{2}==Epsilon{2}
            Epsilon{2}(tmp)=0.5*Epsilon{2}(tmp);
         else
            Epsilon{2}(tmp)=(previous_Epsilon{2}(tmp)+Epsilon{2}(tmp))/2;
         end
      end
      previous_Epsilon=Epsilon;
      
   elseif all(fval<-5) % Use min |e| S.T. magnitude < Mp to bring the peaks down
	   previous_Epsilon=Epsilon;
      if strcmp(fun,'mimo1com')| strcmp(fun,'mimo2com')
         Epsilon{1}=fmincon(enorm_fun,Epsilon{1}*10,[],[],[],[],Epsilon{1},[],enorm_con,option,...
            xwc,y,Mp',xwc(i,ln),ps,ms,qs,order,input_delay,pds,qds);
      else
         Epsilon{2}=fmincon(enorm_fun,Epsilon{2},[],[],[],[],Epsilon{2},[],enorm_con,option,...
            xwc,y,Mp',xwc(i,ln),ps,ms,qs,order,input_delay,pds,qds);
      end
      
   else
   % Calculate the Newton's step
   % ----------------------------
   % calculate the Jacobian
      del_F=jacob(Epsilon,fun,xwc,ps,ms,qs,order,input_delay,pds,qds,Mp,n,0.01);
      E_step=inv(del_F)*(fval+Mp)';
      if strcmp(fun,'mimo1com')| strcmp(fun,'mimo2com')
         Epsilon{1}=Epsilon{1}+lampda*E_step';
      else
         Epsilon{2}=Epsilon{2}+lampda*E_step';
      end
   end
   if strcmp(fun,'mimopartial')| strcmp(fun,'mimo2com')
      for i=1:n
         [qd{i,i},qdnum{i,i},qdden{i,i}]=qd_mat(pd_prime{i,i},mq{i},Epsilon{2}(i),order(i),y);
         qd{i,i}=strrep(qd{i,i},'e',['e(' num2str(i) ')']);
      end
      qds=cell2str(qd);
   end    
   for i=1:n
      vub(ln)=max([0.01 5*xwc(ln)]); % Moving the frequency limits while optimizing
      vlb(ln)=max([0.004 0.2*xwc(ln)]);
      xwctemp=[];temp=[];
   	[xwctemp(1,:),temp(1)]=fmincon(fun,xwc(i,:),[],[],[],[],vlb,vub,constr,option,...
      	[],Epsilon,-1,0,ps,ms,qs,order,i,i,input_delay,pds,qds);
   	vub(ln)= vub(ln)*10;
   	vlb(ln)= vlb(ln)*10;
   	[xwctemp(2,:),temp(2)]=fmincon(fun,vub,[],[],[],[],vlb,vub,constr,option,...
      	[],Epsilon,-1,0,ps,ms,qs,order,i,i,input_delay,pds,qds);
   	vub(ln)= vub(ln)*10;
   	vlb(ln)= vlb(ln)*10;
   	[xwctemp(3,:),temp(3)]=fmincon(fun,vlb,[],[],[],[],vlb,vub,constr,option,...
     		[],Epsilon,-1,0,ps,ms,qs,order,i,i,input_delay,pds,qds);
  		[fval(i),index]=min(temp);
  		xwc(i,:)=xwctemp(index,:);
   end
   if strcmp(fun,'mimo1com')| strcmp(fun,'mimo2com')
      disp([iter Epsilon{1}]);
   else
      disp([iter Epsilon{2}]);
   end
end
toc

% A sub function to print result
function print_result(X)
[m,n]=size(X);
for i=1:m
   fprintf('%g ',X(i,:));
   fprintf('\n');
end

function [J]=jacob(Epsilon,fun,x,ps,ms,qs,order,input_delay,pds,qds,Mp,n,dE)
% The nummerical pseudo-Jacobian of the magnitude of the complementary sensitivity function
% assumes that the worse case parameter x does not change with a small purterbation
% of the filter time constants, dE.
J=zeros(n,n);
for i=1:n
   fval=feval(fun,x(i,:),[],Epsilon,1,0,ps,ms,qs,order,i,i,input_delay,pds,qds);
   for j=1:n
      E=Epsilon;
      if strcmp(fun,'mimo1com')| strcmp(fun,'mimo2com')
         E{1}(j)=E{1}(j)+dE;
      elseif strcmp(fun,'mimopartial')
         E{2}(j)=E{2}(j)+dE;
      end
      temp=feval(fun,x(i,:),[],E,1,0,ps,ms,qs,order,i,i,input_delay,pds,qds);
      J(i,j)=(temp-fval)/dE;
   end
end


function [handle]=ct_plot(Epsilon,fun,x,ps,ms,qs,order,input_delay,pds,qds,Mp,n)
%(x,y,e,sign,w,ps,ms,qs,order,row,col,input_delay,pds,qds)
E=2*Epsilon;
X=0.001:E(1)/30:E(1);
Y=0.001:E(2)/30:E(2);
lx=length(X);
ly=length(Y);
[X,Y]=meshgrid(X,Y);
Z=zeros(lx,ly);
handle=figure;
hold on
for k=1:n
for i=1:lx
   for j=1:ly
      Z(i,j)=feval(fun,x(k,:),[],[X(j,i) Y(j,i)],1,0,ps,ms,qs,order,k,k,input_delay,pds,qds);
   end
end
contour(X,Y,Z,Mp);
end
set(handle,'Name',['Chart to slove for next Epsilon with current Epsilon = ', num2str(Epsilon)]);
hold on
plot(Epsilon(1),Epsilon(2),'r*');
title(['x(1,:) = ', lessblnk(num2str(x(1,:)))]);
xlabel('Epsilon(1)');
ylabel('Epsilon(2)');