Skip to content

Commit 7eefe8b

Browse files
repaghrepagh
committed
corrected the stability margin calculations
1 parent ae09104 commit 7eefe8b

2 files changed

Lines changed: 23 additions & 21 deletions

File tree

‎control/margins.py‎

Lines changed: 11 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -150,7 +150,7 @@ def stability_margins(sysdata, returnall=False, epsw=1e-10):
150150
rnum, inum = _polyimsplit(sys.num[0][0])
151151
rden, iden = _polyimsplit(sys.den[0][0])
152152

153-
# test imaginary part of tf == 0, for phase crossover/gain margins
153+
# 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)
156156

@@ -180,8 +180,8 @@ def stability_margins(sysdata, returnall=False, epsw=1e-10):
180180
# to have a minimum
181181
# from comparison to numerical one below, this seems to be wrong!
182182
# no one complained so far
183-
test_wstabn = np.polyadd(_polysqr(rnum), _polysqr(inum))
184-
test_wstabd = np.polyadd(_polysqr(np.polyadd(rnum,rden)),
183+
test_wstabd = np.polyadd(_polysqr(rden), _polysqr(iden))
184+
test_wstabn = np.polyadd(_polysqr(np.polyadd(rnum,rden)),
185185
_polysqr(np.polyadd(inum,iden)))
186186
test_wstab = np.polysub(
187187
np.polymul(np.polyder(test_wstabn),test_wstabd),
@@ -230,21 +230,20 @@ def dstab(w):
230230

231231
# margins, as iterables, converted frdata and xferfcn calculations to
232232
# vector for this
233-
GM = 1/(np.abs(sys.evalfr(w_180)[0][0]))
234-
print(wstab)
235-
SM = 1/np.abs(sys.evalfr(wstab)[0][0]+1)
233+
GM = 1/np.abs(sys.evalfr(w_180)[0][0])
234+
SM = np.abs(sys.evalfr(wstab)[0][0]+1)
236235
PM = np.angle(sys.evalfr(wc)[0][0], deg=True) + 180
237236

238237
if returnall:
239238
return GM, PM, SM, w_180, wc, wstab
240239
else:
241240
return (
242-
(GM.shape[0] or None) and GM[GM==np.min(GM)][0],
243-
(PM.shape[0] or None) and PM[PM==np.min(PM)][0],
244-
(SM.shape[0] or None) and SM[0],
245-
(w_180.shape[0] or None) and w_180[GM==np.min(GM)][0],
246-
(wc.shape[0] or None) and wc[PM==np.min(PM)][0],
247-
(wstab.shape[0] or None) and wstab[0])
241+
(GM.shape[0] or None) and np.amin(GM),
242+
(PM.shape[0] or None) and np.amin(PM),
243+
(SM.shape[0] or None) and np.amin(SM),
244+
(w_180.shape[0] or None) and w_180[GM==np.amin(GM)][0],
245+
(wc.shape[0] or None) and wc[PM==np.amin(PM)][0],
246+
(wstab.shape[0] or None) and wstab[SM==np.amin(SM)])
248247

249248

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

‎control/tests/margin_test.py‎

Lines changed: 12 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
# margin_test.py - test suit for stability margin commands
44
# RMM, 15 Jul 2011
55

6+
from __future__ import print_function
67
import unittest
78
import numpy as np
89
from control.xferfcn import TransferFunction
@@ -23,14 +24,18 @@ def setUp(self):
2324
1./(s**2/(10.**2)+2*0.04*s/10.+1)
2425

2526
def test_stability_margins(self):
26-
gm, pm, sm, wg, wp, ws = stability_margins(self.sys1);
27-
gm, pm, sm, wg, wp, ws = stability_margins(self.sys2);
28-
gm, pm, sm, wg, wp, ws = stability_margins(self.sys3);
29-
gm, pm, sm, wg, wp, ws = stability_margins(self.sys4);
27+
omega = np.logspace(-2, 2, 200)
28+
for sys in (self.sys1, self.sys2, self.sys3, self.sys4):
29+
out = stability_margins(sys)
30+
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)
34+
# final one with fixed values
3035
np.testing.assert_array_almost_equal(
3136
[gm, pm, sm, wg, wp, ws],
32-
[2.2716, 97.5941, 0.9565, 10.0053, 0.0850, 0.4973], 3)
33-
37+
[2.2716, 97.5941, 0.9633, 10.0053, 0.0850, 0.4064], 3)
38+
3439
def test_phase_crossover_frequencies(self):
3540
omega, gain = phase_crossover_frequencies(self.sys2)
3641
np.testing.assert_array_almost_equal(omega, [1.73205, 0.])
@@ -95,7 +100,6 @@ def test_frd(self):
95100
C=K*(1+1.9*s)
96101
TFopen=fresp*C*G
97102
gm, pm, sm, wg, wp, ws = stability_margins(TFopen)
98-
print gm
99103
np.testing.assert_array_almost_equal(
100104
[pm], [44.55], 2)
101105

@@ -105,7 +109,7 @@ def test_nocross(self):
105109
h1 = 1/(1+s)
106110
h2 = 3*(10+s)/(2+s)
107111
h3 = 0.01*(10-s)/(2+s)/(1+s)
108-
gm, pm, sm, wg, wp, ws = stability_margins(h1)
112+
gm, pm, wm, wg, wp, ws = stability_margins(h1)
109113
self.assertEqual(gm, None)
110114
self.assertEqual(wg, None)
111115
gm, pm, wm, wg, wp, ws = stability_margins(h2)
@@ -117,7 +121,6 @@ def test_nocross(self):
117121
out2b = stability_margins(FRD(h2, omega))
118122
out3b = stability_margins(FRD(h3, omega))
119123

120-
121124

122125
def test_suite():
123126
return unittest.TestLoader().loadTestsFromTestCase(TestMargin)

0 commit comments

Comments
 (0)