Skip to content

Commit 396b376

Browse files
murrayrmmurrayrm
committed
* Moved margin command from freqplot.py to margins.py (new file); updated
 matlab.py, init.py, etc to reflect changes * Added PhaseCrossoverFrequencies() function; contributed by Steffen Waldherr * Added initial unit tests for StabilityMargins(), PhaseCrossoverFrequencies()
1 parent 5747e63 commit 396b376

7 files changed

Lines changed: 236 additions & 111 deletions

File tree

‎ChangeLog‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,24 @@
1+
2011-07-15 Richard Murray <murray@malabar.local>
2+
3+
* tests/matlab_test.py (TestMatlab): added unittest for margin()
4+
commands (calling format only)
5+
6+
* src/statesp.py (StateSpace): updated comments
7+
8+
* tests/margin_test.py: set up unit tests for StabilityMargins() and
9+
PhaseCrossoverFrequencies()
10+
11+
* src/__init__.py: added margins.py to __init__
12+
13+
2011-07-14 Richard Murray <murray@malabar.local>
14+
15+
* src/margins.py (GainPhaseMargin): moved freqplot.MarginPlot to
16+
margin.StabilityMargins (MarginPlot didn't actually plot anything)
17+
18+
* src/margins.py (PhaseCrossoverFrequencies): added new function to
19+
compute frequencies that we cross real axis. Contributed by Steffen
20+
Waldherr <waldherr@ist.uni-stuttgart.de>
21+
122
2011-07-11 Richard Murray <murray@malabar.local>
223

324
* src/rlocus.py: added real() and imag() to list of functions

‎src/__init__.py‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -57,15 +57,16 @@
5757

5858
# Import functions from within the control system library
5959
#! Should probably only import the exact functions we use...
60-
from xferfcn import *
61-
from statesp import *
62-
from freqplot import *
63-
from nichols import *
6460
from bdalg import *
65-
from statefbk import *
6661
from delay import *
62+
from freqplot import *
63+
from margins import *
64+
from mateqn import *
6765
from modelsimp import *
66+
from nichols import *
6867
from rlocus import *
69-
from mateqn import *
68+
from statefbk import *
69+
from statesp import *
7070
from timeresp import ForcedResponse, InitialResponse, StepResponse, \
7171
ImpulseResponse
72+
from xferfcn import *

‎src/freqplot.py‎

Lines changed: 0 additions & 96 deletions
Original file line numberDiff line numberDiff line change
@@ -299,101 +299,6 @@ def GangOf4Plot(P, C, omega=None):
299299
phase = np.squeeze(phase_tmp)
300300
plt.subplot(224); plt.loglog(omega, mag);
301301

302-
303-
# gain and phase margins
304-
# contributed by Sawyer B. Fuller <minster@caltech.edu>
305-
def MarginPlot(sysdata, deg=True):
306-
"""Calculate gain and phase margins and associated crossover frequencies
307-
308-
Usage:
309-
310-
gm, pm, sm, wg, wp, ws = margin(sysdata, deg=True)
311-
312-
Parameters
313-
----------
314-
sysdata: linsys or (mag, phase, omega) sequence
315-
sys : linsys
316-
Linear SISO system
317-
mag, phase, omega : sequence of array_like
318-
Input magnitude, phase, and frequencies (rad/sec) sequence from
319-
bode frequency response data
320-
deg=True: boolean
321-
If true, all input and output phases in degrees, else in radians
322-
323-
Returns
324-
-------
325-
gm, pm, sm, wg, wp, ws: float
326-
Gain margin gm, phase margin pm, stability margin sm, and
327-
associated crossover
328-
frequencies wg, wp, and ws of SISO open-loop. If more than
329-
one crossover frequency is detected, returns the lowest corresponding
330-
margin.
331-
"""
332-
#TODO do this precisely without the effects of discretization of frequencies?
333-
#TODO assumes SISO
334-
#TODO unit tests, margin plot
335-
336-
if (not getattr(sysdata, '__iter__', False)):
337-
sys = sysdata
338-
mag, phase, omega = bode(sys, deg=deg, Plot=False)
339-
elif len(sysdata) == 3:
340-
mag, phase, omega = sysdata
341-
else:
342-
raise ValueError("Margin sysdata must be either a linear system or a 3-sequence of mag, phase, omega.")
343-
344-
if deg:
345-
cycle = 360.
346-
crossover = 180.
347-
else:
348-
cycle = 2 * np.pi
349-
crossover = np.pi
350-
351-
wrapped_phase = -np.mod(phase, cycle)
352-
353-
# phase margin from minimum phase among all gain crossovers
354-
neg_mag_crossings_i = np.nonzero(np.diff(mag < 1) > 0)[0]
355-
mag_crossings_p = wrapped_phase[neg_mag_crossings_i]
356-
if len(neg_mag_crossings_i) == 0:
357-
if mag[0] < 1: # gain always less than one
358-
wp = np.nan
359-
pm = np.inf
360-
else: # gain always greater than one
361-
print "margin: no magnitude crossings found"
362-
wp = np.nan
363-
pm = np.nan
364-
else:
365-
min_mag_crossing_i = neg_mag_crossings_i[np.argmin(mag_crossings_p)]
366-
wp = omega[min_mag_crossing_i]
367-
pm = crossover + phase[min_mag_crossing_i]
368-
if pm < 0:
369-
print "warning: system unstable: negative phase margin"
370-
371-
# gain margin from minimum gain margin among all phase crossovers
372-
neg_phase_crossings_i = np.nonzero(np.diff(wrapped_phase < -crossover) > 0)[0]
373-
neg_phase_crossings_g = mag[neg_phase_crossings_i]
374-
if len(neg_phase_crossings_i) == 0:
375-
wg = np.nan
376-
gm = np.inf
377-
else:
378-
min_phase_crossing_i = neg_phase_crossings_i[
379-
np.argmax(neg_phase_crossings_g)]
380-
wg = omega[min_phase_crossing_i]
381-
gm = abs(1/mag[min_phase_crossing_i])
382-
if gm < 1:
383-
print "warning: system unstable: gain margin < 1"
384-
385-
# stability margin from minimum abs distance from -1 point
386-
if deg:
387-
phase_rad = phase * np.pi / 180.
388-
else:
389-
phase_rad = phase
390-
L = mag * np.exp(1j * phase_rad) # complex loop response to -1 pt
391-
min_Lplus1_i = np.argmin(np.abs(L + 1))
392-
sm = np.abs(L[min_Lplus1_i] + 1)
393-
ws = phase[min_Lplus1_i]
394-
395-
return gm, pm, sm, wg, wp, ws
396-
397302
#
398303
# Utility functions
399304
#
@@ -461,4 +366,3 @@ def default_frequency_range(syslist):
461366
bode = BodePlot
462367
nyquist = NyquistPlot
463368
gangof4 = GangOf4Plot
464-
margin = MarginPlot

‎src/margins.py‎

Lines changed: 193 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,193 @@
1+
"""margin.py
2+
3+
Functions for computing stability margins and related functions.
4+
5+
Routeins in this module:
6+
7+
margin.StabilityMargins
8+
margin.PhaseCrossoverFrequencies
9+
"""
10+
11+
"""Copyright (c) 2011 by California Institute of Technology
12+
All rights reserved.
13+
14+
Redistribution and use in source and binary forms, with or without
15+
modification, are permitted provided that the following conditions
16+
are met:
17+
18+
1. Redistributions of source code must retain the above copyright
19+
notice, this list of conditions and the following disclaimer.
20+
21+
2. Redistributions in binary form must reproduce the above copyright
22+
notice, this list of conditions and the following disclaimer in the
23+
documentation and/or other materials provided with the distribution.
24+
25+
3. Neither the name of the California Institute of Technology nor
26+
the names of its contributors may be used to endorse or promote
27+
products derived from this software without specific prior
28+
written permission.
29+
30+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
31+
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
32+
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
33+
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CALTECH
34+
OR THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
35+
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
36+
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
37+
USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
38+
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
39+
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
40+
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
41+
SUCH DAMAGE.
42+
43+
Author: Richard M. Murray
44+
Date: 14 July 2011
45+
46+
$Id: xferfcn.py 165 2011-06-26 02:44:09Z murrayrm $
47+
48+
"""
49+
50+
import xferfcn
51+
from freqplot import bode
52+
import numpy as np
53+
54+
# gain and phase margins
55+
# contributed by Sawyer B. Fuller <minster@caltech.edu>
56+
#! TODO - need to add unit test functions
57+
def StabilityMargins(sysdata, deg=True):
58+
"""Calculate gain, phase and stability margins and associated
59+
crossover frequencies.
60+
61+
Usage:
62+
63+
gm, pm, sm, wg, wp, ws = StabilityMargins(sysdata, deg=True)
64+
65+
Parameters
66+
----------
67+
sysdata: linsys or (mag, phase, omega) sequence
68+
sys : linsys
69+
Linear SISO system
70+
mag, phase, omega : sequence of array_like
71+
Input magnitude, phase, and frequencies (rad/sec) sequence from
72+
bode frequency response data
73+
deg=True: boolean
74+
If true, all input and output phases in degrees, else in radians
75+
76+
Returns
77+
-------
78+
gm, pm, sm, wg, wp, ws: float
79+
Gain margin gm, phase margin pm, stability margin sm, and
80+
associated crossover
81+
frequencies wg, wp, and ws of SISO open-loop. If more than
82+
one crossover frequency is detected, returns the lowest corresponding
83+
margin.
84+
"""
85+
#TODO do this precisely without the effects of discretization of frequencies?
86+
#TODO assumes SISO
87+
#TODO unit tests, margin plot
88+
89+
if (not getattr(sysdata, '__iter__', False)):
90+
sys = sysdata
91+
mag, phase, omega = bode(sys, deg=deg, Plot=False)
92+
elif len(sysdata) == 3:
93+
mag, phase, omega = sysdata
94+
else:
95+
raise ValueError("Margin sysdata must be either a linear system or a 3-sequence of mag, phase, omega.")
96+
97+
if deg:
98+
cycle = 360.
99+
crossover = 180.
100+
else:
101+
cycle = 2 * np.pi
102+
crossover = np.pi
103+
104+
wrapped_phase = -np.mod(phase, cycle)
105+
106+
# phase margin from minimum phase among all gain crossovers
107+
neg_mag_crossings_i = np.nonzero(np.diff(mag < 1) > 0)[0]
108+
mag_crossings_p = wrapped_phase[neg_mag_crossings_i]
109+
if len(neg_mag_crossings_i) == 0:
110+
if mag[0] < 1: # gain always less than one
111+
wp = np.nan
112+
pm = np.inf
113+
else: # gain always greater than one
114+
print "margin: no magnitude crossings found"
115+
wp = np.nan
116+
pm = np.nan
117+
else:
118+
min_mag_crossing_i = neg_mag_crossings_i[np.argmin(mag_crossings_p)]
119+
wp = omega[min_mag_crossing_i]
120+
pm = crossover + phase[min_mag_crossing_i]
121+
if pm < 0:
122+
print "warning: system unstable: negative phase margin"
123+
124+
# gain margin from minimum gain margin among all phase crossovers
125+
neg_phase_crossings_i = np.nonzero(np.diff(wrapped_phase < -crossover) > 0)[0]
126+
neg_phase_crossings_g = mag[neg_phase_crossings_i]
127+
if len(neg_phase_crossings_i) == 0:
128+
wg = np.nan
129+
gm = np.inf
130+
else:
131+
min_phase_crossing_i = neg_phase_crossings_i[
132+
np.argmax(neg_phase_crossings_g)]
133+
wg = omega[min_phase_crossing_i]
134+
gm = abs(1/mag[min_phase_crossing_i])
135+
if gm < 1:
136+
print "warning: system unstable: gain margin < 1"
137+
138+
# stability margin from minimum abs distance from -1 point
139+
if deg:
140+
phase_rad = phase * np.pi / 180.
141+
else:
142+
phase_rad = phase
143+
L = mag * np.exp(1j * phase_rad) # complex loop response to -1 pt
144+
min_Lplus1_i = np.argmin(np.abs(L + 1))
145+
sm = np.abs(L[min_Lplus1_i] + 1)
146+
ws = phase[min_Lplus1_i]
147+
148+
return gm, pm, sm, wg, wp, ws
149+
150+
# Contributed by Steffen Waldherr <waldherr@ist.uni-stuttgart.de>
151+
#! TODO - need to add test functions
152+
def PhaseCrossoverFrequencies(sys):
153+
"""
154+
Compute frequencies and gains at intersections with real axis
155+
in Nyquist plot.
156+
157+
Call as:
158+
omega, gain = PhaseCrossoverFrequencies()
159+
160+
Returns
161+
-------
162+
omega: 1d array of (non-negative) frequencies where Nyquist plot
163+
intersects the real axis
164+
165+
gain: 1d array of corresponding gains
166+
167+
Examples
168+
--------
169+
>>> tf = TransferFunction([1], [1, 2, 3, 4])
170+
>>> PhaseCrossoverFrequenies(tf)
171+
(array([ 1.73205081, 0. ]), array([-0.5 , 0.25]))
172+
"""
173+
174+
# Convert to a transfer function
175+
tf = xferfcn._convertToTransferFunction(sys)
176+
177+
# if not siso, fall back to (0,0) element
178+
#! TODO: should add a check and warning here
179+
num = tf.num[0][0]
180+
den = tf.den[0][0]
181+
182+
# Compute frequencies that we cross over the real axis
183+
numj = (1.j)**np.arange(len(num)-1,-1,-1)*num
184+
denj = (-1.j)**np.arange(len(den)-1,-1,-1)*den
185+
allfreq = np.roots(np.imag(np.polymul(numj,denj)))
186+
realfreq = np.real(allfreq[np.isreal(allfreq)])
187+
realposfreq = realfreq[realfreq >= 0.]
188+
189+
# using real() to avoid rounding errors and results like 1+0j
190+
# it would be nice to have a vectorized version of self.evalfr here
191+
gain = np.real(np.asarray([tf.evalfr(f)[0][0] for f in realposfreq]))
192+
193+
return realposfreq, gain

‎src/matlab.py‎

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -72,6 +72,7 @@
7272
import ctrlutil
7373
import freqplot
7474
import timeresp
75+
import margins
7576
from statesp import StateSpace, _rss_generate, _convertToStateSpace
7677
from xferfcn import TransferFunction, _convertToTransferFunction
7778
from lti import Lti #base class of StateSpace, TransferFunction
@@ -1025,7 +1026,6 @@ def rlocus(sys, klist = None, **keywords):
10251026
rlist = RootLocus(sys, klist, **keywords)
10261027
return rlist, klist
10271028

1028-
10291029
def margin(*args):
10301030
"""Calculate gain and phase margins and associated crossover frequencies
10311031
@@ -1060,15 +1060,14 @@ def margin(*args):
10601060
"""
10611061
if len(args) == 1:
10621062
sys = args[0]
1063-
margins = freqplot.margin(sys)
1063+
margin = margins.StabilityMargins(sys)
10641064
elif len(args) == 3:
1065-
margins = freqplot.margin(args)
1065+
margin = margins.StabilityMargins(args)
10661066
else:
10671067
raise ValueError("Margin needs 1 or 3 arguments; received %i."
10681068
% len(args))
10691069

1070-
return margins[0], margins[1], margins[3], margins[4]
1071-
1070+
return margin[0], margin[1], margin[3], margin[4]
10721071

10731072
def dcgain(*args):
10741073
'''

0 commit comments

Comments
 (0)