Correction to the gain margin calculation. Old implementation also returned

repagh · repagh · commit 1d4dd64a2cdb · 2014-07-08T10:57:18.000Z
gains where arg(H) == 0. 
Switched around the w_180 and wc return parameters, to match the order
of gain margin (matching w_180) and phase margin (matching wc) return.
diff --git a/src/margins.py b/src/margins.py
@@ -80,6 +80,9 @@ def _polysqr(pol):
 # idea for the frequency data solution copied/adapted from
 # https://github.com/alchemyst/Skogestad-Python/blob/master/BODE.py
 # Rene van Paassen <rene.vanpaassen@gmail.com>
+#
+# RvP, July 8, 2014, corrected to exclude phase=0 crossing for the gain
+#                    margin polynomial
 def stability_margins(sysdata, deg=True, returnall=False, epsw=1e-10):
     """Calculate gain, phase and stability margins and associated
     crossover frequencies.
@@ -147,7 +150,19 @@ def stability_margins(sysdata, deg=True, 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)
-        w_180 = np.real(w_180[(np.imag(w_180) == 0) * (w_180 > epsw)])
+
+        # 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)])
+
+        # 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))
+
+        # only keep frequencies where the negative real axis is crossed
+        w_180 = w_180[(resp_w_180 < 0.0)]
+
+        # and sort
         w_180.sort()
 
         # test magnitude is 1 for gain crossover/phase margins
@@ -203,14 +218,14 @@ def dstab(w):
     SM = np.abs(sys.evalfr(wstab)[0][0]+1)
 
     if returnall:
-        return GM, PM, SM, wc, w_180, wstab
+        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], 
             (SM.shape[0] or None) and SM[0], 
-            (wc.shape[0] or None) and wc[0],
             (w_180.shape[0] or None) and w_180[0],
+            (wc.shape[0] or None) and wc[0],
             (wstab.shape[0] or None) and wstab[0])
 
 
diff --git a/tests/margin_test.py b/tests/margin_test.py
@@ -17,11 +17,18 @@ def setUp(self):
         self.sys2 = TransferFunction([1], [1, 2, 3, 4])
         self.sys3 = StateSpace([[1., 4.], [3., 2.]], [[1.], [-4.]],
             [[1., 0.]], [[0.]])
+        s = TransferFunction([1, 0], [1])
+        self.sys4 = (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)
 
     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);
+        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) 
 
     def test_phase_crossover_frequencies(self):
         omega, gain = phase_crossover_frequencies(self.sys2)
