1 ## Copyright (C) 1996, 2000, 2004, 2005, 2006, 2007
2 ## Auburn University. All rights reserved.
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.
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.
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/>.
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.
27 ## LTI model. Must be a single-input and single-output (SISO) system.
29 ## Minimum value of @var{k}.
31 ## Maximum value of @var{k}.
33 ## The increment used in computing gain values.
39 ## Data points plotted: in column 1 real values, in column 2 the imaginary values.
41 ## Gains for real axis break points.
44 ## @strong{Block Diagram}
47 ## u + +---+ +------+ y
48 ## ------>(+)----->| k |----->| SISO |-------+------->
49 ## ^ - +---+ +------+ |
51 ## +---------------------------------+
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
61 ## Adapted-By: Lukas Reichlin <lukas.reichlin@gmail.com>
62 ## Date: December 2009
65 ## TODO: Improve compatibility
67 function [rldata_r, k_break, rlpol, gvec, real_ax_pts] = rlocus (sys, increment, min_k, max_k)
69 ## TODO: multiplot feature: rlocus (sys1, "b", sys2, "r", ...)
71 if (nargin < 1 || nargin > 4)
75 if (! isa (sys, "lti") || ! issiso (sys))
76 error ("rlocus: first argument must be a SISO LTI model");
79 ## Convert the input to a transfer function if necessary
80 [num, den] = tfdata (sys, "vector"); # extract numerator/denominator polynomials
83 ## equalize length of num, den polynomials
84 ## TODO: handle case lnum > lden (non-proper models)
86 error ("rlocus: system has no poles");
88 num = [zeros(1,lden-lnum), num]; # so that derivative is shortened by one
93 nas = lden - lnum; # number of asymptotes
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);
101 maxk = 100*den(1)/num(1);
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);
120 maxk = max (1, 2*maxk); # get at least some root locus
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));
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)));
134 ## check for input arguments:
145 error ("rlocus: increment must be positive");
147 ngain = (maxk-mink)/increment;
154 ngain = max (30, ngain);
155 gvec = linspace (mink, maxk, ngain);
156 if (length (k_break))
157 gvec = sort ([gvec, reshape(k_break, 1, [])]);
160 ## Find the open loop zeros and the initial poles
163 ## update num to be the same length as den
166 num = [zeros(1,lden - lnum),num];
169 ## compute preliminary pole sets
173 rlpol(1:nroots,ii) = vec(sort (roots (den + gain*num)));
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);
183 done = (nargin == 4); # perform a smoothness check
184 while (! done && ngain < 1000)
185 done = 1 ; # assume done
186 dp = abs (diff (rlpol.')).';
189 ## search for poles whose neighbors are distant
191 idx = find (dp > smtol);
193 idx = find (maxdp > smtol);
196 for ii = 1:length(idx)
205 ## isolate poles in p1, p2
206 if (max (abs (p2-p1)) > smtol)
207 newg = linspace (g1, g2, 5);
210 done = 0; # need to process new gains
214 ## process new gain values
215 ngain1 = length (gvec);
216 for ii = (ngain+1):ngain1
218 rlpol(1:nroots,ii) = vec(sort (roots (den + gain*num)));
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);
232 axdata = [real(rlpolv), imag(rlpolv); real(olzer), imag(olzer)];
233 axlim = __axis_limits__ (axdata);
234 rldata = [real(rlpolv), imag(rlpolv) ];
236 %inname = get (sys, "inname");
237 %outname = get (sys, "outname");
239 ## build plot command args pole by pole
241 n_rlpol = rows (rlpol);
243 if (! isempty (rlzer))
247 n_A = length (olpol) - length (olzer);
251 args = cell (3, nelts);
255 len_A = 2*max (abs (axlim));
256 sigma_A = (sum(olpol) - sum(olzer))/n_A;
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)];
262 args{3,kk} = "k--;asymptotes;";
269 for ii = 1:rows(rlpol)
270 args{1,++kk} = real (rlpol (ii,:));
271 args{2,kk} = imag (rlpol (ii,:));
273 args{3,kk} = "b-;locus;";
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;";
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);
294 for ii = 1:rows(rlpol)
295 set (hplt(kk--), "linewidth", 2);
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");
310 function rlpol = sort_roots (rlpol, tolx, toly)
311 ## no point sorting of you've only got one pole!
312 if (rows (rlpol) == 1)
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);
325 [np, ng] = size (rlpol); # num poles, num gains
327 vals = rlpol(:,[jj,jj+1]);
329 for ii = 1:rows(rlpol-1)
331 dval = abs (rlpol(rdx,jj+1)-rlpol(ii,jj));
332 mindist = min (dval);
333 sidx = min (find (dval == mindist)) + ii - 1;
335 c1 = norm (diff(vals.'));
336 [vals(ii,2), vals(sidx,2)] = swap (vals(ii,2), vals(sidx,2));
337 c2 = norm (diff (vals.'));
340 [rlpol(ii,jdx), rlpol(sidx,jdx)] = swap (rlpol(ii,jdx), rlpol(sidx,jdx));
341 vals = rlpol(:,[jj,jj+1]);
350 function [b, a] = swap (a, b)