From 65232705d49fcfd9e8451b227b6f0fd5c3f113f0 Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Wed, 8 Jul 2015 01:10:46 +0200 Subject: [PATCH 1/7] Keep the "smooth" parameter when combining FRD functions with other systems --- control/frdata.py | 37 ++++++++++++++++++++++++------------- 1 file changed, 24 insertions(+), 13 deletions(-) diff --git a/control/frdata.py b/control/frdata.py index bb4b7fbf2..a7130e35f 100644 --- a/control/frdata.py +++ b/control/frdata.py @@ -220,7 +220,8 @@ def __mul__(self, other): # Convert the second argument to a transfer function. if isinstance(other, (int, float, complex)): - return FRD(self.fresp * other, self.omega) + return FRD(self.fresp * other, self.omega, + smooth=(self.ifunc is not None)) else: other = _convertToFRD(other, omega=self.omega) @@ -236,14 +237,17 @@ def __mul__(self, other): dtype=self.fresp.dtype) for i in range(len(self.omega)): fresp[:,:,i] = dot(self.fresp[:,:,i], other.fresp[:,:,i]) - return FRD(fresp, self.omega) + return FRD(fresp, self.omega, + smooth=(self.ifunc is not None) and + (other.ifunc is not None)) def __rmul__(self, other): """Right Multiply two LTI objects (serial connection).""" # Convert the second argument to an frd function. if isinstance(other, (int, float, complex)): - return FRD(self.fresp * other, self.omega) + return FRD(self.fresp * other, self.omega, + smooth=(self.ifunc is not None)) else: other = _convertToFRD(other, omega=self.omega) @@ -260,14 +264,17 @@ def __rmul__(self, other): dtype=self.fresp.dtype) for i in range(len(self.omega)): fresp[:,:,i] = dot(other.fresp[:,:,i], self.fresp[:,:,i]) - return FRD(fresp, self.omega) + return FRD(fresp, self.omega, + smooth=(self.ifunc is not None) and + (other.ifunc is not None)) # TODO: Division of MIMO transfer function objects is not written yet. def __truediv__(self, other): """Divide two LTI objects.""" if isinstance(other, (int, float, complex)): - return FRD(self.fresp * (1/other), self.omega) + return FRD(self.fresp * (1/other), self.omega, + smooth=(self.ifunc is not None)) else: other = _convertToFRD(other, omega=self.omega) @@ -277,7 +284,9 @@ def __truediv__(self, other): raise NotImplementedError( "FRD.__truediv__ is currently implemented only for SISO systems.") - return FRD(self.fresp/other.fresp, self.omega) + return FRD(self.fresp/other.fresp, self.omega, + smooth=(self.ifunc is not None) and + (other.ifunc is not None)) # TODO: Remove when transition to python3 complete def __div__(self, other): @@ -287,7 +296,8 @@ def __div__(self, other): def __rtruediv__(self, other): """Right divide two LTI objects.""" if isinstance(other, (int, float, complex)): - return FRD(other / self.fresp, self.omega) + return FRD(other / self.fresp, self.omega, + smooth=(self.ifunc is not None)) else: other = _convertToFRD(other, omega=self.omega) @@ -306,7 +316,8 @@ def __pow__(self,other): if not type(other) == int: raise ValueError("Exponent must be an integer") if other == 0: - return FRD(ones(self.fresp.shape),self.omega) #unity + return FRD(ones(self.fresp.shape),self.omega, + smooth=(self.ifunc is not None)) #unity if other > 0: return self * (self**(other-1)) if other < 0: @@ -338,7 +349,7 @@ def evalfr(self, omega): except: raise ValueError( "Frequency %f not in frequency list, try an interpolating" - " FRD if you want additional points") + " FRD if you want additional points" % omega) else: if getattr(omega, '__iter__', False): for i in range(self.outputs): @@ -401,7 +412,7 @@ def feedback(self, other=1, sign=-1): self.fresp[:, :, k].view(type=matrix), eye(self.inputs)) - return FRD(fresp, other.omega) + return FRD(fresp, other.omega, smooth=(self.ifunc is not None)) def _convertToFRD(sys, omega, inputs=1, outputs=1): """Convert a system to frequency response data form (if needed). @@ -436,11 +447,11 @@ def _convertToFRD(sys, omega, inputs=1, outputs=1): for k, w in enumerate(omega): fresp[:, :, k] = sys.evalfr(w) - return FRD(fresp, omega) + return FRD(fresp, omega, smooth=True) elif isinstance(sys, (int, float, complex)): fresp = ones((outputs, inputs, len(omega)), dtype=float)*sys - return FRD(fresp, omega) + return FRD(fresp, omega, smooth=True) # try converting constant matrices try: @@ -450,7 +461,7 @@ def _convertToFRD(sys, omega, inputs=1, outputs=1): for i in range(outputs): for j in range(inputs): fresp[i,j,:] = sys[i,j] - return FRD(fresp, omega) + return FRD(fresp, omega, smooth=True) except: pass From ae0910469a3c6288313f12c0ec9c6949f54d70d1 Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Wed, 8 Jul 2015 01:12:09 +0200 Subject: [PATCH 2/7] Fix the margin calculation for FRD/data based margins. TODO: verify the stability margin --- control/margins.py | 50 +++++++++++++++++++---------- control/tests/margin_test.py | 61 +++++++++++++++++++++++++++++++++++- 2 files changed, 94 insertions(+), 17 deletions(-) diff --git a/control/margins.py b/control/margins.py index f775efc83..2ffbc9745 100644 --- a/control/margins.py +++ b/control/margins.py @@ -84,6 +84,9 @@ def _polysqr(pol): # # RvP, July 8, 2014, corrected to exclude phase=0 crossing for the gain # margin polynomial +# RvP, July 8, 2015, augmented to calculate all phase/gain crossings with +# frd data. Correct to return smallest phase +# margin, smallest gain margin and their frequencies def stability_margins(sysdata, returnall=False, epsw=1e-10): """Calculate stability margins and associated crossover frequencies. @@ -175,6 +178,8 @@ def stability_margins(sysdata, returnall=False, epsw=1e-10): # stability margin was a bitch to elaborate, relies on magnitude to # point -1, then take the derivative. Second derivative needs to be >0 # to have a minimum + # from comparison to numerical one below, this seems to be wrong! + # no one complained so far test_wstabn = np.polyadd(_polysqr(rnum), _polysqr(inum)) test_wstabd = np.polyadd(_polysqr(np.polyadd(rnum,rden)), _polysqr(np.polyadd(inum,iden))) @@ -195,37 +200,50 @@ def stability_margins(sysdata, returnall=False, epsw=1e-10): # a bit coarse, have the interpolated frd evaluated again def mod(w): """to give the function to calculate |G(jw)| = 1""" - return [np.abs(sys.evalfr(w[0])[0][0]) - 1] + return np.abs(sys.evalfr(w)[0][0]) - 1 def arg(w): """function to calculate the phase angle at -180 deg""" - return [np.angle(sys.evalfr(w[0])[0][0]) + np.pi] + return np.angle(-sys.evalfr(w)[0][0]) def dstab(w): """function to calculate the distance from -1 point""" - return np.abs(sys.evalfr(w[0])[0][0] + 1.) - - # how to calculate the frequency at which |G(jw)| = 1 - wc = np.array([sp.optimize.fsolve(mod, sys.omega[0])])[0] - w_180 = np.array([sp.optimize.fsolve(arg, sys.omega[0])])[0] - wstab = np.real( - np.array([sp.optimize.fmin(dstab, sys.omega[0], disp=0)])[0]) + return np.abs(sys.evalfr(w)[0][0] + 1.) + + # Find all crossings, note that this depends on omega having + # a correct range + widx = np.where(np.diff(np.sign(mod(sys.omega))))[0] + wc = np.array( + [ sp.optimize.brentq(mod, sys.omega[i], sys.omega[i+1]) + for i in widx if i+1 < len(sys.omega)]) + + # find the phase crossings ang(H(jw) == -180 + widx = np.where(np.diff(np.sign(arg(sys.omega))))[0] + w_180 = np.array( + [ sp.optimize.brentq(arg, sys.omega[i], sys.omega[i+1]) + for i in widx if i+1 < len(sys.omega) ]) + + # there is really only one stab margin; the closest + res = sp.optimize.minimize_scalar( + dstab, bracket=(sys.omega[0], sys.omega[-1])) + wstab = np.array([res.x]) # margins, as iterables, converted frdata and xferfcn calculations to # vector for this - PM = np.angle(sys.evalfr(wc)[0][0], deg=True) + 180 GM = 1/(np.abs(sys.evalfr(w_180)[0][0])) - SM = np.abs(sys.evalfr(wstab)[0][0]+1) - + print(wstab) + SM = 1/np.abs(sys.evalfr(wstab)[0][0]+1) + PM = np.angle(sys.evalfr(wc)[0][0], deg=True) + 180 + if returnall: return GM, PM, SM, w_180, wc, wstab else: return ( - (GM.shape[0] or None) and GM[0], - (PM.shape[0] or None) and PM[0], + (GM.shape[0] or None) and GM[GM==np.min(GM)][0], + (PM.shape[0] or None) and PM[PM==np.min(PM)][0], (SM.shape[0] or None) and SM[0], - (w_180.shape[0] or None) and w_180[0], - (wc.shape[0] or None) and wc[0], + (w_180.shape[0] or None) and w_180[GM==np.min(GM)][0], + (wc.shape[0] or None) and wc[PM==np.min(PM)][0], (wstab.shape[0] or None) and wstab[0]) diff --git a/control/tests/margin_test.py b/control/tests/margin_test.py index 267ee66ab..26a9bfbe8 100644 --- a/control/tests/margin_test.py +++ b/control/tests/margin_test.py @@ -6,6 +6,7 @@ import unittest import numpy as np from control.xferfcn import TransferFunction +from control.frdata import FRD from control.statesp import StateSpace from control.margins import * @@ -28,7 +29,7 @@ def test_stability_margins(self): gm, pm, sm, wg, wp, ws = stability_margins(self.sys4); np.testing.assert_array_almost_equal( [gm, pm, sm, wg, wp, ws], - [2.2716, 97.5941, 1.0454, 10.0053, 0.0850, 0.4973], 3) + [2.2716, 97.5941, 0.9565, 10.0053, 0.0850, 0.4973], 3) def test_phase_crossover_frequencies(self): omega, gain = phase_crossover_frequencies(self.sys2) @@ -59,7 +60,65 @@ def test_mag_phase_omega(self): marg2 = np.array(out2)[ind] np.testing.assert_array_almost_equal(marg1, marg2, 4) + def test_frd(self): + f = np.array([0.005, 0.010, 0.020, 0.030, 0.040, + 0.050, 0.060, 0.070, 0.080, 0.090, + 0.100, 0.200, 0.300, 0.400, 0.500, + 0.750, 1.000, 1.250, 1.500, 1.750, + 2.000, 2.250, 2.500, 2.750, 3.000, + 3.250, 3.500, 3.750, 4.000, 4.250, + 4.500, 4.750, 5.000, 6.000, 7.000, + 8.000, 9.000, 10.000 ]) + gain = np.array([ 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.1, 0.2, 0.3, 0.5, + 0.5, -0.4, -2.3, -4.8, -7.3, + -9.6, -11.7, -13.6, -15.3, -16.9, + -18.3, -19.6, -20.8, -22.0, -23.1, + -24.1, -25.0, -25.9, -29.1, -31.9, + -34.2, -36.2, -38.1 ]) + phase = np.array([ 0, -1, -2, -3, -4, + -5, -6, -7, -8, -9, + -10, -19, -29, -40, -51, + -81, -114, -144, -168, -187, + -202, -214, -224, -233, -240, + -247, -253, -259, -264, -269, + -273, -277, -280, -292, -301, + -307, -313, -317 ]) + # calculate response as complex number + resp = 10**(gain / 20) * np.exp(1j * phase / (180./np.pi)) + # frequency response data + fresp = FRD(resp, f*2*np.pi, smooth=True) + s=TransferFunction([1,0],[1]) + G=1./(s**2) + K=1. + C=K*(1+1.9*s) + TFopen=fresp*C*G + gm, pm, sm, wg, wp, ws = stability_margins(TFopen) + print gm + np.testing.assert_array_almost_equal( + [pm], [44.55], 2) + + def test_nocross(self): + # what happens when no gain/phase crossover? + s = TransferFunction([1, 0], [1]) + h1 = 1/(1+s) + h2 = 3*(10+s)/(2+s) + h3 = 0.01*(10-s)/(2+s)/(1+s) + gm, pm, sm, wg, wp, ws = stability_margins(h1) + self.assertEqual(gm, None) + self.assertEqual(wg, None) + gm, pm, wm, wg, wp, ws = stability_margins(h2) + self.assertEqual(pm, None) + gm, pm, wm, wg, wp, ws = stability_margins(h3) + self.assertEqual(pm, None) + omega = np.logspace(-2,2, 100) + out1b = stability_margins(FRD(h1, omega)) + out2b = stability_margins(FRD(h2, omega)) + out3b = stability_margins(FRD(h3, omega)) + + def test_suite(): return unittest.TestLoader().loadTestsFromTestCase(TestMargin) From 7eefe8b7a54b2f5aae19fc02b062a7d648677550 Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Wed, 8 Jul 2015 14:56:00 +0200 Subject: [PATCH 3/7] corrected the stability margin calculations --- control/margins.py | 23 +++++++++++------------ control/tests/margin_test.py | 21 ++++++++++++--------- 2 files changed, 23 insertions(+), 21 deletions(-) diff --git a/control/margins.py b/control/margins.py index 2ffbc9745..3a95847e6 100644 --- a/control/margins.py +++ b/control/margins.py @@ -150,7 +150,7 @@ def stability_margins(sysdata, returnall=False, epsw=1e-10): rnum, inum = _polyimsplit(sys.num[0][0]) rden, iden = _polyimsplit(sys.den[0][0]) - # test imaginary part of tf == 0, for phase crossover/gain margins + # test (imaginary part of tf) == 0, for phase crossover/gain margins test_w_180 = np.polyadd(np.polymul(inum, rden), np.polymul(rnum, -iden)) w_180 = np.roots(test_w_180) @@ -180,8 +180,8 @@ def stability_margins(sysdata, returnall=False, epsw=1e-10): # to have a minimum # from comparison to numerical one below, this seems to be wrong! # no one complained so far - test_wstabn = np.polyadd(_polysqr(rnum), _polysqr(inum)) - test_wstabd = np.polyadd(_polysqr(np.polyadd(rnum,rden)), + test_wstabd = np.polyadd(_polysqr(rden), _polysqr(iden)) + test_wstabn = np.polyadd(_polysqr(np.polyadd(rnum,rden)), _polysqr(np.polyadd(inum,iden))) test_wstab = np.polysub( np.polymul(np.polyder(test_wstabn),test_wstabd), @@ -230,21 +230,20 @@ def dstab(w): # margins, as iterables, converted frdata and xferfcn calculations to # vector for this - GM = 1/(np.abs(sys.evalfr(w_180)[0][0])) - print(wstab) - SM = 1/np.abs(sys.evalfr(wstab)[0][0]+1) + GM = 1/np.abs(sys.evalfr(w_180)[0][0]) + SM = np.abs(sys.evalfr(wstab)[0][0]+1) PM = np.angle(sys.evalfr(wc)[0][0], deg=True) + 180 if returnall: return GM, PM, SM, w_180, wc, wstab else: return ( - (GM.shape[0] or None) and GM[GM==np.min(GM)][0], - (PM.shape[0] or None) and PM[PM==np.min(PM)][0], - (SM.shape[0] or None) and SM[0], - (w_180.shape[0] or None) and w_180[GM==np.min(GM)][0], - (wc.shape[0] or None) and wc[PM==np.min(PM)][0], - (wstab.shape[0] or None) and wstab[0]) + (GM.shape[0] or None) and np.amin(GM), + (PM.shape[0] or None) and np.amin(PM), + (SM.shape[0] or None) and np.amin(SM), + (w_180.shape[0] or None) and w_180[GM==np.amin(GM)][0], + (wc.shape[0] or None) and wc[PM==np.amin(PM)][0], + (wstab.shape[0] or None) and wstab[SM==np.amin(SM)]) # Contributed by Steffen Waldherr diff --git a/control/tests/margin_test.py b/control/tests/margin_test.py index 26a9bfbe8..9132128da 100644 --- a/control/tests/margin_test.py +++ b/control/tests/margin_test.py @@ -3,6 +3,7 @@ # margin_test.py - test suit for stability margin commands # RMM, 15 Jul 2011 +from __future__ import print_function import unittest import numpy as np from control.xferfcn import TransferFunction @@ -23,14 +24,18 @@ def setUp(self): 1./(s**2/(10.**2)+2*0.04*s/10.+1) def test_stability_margins(self): - gm, pm, sm, wg, wp, ws = stability_margins(self.sys1); - gm, pm, sm, wg, wp, ws = stability_margins(self.sys2); - gm, pm, sm, wg, wp, ws = stability_margins(self.sys3); - gm, pm, sm, wg, wp, ws = stability_margins(self.sys4); + omega = np.logspace(-2, 2, 200) + for sys in (self.sys1, self.sys2, self.sys3, self.sys4): + out = stability_margins(sys) + gm, pm, sm, wg, wp, ws = out + outf = stability_margins(FRD(sys, omega)) + print(sys, out, outf) + np.testing.assert_array_almost_equal(out, outf, 3) + # final one with fixed values np.testing.assert_array_almost_equal( [gm, pm, sm, wg, wp, ws], - [2.2716, 97.5941, 0.9565, 10.0053, 0.0850, 0.4973], 3) - + [2.2716, 97.5941, 0.9633, 10.0053, 0.0850, 0.4064], 3) + def test_phase_crossover_frequencies(self): omega, gain = phase_crossover_frequencies(self.sys2) np.testing.assert_array_almost_equal(omega, [1.73205, 0.]) @@ -95,7 +100,6 @@ def test_frd(self): C=K*(1+1.9*s) TFopen=fresp*C*G gm, pm, sm, wg, wp, ws = stability_margins(TFopen) - print gm np.testing.assert_array_almost_equal( [pm], [44.55], 2) @@ -105,7 +109,7 @@ def test_nocross(self): h1 = 1/(1+s) h2 = 3*(10+s)/(2+s) h3 = 0.01*(10-s)/(2+s)/(1+s) - gm, pm, sm, wg, wp, ws = stability_margins(h1) + gm, pm, wm, wg, wp, ws = stability_margins(h1) self.assertEqual(gm, None) self.assertEqual(wg, None) gm, pm, wm, wg, wp, ws = stability_margins(h2) @@ -117,7 +121,6 @@ def test_nocross(self): out2b = stability_margins(FRD(h2, omega)) out3b = stability_margins(FRD(h3, omega)) - def test_suite(): return unittest.TestLoader().loadTestsFromTestCase(TestMargin) From f309232856af6353dfe1ef751cb63c1d20b1d940 Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Wed, 8 Jul 2015 23:55:03 +0200 Subject: [PATCH 4/7] still imperfect --- control/margins.py | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/control/margins.py b/control/margins.py index 3a95847e6..1dd284a5e 100644 --- a/control/margins.py +++ b/control/margins.py @@ -223,10 +223,18 @@ def dstab(w): [ sp.optimize.brentq(arg, sys.omega[i], sys.omega[i+1]) for i in widx if i+1 < len(sys.omega) ]) + # find all stab margins? + widx = np.where(np.diff(np.sign(np.diff(dstab(sys.omega)))))[0] + wstab = np.array( + [ sp.optimize.minimize_scalar( + dstab, bracket=(sys.omega[i], sys.omega[i+1])) + for i in widx if i+1 < len(sys.omega) and + np.diff(np.diff(dstab(sys.omega[i-1:i+2])))[0] < 0 ]) + # there is really only one stab margin; the closest - res = sp.optimize.minimize_scalar( - dstab, bracket=(sys.omega[0], sys.omega[-1])) - wstab = np.array([res.x]) + #res = sp.optimize.minimize_scalar( + # dstab, bracket=(sys.omega[0], sys.omega[-1])) + #wstab = np.array([res.x]) # margins, as iterables, converted frdata and xferfcn calculations to # vector for this From 815ca2d75f6eb41491751c908727f9703bc22e6c Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Thu, 9 Jul 2015 22:12:00 +0200 Subject: [PATCH 5/7] further attempts at fixing --- control/margins.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/control/margins.py b/control/margins.py index 1dd284a5e..6fc9a6e94 100644 --- a/control/margins.py +++ b/control/margins.py @@ -130,8 +130,8 @@ def stability_margins(sysdata, returnall=False, epsw=1e-10): sys = sysdata elif getattr(sysdata, '__iter__', False) and len(sysdata) == 3: mag, phase, omega = sysdata - sys = frdata.FRD(mag * np.exp(1j * phase * np.pi/180), omega, - smooth=True) + sys = frdata.FRD(mag * np.exp(1j * phase * np.pi/180), + omega, smooth=True) else: sys = xferfcn._convertToTransferFunction(sysdata) except Exception as e: @@ -227,9 +227,10 @@ def dstab(w): widx = np.where(np.diff(np.sign(np.diff(dstab(sys.omega)))))[0] wstab = np.array( [ sp.optimize.minimize_scalar( - dstab, bracket=(sys.omega[i], sys.omega[i+1])) + dstab, bracket=(sys.omega[i], sys.omega[i+1])).x for i in widx if i+1 < len(sys.omega) and np.diff(np.diff(dstab(sys.omega[i-1:i+2])))[0] < 0 ]) + print (wstab) # there is really only one stab margin; the closest #res = sp.optimize.minimize_scalar( From cd30a00d3e59291f9bf90de013a61140ccc0d7ac Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Sat, 11 Jul 2015 14:51:15 +0200 Subject: [PATCH 6/7] 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 --- control/margins.py | 42 +++++++++++++++++++---------- control/tests/margin_test.py | 51 ++++++++++++++++++++++++++++++------ 2 files changed, 71 insertions(+), 22 deletions(-) diff --git a/control/margins.py b/control/margins.py index 6fc9a6e94..4ce4c6f85 100644 --- a/control/margins.py +++ b/control/margins.py @@ -153,20 +153,24 @@ def stability_margins(sysdata, returnall=False, epsw=1e-10): # test (imaginary part of tf) == 0, for phase crossover/gain margins test_w_180 = np.polyadd(np.polymul(inum, rden), np.polymul(rnum, -iden)) w_180 = np.roots(test_w_180) + #print ('1:w_180', w_180) # first remove imaginary and negative frequencies, epsw removes the # "0" frequency for type-2 systems w_180 = np.real(w_180[(np.imag(w_180) == 0) * (w_180 >= epsw)]) + #print ('2:w_180', w_180) # evaluate response at remaining frequencies, to test for phase 180 vs 0 resp_w_180 = np.real(np.polyval(sys.num[0][0], 1.j*w_180) / np.polyval(sys.den[0][0], 1.j*w_180)) + #print ('resp_w_180', resp_w_180) # only keep frequencies where the negative real axis is crossed - w_180 = w_180[(resp_w_180 < 0.0)] + w_180 = w_180[np.real(resp_w_180) < 0.0] # and sort w_180.sort() + #print ('3:w_180', w_180) # test magnitude is 1 for gain crossover/phase margins test_wc = np.polysub(np.polyadd(_polysqr(rnum), _polysqr(inum)), @@ -178,8 +182,6 @@ def stability_margins(sysdata, returnall=False, epsw=1e-10): # stability margin was a bitch to elaborate, relies on magnitude to # point -1, then take the derivative. Second derivative needs to be >0 # to have a minimum - # from comparison to numerical one below, this seems to be wrong! - # no one complained so far test_wstabd = np.polyadd(_polysqr(rden), _polysqr(iden)) test_wstabn = np.polyadd(_polysqr(np.polyadd(rnum,rden)), _polysqr(np.polyadd(inum,iden))) @@ -187,13 +189,19 @@ def stability_margins(sysdata, returnall=False, epsw=1e-10): np.polymul(np.polyder(test_wstabn),test_wstabd), np.polymul(np.polyder(test_wstabd),test_wstabn)) - # find the solutions + # find the solutions, for positive omega, and only real ones wstab = np.roots(test_wstab) + #print('wstabr', wstab) + wstab = np.real(wstab[(np.imag(wstab) == 0) * + (np.real(wstab) >= 0)]) + #print('wstab', wstab) # and find the value of the 2nd derivative there, needs to be positive wstabplus = np.polyval(np.polyder(test_wstab), wstab) + #print('wstabplus', wstabplus) wstab = np.real(wstab[(np.imag(wstab) == 0) * (wstab > epsw) * - (np.abs(wstabplus) > 0.)]) + (wstabplus > 0.)]) + #print('wstab', wstab) wstab.sort() else: @@ -219,23 +227,29 @@ def dstab(w): # find the phase crossings ang(H(jw) == -180 widx = np.where(np.diff(np.sign(arg(sys.omega))))[0] + #print('widx (180)', widx, sys.omega[widx]) + #print('x', sys.evalfr(sys.omega[widx])[0][0]) + widx = widx[np.real(sys.evalfr(sys.omega[widx])[0][0]) <= 0] + #print('widx (180,2)', widx) w_180 = np.array( [ sp.optimize.brentq(arg, sys.omega[i], sys.omega[i+1]) for i in widx if i+1 < len(sys.omega) ]) + #print('x', sys.evalfr(w_180)[0][0]) + #print('w_180', w_180) # find all stab margins? widx = np.where(np.diff(np.sign(np.diff(dstab(sys.omega)))))[0] - wstab = np.array( - [ sp.optimize.minimize_scalar( + #print('widx', widx) + #print('wstabx', sys.omega[widx]) + wstab = np.array([ sp.optimize.minimize_scalar( dstab, bracket=(sys.omega[i], sys.omega[i+1])).x for i in widx if i+1 < len(sys.omega) and - np.diff(np.diff(dstab(sys.omega[i-1:i+2])))[0] < 0 ]) - print (wstab) + np.diff(np.diff(dstab(sys.omega[i-1:i+2])))[0] > 0 ]) + #print('wstabf0', wstab) + wstab = wstab[(wstab >= sys.omega[0]) * + (wstab <= sys.omega[-1])] + #print ('wstabf', wstab) - # there is really only one stab margin; the closest - #res = sp.optimize.minimize_scalar( - # dstab, bracket=(sys.omega[0], sys.omega[-1])) - #wstab = np.array([res.x]) # margins, as iterables, converted frdata and xferfcn calculations to # vector for this @@ -252,7 +266,7 @@ def dstab(w): (SM.shape[0] or None) and np.amin(SM), (w_180.shape[0] or None) and w_180[GM==np.amin(GM)][0], (wc.shape[0] or None) and wc[PM==np.amin(PM)][0], - (wstab.shape[0] or None) and wstab[SM==np.amin(SM)]) + (wstab.shape[0] or None) and wstab[SM==np.amin(SM)][0]) # Contributed by Steffen Waldherr diff --git a/control/tests/margin_test.py b/control/tests/margin_test.py index 9132128da..86fc7e753 100644 --- a/control/tests/margin_test.py +++ b/control/tests/margin_test.py @@ -15,7 +15,25 @@ class TestMargin(unittest.TestCase): """These are tests for the margin commands in margin.py.""" def setUp(self): + # system, gain margin, gm freq, phase margin, pm freq + s = TransferFunction([1, 0], [1]) + self.tsys = ( + (TransferFunction([1, 2], [1, 2, 3]), + [], [], [], []), + (TransferFunction([1], [1, 2, 3, 4]), + [2.001], [1.7321], [], []), + (StateSpace([[1., 4.], [3., 2.]], [[1.], [-4.]], + [[1., 0.]], [[0.]]), + [], [], [147.0743], [2.5483]), + ((8.75*(4*s**2+0.4*s+1))/((100*s+1)*(s**2+0.22*s+1)) * + 1./(s**2/(10.**2)+2*0.04*s/10.+1), + [2.2716], [10.0053], [97.5941, 360-157.7904, 134.7359], + [0.0850, 0.9373, 1.0919])) + + self.sys1 = TransferFunction([1, 2], [1, 2, 3]) + # alternative + # sys1 = tf([1, 2], [1, 2, 3]) self.sys2 = TransferFunction([1], [1, 2, 3, 4]) self.sys3 = StateSpace([[1., 4.], [3., 2.]], [[1.], [-4.]], [[1., 0.]], [[0.]]) @@ -24,17 +42,33 @@ def setUp(self): 1./(s**2/(10.**2)+2*0.04*s/10.+1) def test_stability_margins(self): - omega = np.logspace(-2, 2, 200) - for sys in (self.sys1, self.sys2, self.sys3, self.sys4): - out = stability_margins(sys) + omega = np.logspace(-2, 2, 2000) + for sys,rgm,rwgm,rpm,rwpm in self.tsys: + print(sys) + out = np.array(stability_margins(sys)) gm, pm, sm, wg, wp, ws = out - outf = stability_margins(FRD(sys, omega)) - print(sys, out, outf) - np.testing.assert_array_almost_equal(out, outf, 3) + outf = np.array(stability_margins(FRD(sys, omega))) + print(out,'\n', outf) + print(out != np.array(None)) + np.testing.assert_array_almost_equal( + out[out != np.array(None)], + outf[outf != np.array(None)], 2) + # final one with fixed values np.testing.assert_array_almost_equal( [gm, pm, sm, wg, wp, ws], - [2.2716, 97.5941, 0.9633, 10.0053, 0.0850, 0.4064], 3) + [2.2716, 97.5941, 0.5591, 10.0053, 0.0850, 9.9918], 3) + + def test_stability_margins_all(self): + for sys,rgm,rwgm,rpm,rwpm in self.tsys: + out = stability_margins(sys, returnall=True) + gm, pm, sm, wg, wp, ws = out + print(sys) + for res,comp in zip(out, (rgm,rpm,[],rwgm,rwpm,[])): + if comp: + print(res, '\n', comp) + np.testing.assert_array_almost_equal( + res, comp, 2) def test_phase_crossover_frequencies(self): omega, gain = phase_crossover_frequencies(self.sys2) @@ -57,8 +91,9 @@ def test_mag_phase_omega(self): # test for bug reported in gh-58 sys = TransferFunction(15, [1, 6, 11, 6]) out = stability_margins(sys) - omega = np.logspace(-1,1,100) + omega = np.logspace(-2,2,1000) mag, phase, omega = sys.freqresp(omega) + #print( mag, phase, omega) out2 = stability_margins((mag, phase*180/np.pi, omega)) ind = [0,1,3,4] # indices of gm, pm, wg, wp -- ignore sm marg1 = np.array(out)[ind] From ebde9e4536b3f2d0c1e494bdc3b0584f393e762e Mon Sep 17 00:00:00 2001 From: Rene van Paassen Date: Sat, 11 Jul 2015 19:39:37 +0200 Subject: [PATCH 7/7] need small fix to doc in stability_margins; needed 1e-8 as cutoff for 0-frequency --- control/margins.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/margins.py b/control/margins.py index e6c4b92e6..30cf82110 100644 --- a/control/margins.py +++ b/control/margins.py @@ -104,7 +104,7 @@ def stability_margins(sysdata, returnall=False, epsw=1e-8): minimum stability margins. For frequency data or FRD systems, only one margin is found and returned. epsw: float, optional - Frequencies below this value (default 1e-10) are considered static gain, + Frequencies below this value (default 1e-8) are considered static gain, and not returned as margin. Returns