qd_mat.m

Software for design and tuninig of SISO and MIMO contol systems

function [qd,qdnum,qdden] = qd_mat(T,mq,e,order,y)
%[qd,qdnum,qdden] = qd_mat(T,m,q,e,order,y)
%This function compute qd to make (1-mqqd) cancel both stable and unstable poles
%of transfer function pd. (See Ref.)
% Where:
%	T 	= 	a polynomial vector representing disturbance lag
%
%	mq	=	model * controller generated by mq_gen.m
%			
%	e	=	filter time constant
%
%	y	=	spared variable (in case of uncertain model) 
%			used only for searching the best model (y always =[])
%	order = order of the filter
%
% Note : This function must be used together with the mq_gen function.

qd='1';
qdnum=1;
qdden=1;
poles=roots(T);
poles=sort(poles); % Sort the poles.
poles=poles(find(abs(poles+1/e)> 1e-4)); % remove poles that close to the epsilon.
if isempty(mq{1}) | length(poles) == 0
 return
end
n=length(poles);
A=zeros(n,n);
b=zeros(n,1);
repeat=1;
for i=1:n
   s=poles(i);
   if i>1 & poles(i)==poles(i-1)
      repeat=repeat+1;
   else
      repeat=1; %reset
   end
   if poles(i)==0 & repeat ==1
      repeat=repeat+1;
   end
   Cs=eval(mq{1})/(e*s+1)^(order+n);
   switch repeat
   case 1
      b(i)=1-Cs;
      for j=1:n
         A(i,j)=Cs*s^(n+1-j);
      end
   case 2
      Cs_prime=eval(mq{2})/(e*s+1)^(order+n)-eval(mq{1})*e*(order+n)/(e*s+1)^(order+n+1);
      b(i)=-Cs_prime;
      for j=1:n
         A(i,j)=Cs_prime*s^(n+1-j)+Cs*(n+1-j)*s^(n-j);
      end
   case 3
      tmp=eval(mq{2});
      tmp1=eval(mq{1});
      tmp2=order+n;
      tmp3=e*s+1;
      Cs_prime=tmp/tmp3^(tmp2)-tmp1*e*(tmp2)/tmp3^(tmp2+1);
      Cs_2prime=eval(mq{3})/tmp3^(tmp2)-2*e*(tmp2)*tmp/tmp3^(tmp2+1);
      Cs_2prime=Cs_2prime+e^2*tmp2*(tmp2+1)*tmp1/tmp3^(tmp2+2);
      b(i)=-Cs_2prime;
      for j=1:n
         if j==n
            A(i,j)=Cs_2prime*s^(n+1-j)+2*Cs_prime*(n+1-j)*s^(n-j);
         else
            A(i,j)=Cs_2prime*s^(n+1-j)+2*Cs_prime*(n+1-j)*s^(n-j)+...
               Cs*(n+1-j)*(n-j)*s^(n-j-1);
         end
      end
   end
end
Alpha=real(inv(A)*b);
%Alpha=inv(A)*b  % This line is only for testing to see if imaginary parts are really small.
qd='(';
qdden=[1];
for k=1:n
   if k==n
      qd=[qd num2str(Alpha(k)) '*s+'];
   else
      qd=[qd num2str(Alpha(k)) '*s^' num2str(n+1-k) '+'];
   end
   qdden=conv(qdden,[e 1]);
end
if n==1
   qd=[qd '1)/(e*s+1)'];
else
   qd=[qd '1)/(e*s+1)^' num2str(n)];
end
qdnum=[Alpha' 1];
s=sqrt(-1);
if angle(eval(qd))< 0
   qd='1';
   qdnum=[1];
   qdden=[1];
end