Skip to content

Commit cd30a00

Browse files
repaghrepagh
committed
corrected margin calculations; extended test to do all-margin calculations
check margin calculation on analytic tf with calculation on numeric (interpolation with FRD) variant. Extended test cases
1 parent 815ca2d commit cd30a00

2 files changed

Lines changed: 71 additions & 22 deletions

File tree

‎control/margins.py‎

Lines changed: 28 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -153,20 +153,24 @@ def stability_margins(sysdata, returnall=False, epsw=1e-10):
153153
# test (imaginary part of tf) == 0, for phase crossover/gain margins
154154
test_w_180 = np.polyadd(np.polymul(inum, rden), np.polymul(rnum, -iden))
155155
w_180 = np.roots(test_w_180)
156+
#print ('1:w_180', w_180)
156157

157158
# first remove imaginary and negative frequencies, epsw removes the
158159
# "0" frequency for type-2 systems
159160
w_180 = np.real(w_180[(np.imag(w_180) == 0) * (w_180 >= epsw)])
161+
#print ('2:w_180', w_180)
160162

161163
# evaluate response at remaining frequencies, to test for phase 180 vs 0
162164
resp_w_180 = np.real(np.polyval(sys.num[0][0], 1.j*w_180) /
163165
np.polyval(sys.den[0][0], 1.j*w_180))
166+
#print ('resp_w_180', resp_w_180)
164167

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

168171
# and sort
169172
w_180.sort()
173+
#print ('3:w_180', w_180)
170174

171175
# test magnitude is 1 for gain crossover/phase margins
172176
test_wc = np.polysub(np.polyadd(_polysqr(rnum), _polysqr(inum)),
@@ -178,22 +182,26 @@ def stability_margins(sysdata, returnall=False, epsw=1e-10):
178182
# stability margin was a bitch to elaborate, relies on magnitude to
179183
# point -1, then take the derivative. Second derivative needs to be >0
180184
# to have a minimum
181-
# from comparison to numerical one below, this seems to be wrong!
182-
# no one complained so far
183185
test_wstabd = np.polyadd(_polysqr(rden), _polysqr(iden))
184186
test_wstabn = np.polyadd(_polysqr(np.polyadd(rnum,rden)),
185187
_polysqr(np.polyadd(inum,iden)))
186188
test_wstab = np.polysub(
187189
np.polymul(np.polyder(test_wstabn),test_wstabd),
188190
np.polymul(np.polyder(test_wstabd),test_wstabn))
189191

190-
# find the solutions
192+
# find the solutions, for positive omega, and only real ones
191193
wstab = np.roots(test_wstab)
194+
#print('wstabr', wstab)
195+
wstab = np.real(wstab[(np.imag(wstab) == 0) *
196+
(np.real(wstab) >= 0)])
197+
#print('wstab', wstab)
192198

193199
# and find the value of the 2nd derivative there, needs to be positive
194200
wstabplus = np.polyval(np.polyder(test_wstab), wstab)
201+
#print('wstabplus', wstabplus)
195202
wstab = np.real(wstab[(np.imag(wstab) == 0) * (wstab > epsw) *
196-
(np.abs(wstabplus) > 0.)])
203+
(wstabplus > 0.)])
204+
#print('wstab', wstab)
197205
wstab.sort()
198206

199207
else:
@@ -219,23 +227,29 @@ def dstab(w):
219227

220228
# find the phase crossings ang(H(jw) == -180
221229
widx = np.where(np.diff(np.sign(arg(sys.omega))))[0]
230+
#print('widx (180)', widx, sys.omega[widx])
231+
#print('x', sys.evalfr(sys.omega[widx])[0][0])
232+
widx = widx[np.real(sys.evalfr(sys.omega[widx])[0][0]) <= 0]
233+
#print('widx (180,2)', widx)
222234
w_180 = np.array(
223235
[ sp.optimize.brentq(arg, sys.omega[i], sys.omega[i+1])
224236
for i in widx if i+1 < len(sys.omega) ])
237+
#print('x', sys.evalfr(w_180)[0][0])
238+
#print('w_180', w_180)
225239

