improving the margin calculation & choice of which margin to

repagh · repagh · commit 6caad9a1a39e · 2017-06-10T12:11:23.000+02:00
present, as much as practical in line with Matlab and
&gt; A note on the Gain and Phase Margin Concepts
&gt; Journal of Control and Systems Engineering, Yazdan Bavafi-Toosi,
&gt; Dec 205, vol 3 iss 1, pp 51-59
diff --git a/control/margins.py b/control/margins.py
@@ -157,13 +157,15 @@ def stability_margins(sysdata, returnall=False, epsw=0.0):
 
         # 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)])
+        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))
-        
+        with np.errstate(all='ignore'):
+            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))
+
         # only keep frequencies where the negative real axis is crossed
         w_180 = w_180[np.real(resp_w_180) <= 0.0]
 
@@ -252,20 +254,27 @@ def dstab(w):
 
     # margins, as iterables, converted frdata and xferfcn calculations to
     # vector for this
-    gain_w_180 = np.abs(sys.evalfr(w_180)[0][0])
-    GM = np.where(gain_w_180 == 0.0, float('inf'), 1.0/gain_w_180)
+    with np.errstate(all='ignore'):
+        gain_w_180 = np.abs(sys.evalfr(w_180)[0][0])
+        GM = 1.0/gain_w_180
     SM = np.abs(sys.evalfr(wstab)[0][0]+1)
     PM = np.remainder(np.angle(sys.evalfr(wc)[0][0], deg=True), 360.0) - 180.0
     
     if returnall:
         return GM, PM, SM, w_180, wc, wstab
     else:
+        if GM.shape[0]:
+            with np.errstate(all='ignore'):
+                gmidx = np.where(np.abs(np.log(GM)) == 
+                                 np.min(np.abs(np.log(GM))))
+        if PM.shape[0]:
+            pmidx = np.where(np.abs(PM) == np.amin(np.abs(PM)))[0]
         return (
-            (not GM.shape[0] and float('inf')) or np.amin(GM),
-            (not PM.shape[0] and float('inf')) or np.amin(PM),
+            (not GM.shape[0] and float('inf')) or GM[gmidx][0],
+            (not PM.shape[0] and float('inf')) or PM[pmidx][0],
             (not SM.shape[0] and float('inf')) or np.amin(SM),
-            (not w_180.shape[0] and float('nan')) or w_180[GM==np.amin(GM)][0],
-            (not wc.shape[0] and float('nan')) or wc[PM==np.amin(PM)][0],
+            (not w_180.shape[0] and float('nan')) or w_180[gmidx][0],
+            (not wc.shape[0] and float('nan')) or wc[pmidx][0],
             (not wstab.shape[0] and float('nan')) or wstab[SM==np.amin(SM)][0])
 
 
diff --git a/control/tests/margin_test.py b/control/tests/margin_test.py
@@ -85,7 +85,8 @@ def setUp(self):
             dict(sys='type3', K=1.0, digits=3, result=(
                 0.0626, 37.1748, 0.1119, 0.7951)),
             )
-          
+        
+        
         # from "A note on the Gain and Phase Margin Concepts
         # Journal of Control and Systems Engineering, Yazdan Bavafi-Toosi,
         # Dec 205, vol 3 iss 1, pp 51-59
@@ -129,8 +130,13 @@ def setUp(self):
         self.yazdan['example25b'] = self.yazdan['example25a']*100
         self.yazdan['example22'] = self.yazdan['example21']*(s**2 - 2*s + 401)
         self.ymargin = (
-            dict(sys='example21', K=1.0, result=(
-                0.01, 345.43, 0., 0, 0, 0.01)))
+            dict(sys='example21', K=1.0, digits=2, result=(
+                0.0100,  -14.5640,         0,    0.0022)),
+            dict(sys='example21', K=1000.0, digits=2, result=(
+                0.1793, 22.5215, 0.0243, 0.0630)),
+            dict(sys='example21', K=5000.0, digits=4, result=(
+                4.5596, 21.2101, 0.4385, 0.1868)),
+                    )
 
         self.yallmargin = (
             dict(sys='example21', K=1.0, result=(
@@ -264,15 +270,29 @@ def test_nocross(self):
         out3b = stability_margins(FRD(h3, omega))
 
     def test_zmore_margin(self):
+        print("""
+        warning, Matlab gives different values (0 and 0) for gain
+        margin of the following system:
+        {:s}
+        python-control gives inf
+        difficult to argue which is right? Special case or different
+        approach?
+        """.format(str(self.types['type2'])))
+                
         sdict = self.tmargin[0]
         for test in self.tmargin[1:]:
             res = margin(sdict[test['sys']]*test['K'])
             print("more margin {}\n".format(sdict[test['sys']]),
                   res, '\n', test['result'])
             assert_array_almost_equal(
                 res, test['result'], test['digits'])
-            
-        
+        sdict = self.yazdan
+        for test in self.ymargin:
+            res = margin(sdict[test['sys']]*test['K'])
+            print("more margin {}\n".format(sdict[test['sys']]),
+                  res, '\n', test['result'])
+            assert_array_almost_equal(
+                res, test['result'], test['digits'])
         
 def test_suite():
     return unittest.TestLoader().loadTestsFromTestCase(TestMargin)
