octave_packages/control-2.3.52/rlocus.m

   1 ## Copyright (C) 1996, 2000, 2004, 2005, 2006, 2007
   2 ##               Auburn University. All rights reserved.
   3 ##
   4 ##
   5 ## This program is free software; you can redistribute it and/or modify it
   6 ## under the terms of the GNU General Public License as published by
   7 ## the Free Software Foundation; either version 3 of the License, or (at
   8 ## your option) any later version.
   9 ##
  10 ## This program is distributed in the hope that it will be useful, but
  11 ## WITHOUT ANY WARRANTY; without even the implied warranty of
  12 ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13 ## General Public License for more details.
  14 ##
  15 ## You should have received a copy of the GNU General Public License
  16 ## along with this program; see the file COPYING.  If not, see
  17 ## <http://www.gnu.org/licenses/>.
  18
  19 ## -*- texinfo -*-
  20 ## @deftypefn {Function File} rlocus (@var{sys})
  21 ## @deftypefnx {Function File} {[@var{rldata}, @var{k}] =} rlocus (@var{sys}, @var{increment}, @var{min_k}, @var{max_k})
  22 ## Display root locus plot of the specified @acronym{SISO} system.
  23 ##
  24 ## @strong{Inputs}
  25 ## @table @var
  26 ## @item sys
  27 ## LTI model.  Must be a single-input and single-output (SISO) system.
  28 ## @item min_k
  29 ## Minimum value of @var{k}.
  30 ## @item max_k
  31 ## Maximum value of @var{k}.
  32 ## @item increment
  33 ## The increment used in computing gain values.
  34 ## @end table
  35 ##
  36 ## @strong{Outputs}
  37 ## @table @var
  38 ## @item rldata
  39 ## Data points plotted: in column 1 real values, in column 2 the imaginary values.
  40 ## @item k
  41 ## Gains for real axis break points.
  42 ## @end table
  43 ##
  44 ## @strong{Block Diagram}
  45 ## @example
  46 ## @group
  47 ##  u    +         +---+      +------+             y
  48 ## ------>(+)----->| k |----->| SISO |-------+------->
  49 ##         ^ -     +---+      +------+       |
  50 ##         |                                 |
  51 ##         +---------------------------------+
  52 ## @end group
  53 ## @end example
  54 ## @end deftypefn
  55
  56 ## Author: David Clem
  57 ## Author: R. Bruce Tenison <btenison@eng.auburn.edu>
  58 ## Updated by Kristi McGowan July 1996 for intelligent gain selection
  59 ## Updated by John Ingram July 1996 for systems
  60
  61 ## Adapted-By: Lukas Reichlin <lukas.reichlin@gmail.com>
  62 ## Date: December 2009
  63 ## Version: 0.4
  64
  65 ## TODO: Improve compatibility
  66
  67 function [rldata_r, k_break, rlpol, gvec, real_ax_pts] = rlocus (sys, increment, min_k, max_k)
  68
  69   ## TODO: multiplot feature:   rlocus (sys1, "b", sys2, "r", ...)
  70
  71   if (nargin < 1 || nargin > 4)
  72     print_usage ();
  73   endif
  74
  75   if (! isa (sys, "lti") || ! issiso (sys))
  76     error ("rlocus: first argument must be a SISO LTI model");
  77   endif
  78
  79   ## Convert the input to a transfer function if necessary
  80   [num, den] = tfdata (sys, "vector");     # extract numerator/denominator polynomials
  81   lnum = length (num);
  82   lden = length (den);
  83   ## equalize length of num, den polynomials
  84   ## TODO: handle case lnum > lden (non-proper models)
  85   if (lden < 2)
  86     error ("rlocus: system has no poles");
  87   elseif (lnum < lden)
  88     num = [zeros(1,lden-lnum), num];       # so that derivative is shortened by one
  89   endif
  90
  91   olpol = roots (den);
  92   olzer = roots (num);
  93   nas = lden - lnum;                       # number of asymptotes
  94   maxk = 0;
  95   if (nas > 0)
  96     cas = (sum (olpol) - sum (olzer)) / nas;
  97     angles = (2*[1:nas]-1)*pi/nas;
  98     ## printf("rlocus: there are %d asymptotes centered at %f\n", nas, cas);
  99   else
 100     cas = angles = [];
 101     maxk = 100*den(1)/num(1);
 102   endif
 103
 104
 105   ## compute real axis break points and corresponding gains
 106   dnum = polyder (num);
 107   dden = polyder (den);
 108   brkp = conv (den, dnum) - conv (num, dden);
 109   real_ax_pts = roots (brkp);
 110   real_ax_pts = real_ax_pts(find (imag (real_ax_pts) == 0));
 111   k_break = -polyval (den, real_ax_pts) ./ polyval (num, real_ax_pts);
 112   idx = find (k_break >= 0);
 113   k_break = k_break(idx);
 114   real_ax_pts = real_ax_pts(idx);
 115   if (! isempty (k_break))
 116     maxk = max (max (k_break), maxk);
 117   endif
 118
 119   if (nas == 0)
 120     maxk = max (1, 2*maxk);                # get at least some root locus
 121   else
 122     ## get distance from breakpoints, poles, and zeros to center of asymptotes
 123     dmax = 3*max (abs ([vec(olzer); vec(olpol); vec(real_ax_pts)] - cas));
 124     if (dmax == 0)
 125       dmax = 1;
 126     endif
 127
 128     ## get gain for dmax along each asymptote, adjust maxk if necessary
 129     svals = cas + dmax * exp (j*angles);
 130     kvals = -polyval (den, svals) ./ polyval (num, svals);
 131     maxk = max (maxk, max (real (kvals)));
 132   endif
 133
 134   ## check for input arguments:
 135   if (nargin > 2)
 136     mink = min_k;
 137   else
 138     mink = 0;
 139   endif
 140   if (nargin > 3)
 141     maxk = max_k;
 142   endif
 143   if (nargin > 1)
 144     if (increment <= 0)
 145       error ("rlocus: increment must be positive");
 146     else
 147       ngain = (maxk-mink)/increment;
 148     endif
 149   else
 150     ngain = 30;
 151   endif
 152
 153   ## vector of gains
 154   ngain = max (30, ngain);
 155   gvec = linspace (mink, maxk, ngain);
 156   if (length (k_break))
 157     gvec = sort ([gvec, reshape(k_break, 1, [])]);
 158   endif
 159
 160   ## Find the open loop zeros and the initial poles
 161   rlzer = roots (num);
 162
 163   ## update num to be the same length as den
 164   lnum = length (num);
 165   if (lnum < lden)
 166     num = [zeros(1,lden - lnum),num];
 167   endif
 168
 169   ## compute preliminary pole sets
 170   nroots = lden - 1;
 171   for ii = 1:ngain
 172    gain = gvec(ii);
 173    rlpol(1:nroots,ii) = vec(sort (roots (den + gain*num)));
 174   endfor
 175
 176   ## set smoothing tolerance
 177   smtolx = 0.01*(max (max (real (rlpol))) - min (min (real (rlpol))));
 178   smtoly = 0.01*(max (max (imag (rlpol))) - min (min (imag (rlpol))));
 179   smtol = max (smtolx, smtoly);
 180   ## sort according to nearest-neighbor
 181   rlpol = sort_roots (rlpol, smtolx, smtoly);
 182
 183   done = (nargin == 4);                    # perform a smoothness check
 184   while (! done && ngain < 1000)
 185     done = 1 ;                             # assume done
 186     dp = abs (diff (rlpol.')).';
 187     maxdp = max (dp);
 188
 189     ## search for poles whose neighbors are distant
 190     if (lden == 2)
 191       idx = find (dp > smtol);
 192     else
 193       idx = find (maxdp > smtol);
 194     endif
 195
 196     for ii = 1:length(idx)
 197       i1 = idx(ii);
 198       g1 = gvec(i1);
 199       p1 = rlpol(:,i1);
 200
 201       i2 = idx(ii)+1;
 202       g2 = gvec(i2);
 203       p2 = rlpol(:,i2);
 204
 205       ## isolate poles in p1, p2
 206       if (max (abs (p2-p1)) > smtol)
 207         newg = linspace (g1, g2, 5);
 208         newg = newg(2:4);
 209         gvec = [gvec,newg];
 210         done = 0;                          # need to process new gains
 211       endif
 212     endfor
 213
 214     ## process new gain values
 215     ngain1 = length (gvec);
 216     for ii = (ngain+1):ngain1
 217       gain = gvec(ii);
 218       rlpol(1:nroots,ii) = vec(sort (roots (den + gain*num)));
 219     endfor
 220
 221     [gvec, idx] = sort (gvec);
 222     rlpol = rlpol(:,idx);
 223     ngain = length (gvec);
 224     ## sort according to nearest-neighbor
 225     rlpol = sort_roots (rlpol, smtolx, smtoly);
 226   endwhile
 227   rldata = rlpol;
 228
 229   ## Plot the data
 230   if (nargout  == 0)
 231     rlpolv = vec(rlpol);
 232     axdata = [real(rlpolv), imag(rlpolv); real(olzer), imag(olzer)];
 233     axlim = __axis_limits__ (axdata);
 234     rldata = [real(rlpolv), imag(rlpolv) ];
 235
 236     %inname = get (sys, "inname");
 237     %outname = get (sys, "outname");
 238
 239     ## build plot command args pole by pole
 240
 241     n_rlpol = rows (rlpol);
 242     nelts = n_rlpol+1;
 243     if (! isempty (rlzer))
 244       nelts++;
 245     endif
 246     ## add asymptotes
 247     n_A = length (olpol) - length (olzer);
 248     if (n_A > 0)
 249       nelts += n_A;
 250     endif
 251     args = cell (3, nelts);
 252     kk = 0;
 253     ## asymptotes first
 254     if (n_A > 0)
 255       len_A = 2*max (abs (axlim));
 256       sigma_A = (sum(olpol) - sum(olzer))/n_A;
 257       for i_A=0:n_A-1
 258         phi_A = pi*(2*i_A + 1)/n_A;
 259         args{1,++kk} = [sigma_A sigma_A+len_A*cos(phi_A)];
 260         args{2,kk} = [0 len_A*sin(phi_A)];
 261         if (i_A == 1)
 262           args{3,kk} = "k--;asymptotes;";
 263         else
 264           args{3,kk} = "k--";
 265         endif
 266       endfor
 267     endif
 268     ## locus next
 269     for ii = 1:rows(rlpol)
 270       args{1,++kk} = real (rlpol (ii,:));
 271       args{2,kk} = imag (rlpol (ii,:));
 272       if (ii == 1)
 273         args{3,kk} = "b-;locus;";
 274       else
 275         args{3,kk} = "b-";
 276       endif
 277     endfor
 278     ## poles and zeros last
 279     args{1,++kk} = real (olpol);
 280     args{2,kk} = imag (olpol);
 281     args{3,kk} = "rx;open loop poles;";
 282     if (! isempty (rlzer))
 283       args{1,++kk} = real (rlzer);
 284       args{2,kk} = imag (rlzer);
 285       args{3,kk} = "go;zeros;";
 286     endif
 287
 288     set (gcf,"visible","off");
 289     hplt = plot (args{:});
 290     set (hplt(kk--), "markersize", 2);
 291     if (! isempty (rlzer))
 292       set (hplt(kk--), "markersize", 2);
 293     endif
 294     for ii = 1:rows(rlpol)
 295       set (hplt(kk--), "linewidth", 2);
 296     endfor
 297     legend ("boxon", 2);
 298     grid ("on");
 299     axis (axlim);
 300     title (["Root Locus of ", inputname(1)]);
 301     xlabel (sprintf ("Real Axis     gain = [%g, %g]", gvec(1), gvec(ngain)));
 302     ylabel ("Imaginary Axis");
 303     set (gcf (), "visible", "on");
 304   else
 305     rldata_r = rldata;
 306   endif
 307 endfunction
 308
 309
 310 function rlpol = sort_roots (rlpol, tolx, toly)
 311   ## no point sorting of you've only got one pole!
 312   if (rows (rlpol) == 1)
 313     return;
 314   endif
 315
 316   ## reorder entries in each column of rlpol to be by their nearest-neighbors rlpol
 317   dp = diff (rlpol.').';
 318   drp = max (real (dp));
 319   dip = max (imag (dp));
 320   idx = find (drp > tolx | dip > toly);
 321   if (isempty (idx))
 322     return;
 323   endif
 324
 325   [np, ng] = size (rlpol);                 # num poles, num gains
 326   for jj = idx
 327     vals = rlpol(:,[jj,jj+1]);
 328     jdx = (jj+1):ng;
 329     for ii = 1:rows(rlpol-1)
 330       rdx = ii:np;
 331       dval = abs (rlpol(rdx,jj+1)-rlpol(ii,jj));
 332       mindist = min (dval);
 333       sidx = min (find (dval == mindist)) + ii - 1;
 334       if (sidx != ii)
 335         c1 = norm (diff(vals.'));
 336         [vals(ii,2), vals(sidx,2)] = swap (vals(ii,2), vals(sidx,2));
 337         c2 = norm (diff (vals.'));
 338         if (c1 > c2)
 339           ## perform the swap
 340           [rlpol(ii,jdx), rlpol(sidx,jdx)] = swap (rlpol(ii,jdx), rlpol(sidx,jdx));
 341           vals = rlpol(:,[jj,jj+1]);
 342         endif
 343       endif
 344     endfor
 345   endfor
 346
 347 endfunction
 348
 349
 350 function [b, a] = swap (a, b)
 351
 352 endfunction