Skip to content

Commit 55878a3

Browse files
repaghrepagh
authored
Merge pull request python-control#148 from repagh/fix-initial-discrete-jun2017
Fix initial discrete jun2017
2 parents e3c0f79 + 71530a4 commit 55878a3

4 files changed

Lines changed: 223 additions & 46 deletions

File tree

‎control/margins.py‎

Lines changed: 38 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -2,10 +2,11 @@
22
33
Functions for computing stability margins and related functions.
44
5-
Routeins in this module:
5+
Routines in this module:
66
77
margin.stability_margins
88
margin.phase_crossover_frequencies
9+
margin.margin
910
"""
1011

1112
# Python 3 compatibility (needs to go here)
@@ -87,7 +88,17 @@ def _polysqr(pol):
8788
# RvP, July 8, 2015, augmented to calculate all phase/gain crossings with
8889
# frd data. Correct to return smallest phase
8990
# margin, smallest gain margin and their frequencies
90-
def stability_margins(sysdata, returnall=False, epsw=1e-8):
91+
# RvP, Jun 10, 2017, modified the inclusion of roots found for phase
92+
# crossing to include all >= 0, made subsequent calc
93+
# insensitive to div by 0
94+
# also changed the selection of which crossings to
95+
# return on basis of "A note on the Gain and Phase
96+
# Margin Concepts" Journal of Control and Systems
97+
# Engineering, Yazdan Bavafi-Toosi, Dec 2015, vol 3
98+
# issue 1, pp 51-59, closer to Matlab behavior, but
99+
# not completely identical in edge cases, which don't
100+
# cross but touch gain=1
101+
def stability_margins(sysdata, returnall=False, epsw=0.0):
91102
"""Calculate stability margins and associated crossover frequencies.
92103
93104
Parameters
@@ -104,7 +115,7 @@ def stability_margins(sysdata, returnall=False, epsw=1e-8):
104115
minimum stability margins. For frequency data or FRD systems, only one
105116
margin is found and returned.
106117
epsw: float, optional
107-
Frequencies below this value (default 1e-8) are considered static gain,
118+
Frequencies below this value (default 0.0) are considered static gain,
108119
and not returned as margin.
109120
110121
Returns
@@ -161,12 +172,13 @@ def stability_margins(sysdata, returnall=False, epsw=1e-8):
161172
#print ('2:w_180', w_180)
162173

163174
# evaluate response at remaining frequencies, to test for phase 180 vs 0
164-
resp_w_180 = np.real(np.polyval(sys.num[0][0], 1.j*w_180) /
165-
np.polyval(sys.den[0][0], 1.j*w_180))
166-
#print ('resp_w_180', resp_w_180)
175+
with np.errstate(all='ignore'):
176+
resp_w_180 = np.real(
177+
np.polyval(sys.num[0][0], 1.j*w_180) /
178+
np.polyval(sys.den[0][0], 1.j*w_180))
167179

168180
# only keep frequencies where the negative real axis is crossed
169-
w_180 = w_180[np.real(resp_w_180) < 0.0]
181+
w_180 = w_180[np.real(resp_w_180) <= 0.0]
170182

171183
# and sort
172184
w_180.sort()
@@ -253,20 +265,30 @@ def dstab(w):
253265

