home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 10.ddi / CONTROL.DI$ / RLOCUS.M < prev    next >
Encoding:
Text File  |  1993-03-11  |  8.9 KB  |  294 lines
  1. function [rout,k] = rlocus(a,b,c,d,e,f)
  2. %RLOCUS    Evans root locus.
  3. %    RLOCUS(NUM,DEN) calculates and plots the locus of the roots of:
  4. %                              NUM(s)
  5. %               H(s) = 1 + k ----------  =  0
  6. %                              DEN(s)
  7. %    for a set of gains K which are adaptively calculated to produce a 
  8. %    smooth plot. Alternatively the vector K can be specified with an 
  9. %    optional right-hand argument RLOCUS(NUM,DEN,K). Vectors NUM and 
  10. %    DEN must contain the numerator and denominator coefficients in 
  11. %    descending powers of s or z. When invoked with left hand arguments
  12. %        R = RLOCUS(NUM,DEN,K)  or  [R,K] = RLOCUS(NUM,DEN)
  13. %    returns the matrix R with LENGTH(K) rows and (LENGTH(DEN(-1) 
  14. %    columns containing the complex root locations.  Each row of the 
  15. %    matrix corresponds to a gain from vector K.  If a second left hand
  16. %    argument is included, the gains are returned in K.
  17. % 
  18. %    RLOCUS(A,B,C,D), R = RLOCUS(A,B,C,D,K), or [R,K] = RLOCUS(A,B,C,D)
  19. %    finds the root-locus from the equivalent SISO state-space system
  20. %    (A,B,C,D).
  21. %                    dx/dt = Ax + Bu    u = -k*y
  22. %                        y = Cx + Du
  23. %
  24. %    See also: PZMAP and RLOCFIND.
  25.  
  26. %     J.N. Little 10-11-85
  27. %    Revised A.C.W.Grace 7-8-89, 6-21-92 
  28. %    Copyright (c) 1986-93 by the MathWorks, Inc.
  29.  
  30. if nargin==0, eval('exresp(''rlocus'');'), return, end
  31.  
  32. error(nargchk(2,6,nargin));
  33.  
  34. % The next variable controls how many points are plotted on the graph.
  35. precision = 1; %Set to a higher number (e.g. 2 for more points on the graph).
  36. k=[];
  37.  
  38. if (nargin==3 | nargin==2 )     % Convert to state space
  39.     if nargin==3, k = c; end
  40.     [num,den] = tfchk(a,b);
  41.     [a,b,c,d] = tf2ss(a,b);
  42. end
  43.  
  44. [ny,nu] = size(d);
  45. if (ny*nu==0)|isempty(a), k=[]; if nargout~=0, rout=[]; end, return, end
  46.  
  47. %  Multivariable systems
  48. error(abcdchk(a,b,c,d));
  49. if nargin==5,
  50.   k=e; 
  51. elseif nargin==6,
  52.   b=b(:,e); d=d(:,e); k = f;
  53. end
  54.  
  55. % Trap MIMO case
  56. [ny,nu] = size(d);
  57. if ny*nu~=1,
  58.   if nargout~=0,
  59.     disp('Warning: Root locus with outputs must be SISO.'), 
  60.     return,
  61.   end
  62.   [e,nargin,r]=mulresp('rlocus',a,b,c,d,k,0,0);
  63.   if ~e, if nargout, rout = r; end, return, end
  64. end
  65.  
  66. sp=sum([1:length(a)].^2);
  67.  
  68. % Set up graphics and other parameters.
  69.  
  70. if isempty(k) | nargout==0
  71. % Work out scales and ranges
  72.     kgiven = 0;
  73.     ep=eig(a);
  74.     p=length(ep);
  75.     if nargin>=4
  76.         tz=tzero(a,b,c,d);
  77.         z=length(tz);
  78.         sp2=[1:z].^2;
  79.         den=poly(ep);
  80.         num=poly(tz);
  81.     else
  82.         tz=roots(num);
  83.         z=length(tz);
  84.     end
  85.     den2 = den + 1e-20*(den==0); 
  86.     dcgain=num(length(num))/den2(length(den));
  87. % Normalize for better numerical properties
  88.     if abs(den(1)) > eps & den(1) ~= 1
  89.         den=den/den(1);
  90.         num=num/den(1);
  91.     end
  92.     mep=max([eps;abs([real(ep);real(tz);imag(ep);imag(tz)])]);
  93.     if z==p
  94. % Round up axis to units to the nearest 5
  95.         ax=1.2*mep;        
  96.     else
  97.         % Round graph axis    
  98.         exponent=5*10^(floor(log10(mep))-1);
  99.         ax=2*round(mep/exponent)*exponent;    
  100.     end
  101.     if nargout==0 
  102. % Real and imaginary axis - uncomment these if you don't want them
  103.         axstate = axis('state');
  104.         holdon = ishold;
  105.         newplot;
  106.         if ~holdon
  107.             plot([-ax,ax],[0,0],'w:',[0,0],[-ax,ax],'w:');
  108.             axis([-ax,ax,-ax,ax])
  109.         else
  110.             ax4 = axis;
  111.             ax = sum(abs(ax4))/4;
  112.             plot([ax4(1:2)],[0,0],'w:',[0,0],[ax4(3:4)],'w:');
  113.             axis(ax4)
  114.         end
  115. % If plotting is required then set up axis and titles
  116.         hold on
  117.         plot(real(ep),imag(ep),'x');
  118.         if ~isempty(tz)
  119.             plot(real(tz),imag(tz),'o');
  120.         end
  121.         xlabel('Real Axis')
  122.         ylabel('Imag Axis')
  123.         % Use none as Erasemode for fast plotting
  124.         erasemode = 'none';
  125.         drawnow
  126.     end
  127. end
  128.  
  129. % Adaptively search for values of gain  if K is not specified
  130.  
  131. if isempty(k)
  132. % Set up initial gain based on D.C.Gain of open loop and 
  133. % positions of zeros and poles.
  134. % Since closed loop den = num + k*den, sensitivity is related
  135. % to difference between num and denominator coefficients.
  136.     diff=abs(num)./abs(den2(1:length(num)));
  137.     kinit=0.01/(abs(dcgain) + polyval(diff,4))+1e-12;
  138.     bc=b*c;
  139.     k=[0,1e-4*kinit,kinit]; dist=kinit;
  140.     r(:,1)=ep;
  141.     r(:,2)=vsort(ep, eig(a-bc*(k(2)./(1+k(2)*d))), sp);
  142.     if nargout==0, plot( real([ep,r(:,2)])', imag([ep,r(:,2)])','-' ,'Erasemode', erasemode), end
  143.     i=2; ij=sqrt(-1); perr=1;
  144.     terminate=0;     
  145.     while  ~terminate
  146.         i=i+1;
  147.         ki=k(i)./(1+k(i)*d);
  148.         r1=eig(a-bc*ki);
  149.         mx=max(abs([real(r1).';imag(r1).']));
  150.         % If any line outside box then set erasemode back to normal
  151.         % for clipping
  152.         if any(mx > ax), erasemode = 'normal'; end
  153. % Sort out eigenvalues so that plotting appears continuous. 
  154.         [r(:,i),pind]=vsort(r(:,i-1),r1,sp);
  155. % Adjust values of k based on linearity of the roots:
  156.         % First two points in line
  157.         y1=imag(r(:,i-2)); y2=imag(r(:,i-1));
  158.         x1=real(r(:,i-2)); x2=real(r(:,i-1));
  159.         % Current  points
  160.         y=imag(r(:,i)); x=real(r(:,i));
  161.         % Nearest x-y co-ordinates of new point to line
  162.         [newx,newy]=perpxy(x1,y1,x2,y2,x,y);
  163.         % Error estimation
  164.         err=sqrt((newy-y).^2+(newx-x).^2);
  165.         distm=norm((y-y2)+ij*(x-x2));
  166.         fferr=find(~finite(err));
  167.         err(fferr)=zeros(length(fferr),1);
  168.         perr=precision*max(err(find(finite(err))))/(distm+ax/(1e3*(distm+eps))); 
  169.  
  170. % If percentage error greater than threshold then go back and 
  171. % re-evaluate more roots:
  172.         pind = any((y==0)~=(y2==0));
  173.         if perr>0.2 | pind
  174.             % Decrement distance between gains
  175.             npts=3+5*round(min([5,perr*3]))+17*pind;
  176.             kval=logspace(log10(k(i-1)),log10(k(i)),npts);
  177.             dist=(k(i)-k(i-1))/(npts-7*pind);
  178.             i=i-1;
  179.             for kcnt=kval(2:npts)
  180.                 i=i+1;
  181.                 k(i)=kcnt;
  182.                 ki=kcnt./(1+kcnt*d);
  183.                 r1=eig(a-bc*ki);
  184.                 r(:,i) = vsort(r(:,i-1),r1,sp);
  185.                 y=imag(r(:,i)); y2=imag(r(:,i-1));
  186.                 x=real(r(:,i)); x2=real(r(:,i-1));
  187.                 ind = 0; 
  188.                 if nargout==0  
  189. % Intersection of rlocus on the  real axis.
  190.                     if pind   
  191. % The inequality abs(y+y2)<ax makes sure that its not one that 
  192. % goes off to inf and then 
  193. % comes back on the real axis. 
  194.                         ind = find((y==0)~=(y2==0) & abs(y+y2)<ax);
  195.                         if ~isempty(ind)
  196.                             if (imag(r(ind(1),i))==0), cix = 1; else, cix = 0; end
  197.                             realr = r(:,i-cix);
  198.                             realr(ind)=real(r(ind,i-cix));
  199.                             plot( real([realr,r(:,i-1)])', imag([realr,r(:,i-1)])','-','Erasemode',erasemode)
  200.                             plot( real([r(:,i),realr])', imag([r(:,i),realr])','-' ,'Erasemode',erasemode)
  201.                         else
  202.                             ind=0;
  203.                         end
  204.                     end
  205.                     if ~ind
  206. % Fix for plot when rlocus goes from -inf to inf or -inf to inf
  207.                         infind = find(abs(x)>ax & sign(x2)~=sign(x));
  208.                         if length(infind)>0
  209.                             x(infind) = sign(x2(infind))*ax;
  210.                         end 
  211.                         plot([x,x2]',[y,y2]','-','Erasemode',erasemode)
  212.                     end
  213.  
  214.                 end
  215.             end
  216.         else
  217. % Fix for plot when rlocus goes comes from -inf to inf or -inf to inf
  218.             infind = find(abs(x)>ax & sign(x2)~=sign(x) );
  219.             if length(infind)>0
  220.                 x(infind) = sign(x2(infind))*ax;
  221.             end 
  222.             % Use none as Erasemode for fast plotting
  223.             if nargout==0, plot([x,x2]',[y,y2]','-','Erasemode',erasemode), end
  224.             % Increase/decrease distance to next k based on linearity estimate:
  225.             dist=dist*(0.3/precision+exp(1-15*perr));
  226.         end
  227.         % Next gain value
  228.         k(i+1)=k(i)+dist;
  229. % Termination criterion
  230.         terminate=1;
  231.         if z==p
  232.             tz = vsort(r1, tz);
  233.             terminate=max(abs(tz-r1))<ax/100;
  234.         else 
  235.             % Make sure all loci tending to infinity are out of graph before terminating
  236.             % Terminate when all poles have approached their corresponding  zeros
  237.             % Note: this may not work well if zeros are close together
  238.             if z > 0
  239.                 terminate=max(min(abs(r1*ones(1,z)-ones(p,1)*tz.')))<ax/100;
  240.             end
  241.             mx=max(abs([real(r1).';imag(r1).']));
  242.             terminate = ( sum(mx>1.2*ax)>=p-z & terminate );
  243.         end
  244.         terminate = terminate | abs(k(i)) > 1e30;
  245.         if nargout == 0
  246.             if rem(i,10) == 0 % Draw graph every ten iterations
  247.                 drawnow
  248.             end
  249.         end
  250.     end
  251.  
  252. else
  253. % When K is given:
  254.     kgiven = 1;
  255.     [ns,nu] = size(b);
  256.     nk = length(k); i=1;
  257.     r  = sqrt(-1) * ones(ns,nk);
  258.     bc = b*c; 
  259. % Find eigenvalues of:  A - B*inv(I+k*D)*k*C:
  260.     k2 = k./(1+k.*d);
  261.     r(:,1)=eig(a-bc*k2(1)); k2(1)=[];
  262.     if nk == 1 & nargout == 0
  263.         % Special case when only one point
  264.         plot(real(r(:,1))', imag(r(:,1)), '+');
  265.     end
  266.     for ki=k2
  267.         i = i + 1;
  268.         r1 = eig(a-bc*ki);
  269. % Sort eigenvalues
  270.         r(:,i)=vsort(r(:,i-1),r1,sp);
  271.         if nargout==0
  272.             % Plot in pairs to get colored crosses
  273.             plot( real([r(:,i),r(:,i-1)])', imag([r(:,i),r(:,i-1)])','+','Erasemode','none')
  274.         end
  275.     end
  276. end
  277. r = r.';
  278.  
  279. if nargout==0, 
  280. % Uncomment the next line to obtain grid on plot
  281.     %grid
  282.     % Return graphics to original state
  283.     if ~holdon, hold off, end
  284.     % axis(axstate);
  285.     return % Suppress output
  286. end
  287.  
  288. rout = r;
  289. % Last element of k is never used
  290. if ~kgiven 
  291.     k(length(k))=[];
  292.     k = k.';
  293. end
  294.