octave_packages/control-2.3.52/margin.m

   1 ## Copyright (C) 2009, 2010, 2011, 2012   Lukas F. Reichlin
   2 ##
   3 ## This file is part of LTI Syncope.
   4 ##
   5 ## LTI Syncope is free software: you can redistribute it and/or modify
   6 ## it under the terms of the GNU General Public License as published by
   7 ## the Free Software Foundation, either version 3 of the License, or
   8 ## (at your option) any later version.
   9 ##
  10 ## LTI Syncope is distributed in the hope that it will be useful,
  11 ## but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13 ## GNU General Public License for more details.
  14 ##
  15 ## You should have received a copy of the GNU General Public License
  16 ## along with LTI Syncope.  If not, see <http://www.gnu.org/licenses/>.
  17
  18 ## -*- texinfo -*-
  19 ## @deftypefn{Function File} {[@var{gamma}, @var{phi}, @var{w_gamma}, @var{w_phi}] =} margin (@var{sys})
  20 ## @deftypefnx{Function File} {[@var{gamma}, @var{phi}, @var{w_gamma}, @var{w_phi}] =} margin (@var{sys}, @var{tol})
  21 ## Gain and phase margin of a system.  If no output arguments are given, both gain and phase margin
  22 ## are plotted on a bode diagram.  Otherwise, the margins and their corresponding frequencies are
  23 ## computed and returned.
  24 ##
  25 ## @strong{Inputs}
  26 ## @table @var
  27 ## @item sys
  28 ## LTI model.  Must be a single-input and single-output (SISO) system.
  29 ## @item tol
  30 ## Imaginary parts below @var{tol} are assumed to be zero.
  31 ## If not specified, default value @code{sqrt (eps)} is taken.
  32 ## @end table
  33 ##
  34 ## @strong{Outputs}
  35 ## @table @var
  36 ## @item gamma
  37 ## Gain margin (as gain, not dBs).
  38 ## @item phi
  39 ## Phase margin (in degrees).
  40 ## @item w_gamma
  41 ## Frequency for the gain margin (in rad/s).
  42 ## @item w_phi
  43 ## Frequency for the phase margin (in rad/s).
  44 ## @end table
  45 ##
  46 ## @strong{Equations}
  47 ## @example
  48 ## @group
  49 ## CONTINUOUS SYSTEMS
  50 ## Gain Margin
  51 ##         _               _
  52 ## L(jw) = L(jw)      BTW: L(jw) = L(-jw) = conj (L(jw))
  53 ##
  54 ## num(jw)   num(-jw)
  55 ## ------- = --------
  56 ## den(jw)   den(-jw)
  57 ##
  58 ## num(jw) den(-jw) = num(-jw) den(jw)
  59 ##
  60 ## imag (num(jw) den(-jw)) = 0
  61 ## imag (num(-jw) den(jw)) = 0
  62 ## @end group
  63 ## @end example
  64 ## @example
  65 ## @group
  66 ## Phase Margin
  67 ##           |num(jw)|
  68 ## |L(jw)| = |-------| = 1
  69 ##           |den(jw)|
  70 ##   _     2      2
  71 ## z z = Re z + Im z
  72 ##
  73 ## num(jw)   num(-jw)
  74 ## ------- * -------- = 1
  75 ## den(jw)   den(-jw)
  76 ##
  77 ## num(jw) num(-jw) - den(jw) den(-jw) = 0
  78 ##
  79 ## real (num(jw) num(-jw) - den(jw) den(-jw)) = 0
  80 ## @end group
  81 ## @end example
  82 ## @example
  83 ## @group
  84 ## DISCRETE SYSTEMS
  85 ## Gain Margin
  86 ##                              jwT         log z
  87 ## L(z) = L(1/z)      BTW: z = e    --> w = -----
  88 ##                                           j T
  89 ## num(z)   num(1/z)
  90 ## ------ = --------
  91 ## den(z)   den(1/z)
  92 ##
  93 ## num(z) den(1/z) - num(1/z) den(z) = 0
  94 ## @end group
  95 ## @end example
  96 ## @example
  97 ## @group
  98 ## Phase Margin
  99 ##          |num(z)|
 100 ## |L(z)| = |------| = 1
 101 ##          |den(z)|
 102 ##
 103 ## L(z) L(1/z) = 1
 104 ##
 105 ## num(z)   num(1/z)
 106 ## ------ * -------- = 1
 107 ## den(z)   den(1/z)
 108 ##
 109 ## num(z) num(1/z) - den(z) den(1/z) = 0
 110 ## @end group
 111 ## @end example
 112 ## @example
 113 ## @group
 114 ## PS: How to get L(1/z)
 115 ##           4       3       2
 116 ## p(z) = a z  +  b z  +  c z  +  d z  +  e
 117 ##
 118 ##             -4      -3      -2      -1
 119 ## p(1/z) = a z  +  b z  +  c z  +  d z  +  e
 120 ##
 121 ##           -4                    2       3       4
 122 ##        = z   ( a  +  b z  +  c z  +  d z  +  e z  )
 123 ##
 124 ##               4       3       2                     4
 125 ##        = ( e z  +  d z  +  c z  +  b z  +  a ) / ( z  )
 126 ## @end group
 127 ## @end example
 128 ##
 129 ## @seealso{roots}
 130 ## @end deftypefn
 131
 132 ## Author: Lukas Reichlin <lukas.reichlin@gmail.com>
 133 ## Created: July 2009
 134 ## Version: 0.9.1
 135
 136 function [gamma_r, phi_r, w_gamma_r, w_phi_r] = margin (sys, tol = sqrt (eps))
 137
 138   ## TODO: multiplot feature:   margin (sys1, "b", sys2, "r", ...)
 139
 140   ## check whether arguments are OK
 141   if (nargin < 1 || nargin > 2)
 142     print_usage ();
 143   endif
 144
 145   if (! isa (sys, "lti"))
 146     error ("margin: argument sys must be an LTI system");
 147   endif
 148
 149   if (! issiso (sys))
 150     error ("margin: argument sys must be a SISO system");
 151   endif
 152
 153   ## get transfer function
 154   [num, den, tsam] = tfdata (sys, "vector");
 155   continuous = isct (sys);
 156   tsam = abs (tsam);                                     # use 1 second as default if tsam == -1
 157
 158
 159   if (continuous)                                        # CONTINUOUS SYSTEM
 160
 161     ## create polynomials s -> jw
 162     l_num = length (num);
 163     l_den = length (den);
 164
 165     num_jw = num .* i.^(l_num-1 : -1 : 0);
 166     den_jw = den .* i.^(l_den-1 : -1 : 0);
 167
 168     ## GAIN MARGIN
 169     ## create gm polynomial
 170     gm_poly = imag (conv (num_jw, conj (den_jw)));
 171
 172     ## find frequencies w
 173     w = roots (gm_poly);
 174
 175     ## filter results
 176     [gamma, w_gamma] = gm_filter (w, num, den, tsam, tol, continuous);
 177
 178     ## PHASE MARGIN
 179     ## create pm polynomials
 180     poly_1 = conv (num_jw, conj (num_jw));
 181     poly_2 = conv (den_jw, conj (den_jw));
 182
 183     ## make polynomials equally long for subtraction
 184     [poly_1, poly_2] = poly_equalizer (poly_1, poly_2);
 185
 186     ## subtract polynomials
 187     pm_poly = real (poly_1 - poly_2);
 188
 189     ## find frequencies w
 190     w = roots (pm_poly);
 191
 192     ## filter results
 193     [phi, w_phi] = pm_filter (w, num, den, tsam, tol, continuous);
 194
 195
 196   else                                                   # DISCRETE SYSTEM
 197
 198     ## create polynomials z -> 1/z
 199     l_num = length (num);
 200     l_den = length (den);
 201
 202     num_rev = fliplr (num);
 203     den_rev = fliplr (den);
 204
 205     num_div = zeros (1, l_num);
 206     den_div = zeros (1, l_den);
 207     num_div(1) = 1;
 208     den_div(1) = 1;
 209
 210     num_inv = conv (num_rev, den_div);
 211     den_inv = conv (den_rev, num_div);
 212
 213     ## GAIN MARGIN
 214     ## create gm polynomial
 215     poly_1 = conv (num, den_inv);
 216     poly_2 = conv (num_inv, den);
 217
 218     ## make polynomials equally long for subtraction
 219     [poly_1, poly_2] = poly_equalizer (poly_1, poly_2);
 220
 221     ## subtract polynomials
 222     gm_poly = poly_1 - poly_2;
 223
 224     ## find frequencies z
 225     z = roots (gm_poly);
 226
 227     ## filter results
 228     idx = find (abs (abs (z) - 1) < tol);                # find z with magnitude 1
 229
 230     if (length (idx) > 0)                                # if z with magnitude 1 exist
 231       z_gm = z(idx);
 232       w = log (z_gm) / (i*tsam);                         # get frequencies w from z
 233       [gamma, w_gamma] = gm_filter (w, num, den, tsam, tol, continuous);
 234     else                                                 # there are no z with magnitude 1
 235       gamma = Inf;
 236       w_gamma = NaN;
 237     endif
 238
 239     ## PHASE MARGIN
 240     ## create pm polynomials
 241     poly_1 = conv (num, num_inv);
 242     poly_2 = conv (den, den_inv);
 243
 244     ## make polynomials equally long for subtraction
 245     [poly_1, poly_2] = poly_equalizer (poly_1, poly_2);
 246
 247     ## subtract polynomials
 248     pm_poly = poly_1 - poly_2;
 249
 250     ## find frequencies z
 251     z = roots (pm_poly);
 252
 253     ## filter results
 254     idx = find (abs (abs (z) - 1) < tol);                # find z with magnitude 1
 255
 256     if (length (idx) > 0)                                # if z with magnitude 1 exist
 257       z_gm = z(idx);
 258       w = log (z_gm) / (i*tsam);                         # get frequencies w from z
 259       [phi, w_phi] = pm_filter (w, num, den, tsam, tol, continuous);
 260     else                                                 # there are no z with magnitude 1
 261       phi = 180;
 262       w_phi = NaN;
 263     endif
 264
 265   endif
 266
 267
 268   if (nargout == 0)                                      # show bode diagram
 269
 270     [H, w] = __frequency_response__ (sys, [], false, 0, "std");
 271
 272     H = reshape (H, [], 1);
 273     mag_db = 20 * log10 (abs (H));
 274     pha = unwrap (arg (H)) * 180 / pi;
 275     gamma_db = 20 * log10 (gamma);
 276
 277     wv = [min(w), max(w)];
 278     ax_vec_mag = __axis_limits__ ([w(:), mag_db(:)]);
 279     ax_vec_mag(1:2) = wv;
 280     ax_vec_pha = __axis_limits__ ([w(:), pha(:)]);
 281     ax_vec_pha(1:2) = wv;
 282
 283     wgm = [w_gamma, w_gamma];
 284     mgmh = [-gamma_db, ax_vec_mag(3)];
 285     mgm = [0, -gamma_db];
 286     pgm = [ax_vec_pha(4), -180];
 287
 288     wpm = [w_phi, w_phi];
 289     mpm = [0, ax_vec_mag(3)];
 290     ppmh = [ax_vec_pha(4), phi - 180];
 291     ppm = [phi - 180, -180];
 292
 293     title_str = sprintf ("GM = %g dB (at %g rad/s),   PM = %g deg (at %g rad/s)",
 294                          gamma_db, w_gamma, phi, w_phi);
 295     if (continuous)
 296       xl_str = "Frequency [rad/s]";
 297     else
 298       xl_str = sprintf ("Frequency [rad/s]     w_N = %g", pi/tsam);
 299     endif
 300
 301     subplot (2, 1, 1)
 302     semilogx (w, mag_db, "b", wv, [0, 0], ":k", wgm, mgmh, ":k", wgm, mgm, "r", wpm, mpm, ":k")
 303     axis (ax_vec_mag)
 304     grid ("on")
 305     title (title_str)
 306     ylabel ("Magnitude [dB]")
 307
 308     subplot (2, 1, 2)
 309     semilogx (w, pha, "b", wv, [-180, -180], ":k", wgm, pgm, ":k", wpm, ppmh, ":k", wpm, ppm, "r")
 310     axis (ax_vec_pha)
 311     grid ("on")
 312     xlabel (xl_str)
 313     ylabel ("Phase [deg]")
 314
 315   else                                                   # return values
 316     gamma_r = gamma;
 317     phi_r = phi;
 318     w_gamma_r = w_gamma;
 319     w_phi_r = w_phi;
 320   endif
 321
 322 endfunction
 323
 324
 325 function [poly_eq_1, poly_eq_2] = poly_equalizer (poly_1, poly_2)
 326
 327   l_p1 = length (poly_1);
 328   l_p2 = length (poly_2);
 329   l_max = max (l_p1, l_p2);
 330
 331   lead_zer_1 = zeros (1, l_max - l_p1);
 332   lead_zer_2 = zeros (1, l_max - l_p2);
 333
 334   poly_eq_1 = horzcat (lead_zer_1, poly_1);
 335   poly_eq_2 = horzcat (lead_zer_2, poly_2);
 336
 337 endfunction
 338
 339
 340 function [gamma, w_gamma] = gm_filter (w, num, den, tsam, tol, continuous)
 341
 342   idx = find ((abs (imag (w)) < tol) & (real (w) > 0));  # find frequencies in R+
 343
 344   if (length (idx) > 0)                                  # if frequencies in R+ exist
 345     w_gm = real (w(idx));
 346
 347     if (continuous)
 348       s = i * w_gm;
 349     else
 350       s = exp (i * w_gm * tsam);
 351     endif
 352
 353     f_resp = polyval (num, s) ./ polyval (den, s);
 354     gm = (abs (f_resp)).^-1;
 355
 356     ## find crossings between 0 and -1
 357     idx = find ((real (f_resp) < 0) & (real (f_resp) >= -1));
 358
 359     if (length (idx) > 0)                                # if crossings between 0 and -1 exist
 360       gm = gm(idx);
 361       w_gm = w_gm(idx);
 362       [gamma, idx] = min (gm);
 363       w_gamma = w_gm(idx);
 364     else                                                 # there are no crossings between 0 and -1
 365       idx = find (real (f_resp) < -1);                   # find crossings between -1 and -Inf
 366
 367       if (length (idx) > 0)                              # if crossings between -1 and -Inf exist
 368         gm = gm(idx);
 369         w_gm = w_gm(idx);
 370         [gamma, idx] = max (gm);
 371         w_gamma = w_gm(idx);
 372       else
 373         gamma = Inf;
 374         w_gamma = NaN;
 375       endif
 376     endif
 377   else                                                   # there are no frequencies in R+
 378     gamma = Inf;
 379     w_gamma = NaN;
 380   endif
 381
 382 endfunction
 383
 384
 385 function [phi, w_phi] = pm_filter (w, num, den, tsam, tol, continuous)
 386
 387   idx = find ((abs (imag (w)) < tol) & (real (w) > 0));  # find frequencies in R+
 388
 389   if (length (idx) > 0)                                  # if frequencies in R+ exist
 390     w_pm = real (w(idx));
 391
 392     if (continuous)
 393       s = i * w_pm;
 394     else
 395       s = exp (i * w_pm * tsam);
 396     endif
 397
 398     f_resp = polyval (num, s) ./ polyval (den, s);
 399     pm = 180 + arg (f_resp) ./ pi .* 180;
 400
 401     [phi, idx] = min (pm);
 402     w_phi = w_pm(idx);
 403   else                                                   # there are no frequencies in R+
 404     phi = 180;
 405     w_phi = NaN;
 406   endif
 407
 408 endfunction
 409
 410
 411 %!shared margin_c, margin_c_exp, margin_d, margin_d_exp
 412 %! sysc = tf ([24], [1, 6, 11, 6]);
 413 %! [gamma_c, phi_c, w_gamma_c, w_phi_c] = margin (sysc);
 414 %! sysd = c2d (sysc, 0.3);
 415 %! [gamma_d, phi_d, w_gamma_d, w_phi_d] = margin (sysd);
 416 %!
 417 %! margin_c = [gamma_c, phi_c, w_gamma_c, w_phi_c];
 418 %! margin_d = [gamma_d, phi_d, w_gamma_d, w_phi_d];
 419 %!
 420 %! ## results from this implementation and the "dark side" diverge
 421 %! ## from the third digit after the decimal point on
 422 %!
 423 %! gamma_c_exp = 2.50;
 424 %! phi_c_exp = 35.43;
 425 %! w_gamma_c_exp = 3.32;
 426 %! w_phi_c_exp = 2.06;
 427 %!
 428 %! gamma_d_exp = 1.41;
 429 %! phi_d_exp = 18.60;
 430 %! w_gamma_d_exp = 2.48;
 431 %! w_phi_d_exp = 2.04;
 432 %!
 433 %! margin_c_exp = [gamma_c_exp, phi_c_exp, w_gamma_c_exp, w_phi_c_exp];
 434 %! margin_d_exp = [gamma_d_exp, phi_d_exp, w_gamma_d_exp, w_phi_d_exp];
 435 %!
 436 %!assert (margin_c, margin_c_exp, 1e-2);
 437 %!assert (margin_d, margin_d_exp, 1e-2);