254266
# margins, as iterables, converted frdata and xferfcn calculations to
255267
# vector for this
256-
GM = 1/np.abs(sys.evalfr(w_180)[0][0])
268+
with np.errstate(all='ignore'):
269+
gain_w_180 = np.abs(sys.evalfr(w_180)[0][0])
270+
GM = 1.0/gain_w_180
257271
SM = np.abs(sys.evalfr(wstab)[0][0]+1)
258-
PM = np.angle(sys.evalfr(wc)[0][0], deg=True) + 180
259-
272+
PM = np.remainder(np.angle(sys.evalfr(wc)[0][0], deg=True), 360.0) - 180.0
273+
260274
if returnall:
261275
return GM, PM, SM, w_180, wc, wstab
262276
else:
277+
if GM.shape[0] and not np.isinf(GM).all():
278+
with np.errstate(all='ignore'):
279+
gmidx = np.where(np.abs(np.log(GM)) ==
280+
np.min(np.abs(np.log(GM))))
281+
else:
282+
gmidx = -1
283+
if PM.shape[0]:
284+
pmidx = np.where(np.abs(PM) == np.amin(np.abs(PM)))[0]
263285
return (
264-
(GM.shape[0] or None) and np.amin(GM),
265-
(PM.shape[0] or None) and np.amin(PM),
266-
(SM.shape[0] or None) and np.amin(SM),
267-
(w_180.shape[0] or None) and w_180[GM==np.amin(GM)][0],
268-
(wc.shape[0] or None) and wc[PM==np.amin(PM)][0],
269-
(wstab.shape[0] or None) and wstab[SM==np.amin(SM)][0])
286+
(not gmidx != -1 and float('inf')) or GM[gmidx][0],
287+
(not PM.shape[0] and float('inf')) or PM[pmidx][0],
288+
(not SM.shape[0] and float('inf')) or np.amin(SM),
289+
(not gmidx != -1 and float('nan')) or w_180[gmidx][0],
290+
(not wc.shape[0] and float('nan')) or wc[pmidx][0],
291+
(not wstab.shape[0] and float('nan')) or wstab[SM==np.amin(SM)][0])
270292

271293

272294
# Contributed by Steffen Waldherr <waldherr@ist.uni-stuttgart.de>

‎control/tests/margin_test.py‎

Lines changed: 168 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,22 @@
1111
from control.statesp import StateSpace
1212
from control.margins import *
1313

14+
def assert_array_almost_equal(x, y, ndigit=4):
15+
16+
x = np.array(x)
17+
y = np.array(y)
18+
try:
19+
if np.isfinite(x).any() and \
20+
np.equal(np.isfinite(x), np.isfinite(y)).all() and \
21+
np.equal(np.isnan(x), np.isnan(y)).all():
22+
np.testing.assert_array_almost_equal(
23+
x[np.isfinite(x)], y[np.isfinite(y)], ndigit)
24+
return
25+
except TypeError as e:
26+
print("Error", e, "with", x, "and", y)
27+
#raise e
28+
np.testing.assert_array_almost_equal(x, y, ndigit)
29+
1430
class TestMargin(unittest.TestCase):
1531
"""These are tests for the margin commands in margin.py."""
1632

@@ -25,11 +41,21 @@ def setUp(self):
2541
(StateSpace([[1., 4.], [3., 2.]], [[1.], [-4.]],
2642
[[1., 0.]], [[0.]]),
2743
[], [], [147.0743], [2.5483]),
28-
((8.75*(4*s**2+0.4*s+1))/((100*s+1)*(s**2+0.22*s+1)) *
29-
1./(s**2/(10.**2)+2*0.04*s/10.+1),
30-
[2.2716], [10.0053], [97.5941, 360-157.7904, 134.7359],
44+
((8.75*(4*s**2+0.4*s+1))/((100*s+1)*(s**2+0.22*s+1)) *
45+
1./(s**2/(10.**2)+2*0.04*s/10.+1),
46+
[2.2716], [10.0053], [97.5941, -157.7904, 134.7359],
3147
[0.0850, 0.9373, 1.0919]))
48+
3249

50+
"""
51+
sys1 = tf([1, 2], [1, 2, 3]);
52+
sys2 = tf([1], [1, 2, 3, 4]);
53+
sys3 = ss([1, 4; 3, 2], [1; -4], ...
54+
[1, 0], [0])
55+
s = tf('s')
56+
sys4 = (8.75*(4*s^2+0.4*s+1))/((100*s+1)*(s^2+0.22*s+1)) * ...
57+
1.0/(s^2/(10.0^2)+2*0.04*s/10.0+1);
58+
"""
3359

3460
self.sys1 = TransferFunction([1, 2], [1, 2, 3])
3561
# alternative
@@ -43,6 +69,95 @@ def setUp(self):
4369
self.stability_margins4 = \
4470
[2.2716, 97.5941, 0.5591, 10.0053, 0.0850, 9.9918]
4571