226240
# find all stab margins?
227241
widx = np.where(np.diff(np.sign(np.diff(dstab(sys.omega)))))[0]
228-
wstab = np.array(
229-
[ sp.optimize.minimize_scalar(
242+
#print('widx', widx)
243+
#print('wstabx', sys.omega[widx])
244+
wstab = np.array([ sp.optimize.minimize_scalar(
230245
dstab, bracket=(sys.omega[i], sys.omega[i+1])).x
231246
for i in widx if i+1 < len(sys.omega) and
232-
np.diff(np.diff(dstab(sys.omega[i-1:i+2])))[0] < 0 ])
233-
print (wstab)
247+
np.diff(np.diff(dstab(sys.omega[i-1:i+2])))[0] > 0 ])
248+
#print('wstabf0', wstab)
249+
wstab = wstab[(wstab >= sys.omega[0]) *
250+
(wstab <= sys.omega[-1])]
251+
#print ('wstabf', wstab)
234252

235-
# there is really only one stab margin; the closest
236-
#res = sp.optimize.minimize_scalar(
237-
# dstab, bracket=(sys.omega[0], sys.omega[-1]))
238-
#wstab = np.array([res.x])
239253

240254
# margins, as iterables, converted frdata and xferfcn calculations to
241255
# vector for this
@@ -252,7 +266,7 @@ def dstab(w):
252266
(SM.shape[0] or None) and np.amin(SM),
253267
(w_180.shape[0] or None) and w_180[GM==np.amin(GM)][0],
254268
(wc.shape[0] or None) and wc[PM==np.amin(PM)][0],
255-
(wstab.shape[0] or None) and wstab[SM==np.amin(SM)])
269+
(wstab.shape[0] or None) and wstab[SM==np.amin(SM)][0])
256270

257271

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

‎control/tests/margin_test.py‎

Lines changed: 43 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,25 @@ class TestMargin(unittest.TestCase):
1515
"""These are tests for the margin commands in margin.py."""
1616

1717
def setUp(self):
18+
# system, gain margin, gm freq, phase margin, pm freq
19+
s = TransferFunction([1, 0], [1])
20+
self.tsys = (
21+
(TransferFunction([1, 2], [1, 2, 3]),
22+
[], [], [], []),
23+
(TransferFunction([1], [1, 2, 3, 4]),
24+
[2.001], [1.7321], [], []),
25+
(StateSpace([[1., 4.], [3., 2.]], [[1.], [-4.]],
26+
[[1., 0.]], [[0.]]),
27+
[], [], [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],
31+
[0.0850, 0.9373, 1.0919]))
32+
33+
1834
self.sys1 = TransferFunction([1, 2], [1, 2, 3])
35+
# alternative
36+
# sys1 = tf([1, 2], [1, 2, 3])
1937
self.sys2 = TransferFunction([1], [1, 2, 3, 4])
2038
self.sys3 = StateSpace([[1., 4.], [3., 2.]], [[1.], [-4.]],
2139
[[1., 0.]], [[0.]])
@@ -24,17 +42,33 @@ def setUp(self):
2442
1./(s**2/(10.**2)+2*0.04*s/10.+1)
2543

2644
def test_stability_margins(self):
27-
omega = np.logspace(-2, 2, 200)
28-
for sys in (self.sys1, self.sys2, self.sys3, self.sys4):
29-
out = stability_margins(sys)
45+
omega = np.logspace(-2, 2, 2000)
46+
for sys,rgm,rwgm,rpm,rwpm in self.tsys:
47+
print(sys)
48+
out = np.array(stability_margins(sys))
3049
gm, pm, sm, wg, wp, ws = out
31-
outf = stability_margins(FRD(sys, omega))
32-
print(sys, out, outf)
33-
np.testing.assert_array_almost_equal(out, outf, 3)
50+
outf = np.array(stability_margins(FRD(sys, omega)))
51+
print(out,'\n', outf)
52+
print(out != np.array(None))
53+
np.testing.assert_array_almost_equal(
54+
out[out != np.array(None)],
55+
outf[outf != np.array(None)], 2)
56+
3457
# final one with fixed values
3558
np.testing.assert_array_almost_equal(
3659
[gm, pm, sm, wg, wp, ws],
37-
[2.2716, 97.5941, 0.9633, 10.0053, 0.0850, 0.4064], 3)
60+
[2.2716, 97.5941, 0.5591, 10.0053, 0.0850, 9.9918], 3)
61+
62+
def test_stability_margins_all(self):
63+
for sys,rgm,rwgm,rpm,rwpm in self.tsys:
64+
out = stability_margins(sys, returnall=True)
65+
gm, pm, sm, wg, wp, ws = out
66+
print(sys)
67+
for res,comp in zip(out, (rgm,rpm,[],rwgm,rwpm,[])):
68+
if comp:
69+
print(res, '\n', comp)
70+
np.testing.assert_array_almost_equal(
71+
res, comp, 2)
3872

3973
def test_phase_crossover_frequencies(self):
4074
omega, gain = phase_crossover_frequencies(self.sys2)
@@ -57,8 +91,9 @@ def test_mag_phase_omega(self):
5791
# test for bug reported in gh-58
5892
sys = TransferFunction(15, [1, 6, 11, 6])
5993
out = stability_margins(sys)
60-
omega = np.logspace(-1,1,100)
94+
omega = np.logspace(-2,2,1000)
6195
mag, phase, omega = sys.freqresp(omega)
96+
#print( mag, phase, omega)
6297
out2 = stability_margins((mag, phase*180/np.pi, omega))
6398
ind = [0,1,3,4] # indices of gm, pm, wg, wp -- ignore sm
6499
marg1 = np.array(out)[ind]

0 commit comments

Comments
 (0)