msfgainm.m

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

function [Ksp,K,Kdelay,Kspstr,Kstr,invDs,invDstr]=msfgainM(q,order,e,qdeadmt,qnummt,qdenmt,y)
% Find Ksp and Ks for the MSF controller
%[input_delay,dec_M,qdeadmt]=dtarray(Xmdeadmt,y);
% -------------------------------------------------------------
% Treat the internal calculation as a diagonal decoupling.
% (This could be confused, if the user input the model that need column swapping
% to rearrange the system to have smallest delay in the main diagonal.)
% ----------------------------------------------------------------
[temp,qdeadmt]=transform(qdeadmt);
%qnummt=sw_col(qnummt,T);
%qdenmt=sw_col(qdenmt,T);
%order=sw_col(order,T);
%q=sw_col(q,T);
%e=sw_col(e,T);
% ----------------------------------
n=length(order);
invDs={}; % The variable to store D(s)
invDstr={};
for i=1:n
   invDs{i}=lcmpoly(qdenmt(:,i),y); % D(s) = least common multiplier of ...
   invDstr{i}=['1/' mtx2str(invDs{i},1)];
end
Ns=cell(n,n); % Polynomial part of N(s)
for i=1:n
   for j=1:n
      Ns{i,j}=conv(mt2poly(qnummt{i,j},y),deconv(invDs{j},mt2poly(qdenmt{i,j},y)));
   end
end
invDs=diag(invDs);
invDstr=diag(invDstr);
%-----------------------
% Calculate Ksp = q(inf)
%-----------------------
s=1e7; % set s to a big number
fill_str='1/(e(i)*s+1)';
filter=zeros(1,n);
for i=1:n
   filter(i)=eval(fill_str)^order(i);
end
filter=diag(filter);
qs=cell2str(q);
Ksp=inv(eval(qs))*filter; % Ksp final result

%-------------------------------------------------------------------
% Calculate K(s), its string representation, and form the Ksp string
%-------------------------------------------------------------------
Kspstr=cell(n,n); % Initialize variables
Kstr=Kspstr;
K=Kstr;
Bs=K;
filter=cell(1,n); % Calculate invert of the filter
for i=1:n
   filter{i}=1;
   for k=1:order(i)
      filter{i}=conv(filter{i},[e(i) 1]);
   end
end
% Form B*(s) = Ksp*F^(-1)(s)
for i=1:n
   for j=1:n
      Bs{i,j}=filter{j}*Ksp(i,j);
      Kspstr{i,j}=num2str(Ksp(i,j)); % finallize the string representation of Ksp
   end
end
%---------------------------------------------------------------------------
% Loops to calculate K(s):
% K(s) is represented by a 3-Dimensions cell array.
% The first two dimemsion represent combinations I/O element of K(s)
% The 3rd dimension represent the polynomial of each delays term.
% For example, Ks{1,2} = (s+1)exp(-s)+(3s+1)exp(-2s) will be represent by
% Ks(1,2)={[1 1] [3 1]} and Kdelay(1,2)={1 2} expanding in the 3rd dimension.
%---------------------------------------------------------------------------
depth=ones(n,n); % Initialize the depth in the 3rd dimension.
for i=1:n 
   for j=1:n
      K{i,j,1}=0;
      Kdelay(i,j,1)=0;
      for k=1:n
         if Ksp(i,k)~=0
            if any((qdeadmt(k,j)==Kdelay(i,j,:)))
               temp1=min(find(qdeadmt(k,j)==Kdelay(i,j,:))); % find the depth
               if isempty(qnummt{k,j}) | qnummt{k,j}==0
                  temp2=0;
               else
                  temp2=conv(qnummt{k,j},deconv(invDs{j,j},qdenmt{k,j}));
               end
               K{i,j,temp1}=polyadd(K{i,j,temp1},conv(Bs{i,k},...
                  temp2));
            else
               if (length(K{i,j,1})>1 | K{i,j,1}~=0)  
                  depth(i,j)=depth(i,j)+1;
               end
               if isempty(qnummt{k,j}) | qnummt{k,j}==0
                  temp2=0;
               else
                  temp2=conv(qnummt{k,j},deconv(invDs{j,j},qdenmt{k,j}));
               end
               K{i,j,depth(i,j)}=conv(Bs{i,k},temp2);
               Kdelay(i,j,depth(i,j))=qdeadmt(k,j);
            end
         end
      end
      if i==j
         temp2=-invDs{i,j};
         temp=(0==Kdelay(i,j,:));
         if any(temp)
            temp1=min(find(temp));
            K{i,j,temp1}=polyadd(K{i,j,temp1},temp2);
         else
            if (length(K{i,j,1})>1 | K{i,j,1}~=0)
               depth(i,j)=depth(i,j)+1;
            end
            K{i,j,depth(i,j)}=temp2;
            Kdelay(i,j,depth(i,j))=0;
         end
      end
   end
end
for i=1:n % get rid of the very small fist terms.
   for j=1:n
      for k=1:n
         if k<=depth(i,j)
            while length(K{i,j,k})>1 & abs(K{i,j,k}(1)) < 1e-4 
               K{i,j,k}(1)=[];
            end
         else
            K{i,j,k}=0;
            Kdelay(i,j,k)=0;
         end
      end
   end
end
% Generate K(s) string representation
[n,m,l]=size(K);
for i=1:n
   for j=1:n
      temp=1;
      while temp <= l & K{i,j,temp}~=0
         if ~isempty(Kstr{i,j})
            Kstr{i,j}=[Kstr{i,j} '+' mtx2str(K{i,j,temp},1)];
         else
            Kstr{i,j}=[mtx2str(K{i,j,temp},1)];
         end
         if Kdelay(i,j,temp)~=0
            Kstr{i,j}=[Kstr{i,j} '*' mtx2str(Kdelay(i,j,temp),3)];
         end
         temp=temp+1;
      end
   end
end
            
%Ksp=(sw_col(Ksp',T'))';
%Kspstr=(sw_col(Kspstr',inv(T)))';
%K=(sw_col(K',T'))';
%Kstr=(sw_col(Kstr',inv(T)))';
%invDs=sw_col(invDs,inv(T));
%invDstr=sw_col(invDstr,inv(T));
invDs=diag(invDs);