72+
"""
73+
hm1 = s/(s+1);
74+
h0 = 1/(s+1)^3;
75+
h1 = (s + 0.1)/s/(s+1);
76+
h2 = (s + 0.1)/s^2/(s+1);
77+
h3 = (s + 0.1)*(s+0.1)/s^3/(s+1);
78+
"""
79+
self.types = {
80+
'typem1': s/(s+1),
81+
'type0': 1/(s+1)**3,
82+
'type1': (s + 0.1)/s/(s+1),
83+
'type2': (s + 0.1)/s**2/(s+1),
84+
'type3': (s + 0.1)*(s+0.1)/s**3/(s+1) }
85+
self.tmargin = ( self.types,
86+
dict(sys='typem1', K=2.0, digits=3, result=(
87+
float('Inf'), -120.0007, float('NaN'), 0.5774)),
88+
dict(sys='type0', K = 0.8, digits=3, result=(
89+
10.0014, float('inf'), 1.7322, float('nan'))),
90+
dict(sys='type0', K = 2.0, digits=2, result=(
91+
4.000, 67.6058, 1.7322, 0.7663)),
92+
dict(sys='type1', K=1.0, digits=4, result=(
93+
float('Inf'), 144.9032, float('NaN'), 0.3162)),
94+
dict(sys='type2', K=1.0, digits=4, result=(
95+
float('Inf'), 44.4594, float('NaN'), 0.7907)),
96+
dict(sys='type3', K=1.0, digits=3, result=(
97+
0.0626, 37.1748, 0.1119, 0.7951)),
98+
)
99+
100+
101+
# from "A note on the Gain and Phase Margin Concepts
102+
# Journal of Control and Systems Engineering, Yazdan Bavafi-Toosi,
103+
# Dec 2015, vol 3 iss 1, pp 51-59
104+
#
105+
# A cornucopia of tricky systems for phase / gain margin
106+
# Still have to convert more to tests + fix margin to handle
107+
# also these torture cases
108+
"""
109+
% matlab compatible
110+
s = tf('s');
111+
h21 = 0.002*(s+0.02)*(s+0.05)*(s+5)*(s+10)/( ...
112+
(s-0.0005)*(s+0.0001)*(s+0.01)*(s+0.2)*(s+1)*(s+100)^2 );
113+
h23 = ((s+0.1)^2 + 1)*(s-0.1)/( ...
114+
((s+0.1)^2+4)*(s+1) );
115+
h25a = s/(s^2+2*s+2)^4; h25b = h25a*100;
116+
h26a = ((s-0.1)^2 + 1)/( ...
117+
(s + 0.1)*((s-0.2)^2 + 4) ) ;
118+
h26b = ((s-0.1)^2 + 1)/( ...
119+
(s - 0.3)*((s-0.2)^2 + 4) );
120+
"""
121+
self.yazdan = {
122+
'example21' :
123+
0.002*(s+0.02)*(s+0.05)*(s+5)*(s+10)/(
124+
(s-0.0005)*(s+0.0001)*(s+0.01)*(s+0.2)*(s+1)*(s+100)**2 ),
125+
126+
'example23' :
127+
((s+0.1)**2 + 1)*(s-0.1)/(
128+
((s+0.1)**2+4)*(s+1) ),
129+
130+
'example25a' :
131+
s/(s**2+2*s+2)**4,
132+
133+
'example26a' :
134+
((s-0.1)**2 + 1)/(
135+
(s + 0.1)*((s-0.2)**2 + 4) ),
136+
137+
'example26b': ((s-0.1)**2 + 1)/(
138+
(s - 0.3)*((s-0.2)**2 + 4) )
139+
}
140+
self.yazdan['example24'] = self.yazdan['example21']*20000
141+
self.yazdan['example25b'] = self.yazdan['example25a']*100
142+
self.yazdan['example22'] = self.yazdan['example21']*(s**2 - 2*s + 401)
143+
self.ymargin = (
144+
dict(sys='example21', K=1.0, digits=2, result=(
145+
0.0100, -14.5640, 0, 0.0022)),
146+
dict(sys='example21', K=1000.0, digits=2, result=(
147+
0.1793, 22.5215, 0.0243, 0.0630)),
148+
dict(sys='example21', K=5000.0, digits=4, result=(
149+
4.5596, 21.2101, 0.4385, 0.1868)),
150+
)
151+
152+
self.yallmargin = (
153+
dict(sys='example21', K=1.0, result=(
154+
[0.01, 179.2931, 2.2798e+4, 1.5946e+07, 7.2477e+08],
155+
[0, 0.0243, 0.4385, 6.8640, 84.9323],
156+
[-14.5640],
157+
[0.0022]))
158+
)
159+
160+
46161
def test_stability_margins(self):
47162
omega = np.logspace(-2, 2, 2000)
48163
for sys,rgm,rwgm,rpm,rwpm in self.tsys:
@@ -51,22 +166,21 @@ def test_stability_margins(self):
51166
gm, pm, sm, wg, wp, ws = out
52167
outf = np.array(stability_margins(FRD(sys, omega)))
53168
print(out,'\n', outf)
54-
print(out != np.array(None))
55-
np.testing.assert_array_almost_equal(
56-
out[out != np.array(None)],
57-
outf[outf != np.array(None)], 2)
58-
169+
#print(out != np.array(None))
170+
assert_array_almost_equal(
171+
out, outf, 2)
59172
# final one with fixed values
60-
np.testing.assert_array_almost_equal(
173+
assert_array_almost_equal(
61174
[gm, pm, sm, wg, wp, ws],
62175
self.stability_margins4, 3)
63176

64177
def test_margin(self):
65178
gm, pm, wg, wp = margin(self.sys4)
66-
np.testing.assert_array_almost_equal(
179+
assert_array_almost_equal(
67180
[gm, pm, wg, wp],
68181
self.stability_margins4[:2] + self.stability_margins4[3:5], 3)
69182

183+
70184
def test_stability_margins_all(self):
71185
for sys,rgm,rwgm,rpm,rwpm in self.tsys:
72186
out = stability_margins(sys, returnall=True)
@@ -75,25 +189,25 @@ def test_stability_margins_all(self):
75189
for res,comp in zip(out, (rgm,rpm,[],rwgm,rwpm,[])):
76190
if comp:
77191
print(res, '\n', comp)
78-
np.testing.assert_array_almost_equal(
192+
assert_array_almost_equal(
79193
res, comp, 2)
80194

81195
def test_phase_crossover_frequencies(self):
82196
omega, gain = phase_crossover_frequencies(self.sys2)
83-
np.testing.assert_array_almost_equal(omega, [1.73205, 0.])
84-
np.testing.assert_array_almost_equal(gain, [-0.5, 0.25])
197+
assert_array_almost_equal(omega, [1.73205, 0.])
198+
assert_array_almost_equal(gain, [-0.5, 0.25])
85199

86200
tf = TransferFunction([1],[1,1])
87201
omega, gain = phase_crossover_frequencies(tf)
88-
np.testing.assert_array_almost_equal(omega, [0.])
89-
np.testing.assert_array_almost_equal(gain, [1.])
202+
assert_array_almost_equal(omega, [0.])
203+
assert_array_almost_equal(gain, [1.])
90204

91205
# testing MIMO, only (0,0) element is considered
92206
tf = TransferFunction([[[1],[2]],[[3],[4]]],
93207
[[[1, 2, 3, 4],[1,1]],[[1,1],[1,1]]])
94208
omega, gain = phase_crossover_frequencies(tf)
95-
np.testing.assert_array_almost_equal(omega, [1.73205081, 0.])
96-
np.testing.assert_array_almost_equal(gain, [-0.5, 0.25])
209+
assert_array_almost_equal(omega, [1.73205081, 0.])
210+
assert_array_almost_equal(gain, [-0.5, 0.25])
97211

98212
def test_mag_phase_omega(self):
99213
# test for bug reported in gh-58
@@ -106,7 +220,7 @@ def test_mag_phase_omega(self):
106220
ind = [0,1,3,4] # indices of gm, pm, wg, wp -- ignore sm
107221
marg1 = np.array(out)[ind]
108222
marg2 = np.array(out2)[ind]
109-
np.testing.assert_array_almost_equal(marg1, marg2, 4)
223+
assert_array_almost_equal(marg1, marg2, 4)
110224

111225
def test_frd(self):
112226
f = np.array([0.005, 0.010, 0.020, 0.030, 0.040,
@@ -143,7 +257,7 @@ def test_frd(self):
143257
C=K*(1+1.9*s)
144258
TFopen=fresp*C*G
145259
gm, pm, sm, wg, wp, ws = stability_margins(TFopen)
146-
np.testing.assert_array_almost_equal(
260+
assert_array_almost_equal(
147261
[pm], [44.55], 2)
148262

149263
def test_nocross(self):
@@ -153,17 +267,48 @@ def test_nocross(self):
153267
h2 = 3*(10+s)/(2+s)
154268
h3 = 0.01*(10-s)/(2+s)/(1+s)
155269
gm, pm, wm, wg, wp, ws = stability_margins(h1)
156-
self.assertEqual(gm, None)
157-
self.assertEqual(wg, None)
270+
assert_array_almost_equal(
271+
[gm, pm, wg, wp],
272+
[float('Inf'), float('Inf'), float('NaN'), float('NaN')])
158273
gm, pm, wm, wg, wp, ws = stability_margins(h2)
159-
self.assertEqual(pm, None)
274+
self.assertEqual(pm, float('Inf'))
160275
gm, pm, wm, wg, wp, ws = stability_margins(h3)
161-
self.assertEqual(pm, None)
276+
self.assertTrue(np.isnan(wp))
162277
omega = np.logspace(-2,2, 100)
163278
out1b = stability_margins(FRD(h1, omega))
164279
out2b = stability_margins(FRD(h2, omega))
165280
out3b = stability_margins(FRD(h3, omega))
166281

282+
def test_zmore_margin(self):
283+
print("""
284+
warning, Matlab gives different values (0 and 0) for gain
285+
margin of the following system:
286+
{type2!s}
287+
python-control gives inf
288+
difficult to argue which is right? Special case or different
289+
approach?
290+
291+
edge cases, like
292+
{type0!s}
293+
which approaches a gain of 1 for w -> 0, are also not identically
294+
indicated, Matlab gives phase margin -180, at w = 0. for higher or
295+
lower gains, results match
296+
""".format(**self.types))
297+
298+
sdict = self.tmargin[0]
299+
for test in self.tmargin[1:]:
300+
res = margin(sdict[test['sys']]*test['K'])
301+
print("more margin {}\n".format(sdict[test['sys']]),
302+
res, '\n', test['result'])
303+
assert_array_almost_equal(
304+
res, test['result'], test['digits'])
305+
sdict = self.yazdan
306+
for test in self.ymargin:
307+
res = margin(sdict[test['sys']]*test['K'])
308+
print("more margin {}\n".format(sdict[test['sys']]),
309+
res, '\n', test['result'])
310+
assert_array_almost_equal(
311+
res, test['result'], test['digits'])
167312

168313
def test_suite():
169314
return unittest.TestLoader().loadTestsFromTestCase(TestMargin)

‎control/tests/timeresp_test.py‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -228,6 +228,11 @@ def check(u, x0, xtrue):
228228
xtrue = np.array([1./6. * t**3, 0.5 * t**2])
229229
check(u, x0, xtrue)
230230

231+
def test_discrete_initial(self):
232+
h1 = TransferFunction([1.], [1., 0.], 1.)
233+
t, yout = impulse_response(h1, np.arange(4))
234+
np.testing.assert_array_equal(yout[0], [0., 1., 0., 0.])
235+
231236
def suite():
232237
return unittest.TestLoader().loadTestsFromTestCase(TestTimeresp)
233238

0 commit comments

Comments
 (0)