1 ## Copyright (C) 2009, 2010, 2011, 2012 Lukas F. Reichlin
3 ## This file is part of LTI Syncope.
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.
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.
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/>.
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.
28 ## LTI model. Must be a single-input and single-output (SISO) system.
30 ## Imaginary parts below @var{tol} are assumed to be zero.
31 ## If not specified, default value @code{sqrt (eps)} is taken.
37 ## Gain margin (as gain, not dBs).
39 ## Phase margin (in degrees).
41 ## Frequency for the gain margin (in rad/s).
43 ## Frequency for the phase margin (in rad/s).
52 ## L(jw) = L(jw) BTW: L(jw) = L(-jw) = conj (L(jw))
58 ## num(jw) den(-jw) = num(-jw) den(jw)
60 ## imag (num(jw) den(-jw)) = 0
61 ## imag (num(-jw) den(jw)) = 0
68 ## |L(jw)| = |-------| = 1
74 ## ------- * -------- = 1
77 ## num(jw) num(-jw) - den(jw) den(-jw) = 0
79 ## real (num(jw) num(-jw) - den(jw) den(-jw)) = 0
87 ## L(z) = L(1/z) BTW: z = e --> w = -----
93 ## num(z) den(1/z) - num(1/z) den(z) = 0
100 ## |L(z)| = |------| = 1
106 ## ------ * -------- = 1
109 ## num(z) num(1/z) - den(z) den(1/z) = 0
114 ## PS: How to get L(1/z)
116 ## p(z) = a z + b z + c z + d z + e
119 ## p(1/z) = a z + b z + c z + d z + e
122 ## = z ( a + b z + c z + d z + e z )
125 ## = ( e z + d z + c z + b z + a ) / ( z )
132 ## Author: Lukas Reichlin <lukas.reichlin@gmail.com>
133 ## Created: July 2009
136 function [gamma_r, phi_r, w_gamma_r, w_phi_r] = margin (sys, tol = sqrt (eps))
138 ## TODO: multiplot feature: margin (sys1, "b", sys2, "r", ...)
140 ## check whether arguments are OK
141 if (nargin < 1 || nargin > 2)
145 if (! isa (sys, "lti"))
146 error ("margin: argument sys must be an LTI system");
150 error ("margin: argument sys must be a SISO system");
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
159 if (continuous) # CONTINUOUS SYSTEM
161 ## create polynomials s -> jw
162 l_num = length (num);
163 l_den = length (den);
165 num_jw = num .* i.^(l_num-1 : -1 : 0);
166 den_jw = den .* i.^(l_den-1 : -1 : 0);
169 ## create gm polynomial
170 gm_poly = imag (conv (num_jw, conj (den_jw)));
172 ## find frequencies w
176 [gamma, w_gamma] = gm_filter (w, num, den, tsam, tol, continuous);
179 ## create pm polynomials
180 poly_1 = conv (num_jw, conj (num_jw));
181 poly_2 = conv (den_jw, conj (den_jw));
183 ## make polynomials equally long for subtraction
184 [poly_1, poly_2] = poly_equalizer (poly_1, poly_2);
186 ## subtract polynomials
187 pm_poly = real (poly_1 - poly_2);
189 ## find frequencies w
193 [phi, w_phi] = pm_filter (w, num, den, tsam, tol, continuous);
196 else # DISCRETE SYSTEM
198 ## create polynomials z -> 1/z
199 l_num = length (num);
200 l_den = length (den);
202 num_rev = fliplr (num);
203 den_rev = fliplr (den);
205 num_div = zeros (1, l_num);
206 den_div = zeros (1, l_den);
210 num_inv = conv (num_rev, den_div);
211 den_inv = conv (den_rev, num_div);
214 ## create gm polynomial
215 poly_1 = conv (num, den_inv);
216 poly_2 = conv (num_inv, den);
218 ## make polynomials equally long for subtraction
219 [poly_1, poly_2] = poly_equalizer (poly_1, poly_2);
221 ## subtract polynomials
222 gm_poly = poly_1 - poly_2;
224 ## find frequencies z
228 idx = find (abs (abs (z) - 1) < tol); # find z with magnitude 1
230 if (length (idx) > 0) # if z with magnitude 1 exist
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
240 ## create pm polynomials
241 poly_1 = conv (num, num_inv);
242 poly_2 = conv (den, den_inv);
244 ## make polynomials equally long for subtraction
245 [poly_1, poly_2] = poly_equalizer (poly_1, poly_2);
247 ## subtract polynomials
248 pm_poly = poly_1 - poly_2;
250 ## find frequencies z
254 idx = find (abs (abs (z) - 1) < tol); # find z with magnitude 1
256 if (length (idx) > 0) # if z with magnitude 1 exist
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
268 if (nargout == 0) # show bode diagram
270 [H, w] = __frequency_response__ (sys, [], false, 0, "std");
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);
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;
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];
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];
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);
296 xl_str = "Frequency [rad/s]";
298 xl_str = sprintf ("Frequency [rad/s] w_N = %g", pi/tsam);
302 semilogx (w, mag_db, "b", wv, [0, 0], ":k", wgm, mgmh, ":k", wgm, mgm, "r", wpm, mpm, ":k")
306 ylabel ("Magnitude [dB]")
309 semilogx (w, pha, "b", wv, [-180, -180], ":k", wgm, pgm, ":k", wpm, ppmh, ":k", wpm, ppm, "r")
313 ylabel ("Phase [deg]")
325 function [poly_eq_1, poly_eq_2] = poly_equalizer (poly_1, poly_2)
327 l_p1 = length (poly_1);
328 l_p2 = length (poly_2);
329 l_max = max (l_p1, l_p2);
331 lead_zer_1 = zeros (1, l_max - l_p1);
332 lead_zer_2 = zeros (1, l_max - l_p2);
334 poly_eq_1 = horzcat (lead_zer_1, poly_1);
335 poly_eq_2 = horzcat (lead_zer_2, poly_2);
340 function [gamma, w_gamma] = gm_filter (w, num, den, tsam, tol, continuous)
342 idx = find ((abs (imag (w)) < tol) & (real (w) > 0)); # find frequencies in R+
344 if (length (idx) > 0) # if frequencies in R+ exist
345 w_gm = real (w(idx));
350 s = exp (i * w_gm * tsam);
353 f_resp = polyval (num, s) ./ polyval (den, s);
354 gm = (abs (f_resp)).^-1;
356 ## find crossings between 0 and -1
357 idx = find ((real (f_resp) < 0) & (real (f_resp) >= -1));
359 if (length (idx) > 0) # if crossings between 0 and -1 exist
362 [gamma, idx] = min (gm);
364 else # there are no crossings between 0 and -1
365 idx = find (real (f_resp) < -1); # find crossings between -1 and -Inf
367 if (length (idx) > 0) # if crossings between -1 and -Inf exist
370 [gamma, idx] = max (gm);
377 else # there are no frequencies in R+
385 function [phi, w_phi] = pm_filter (w, num, den, tsam, tol, continuous)
387 idx = find ((abs (imag (w)) < tol) & (real (w) > 0)); # find frequencies in R+
389 if (length (idx) > 0) # if frequencies in R+ exist
390 w_pm = real (w(idx));
395 s = exp (i * w_pm * tsam);
398 f_resp = polyval (num, s) ./ polyval (den, s);
399 pm = 180 + arg (f_resp) ./ pi .* 180;
401 [phi, idx] = min (pm);
403 else # there are no frequencies in R+
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);
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];
420 %! ## results from this implementation and the "dark side" diverge
421 %! ## from the third digit after the decimal point on
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;
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;
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];
436 %!assert (margin_c, margin_c_exp, 1e-2);
437 %!assert (margin_d, margin_d_exp, 1e-2);