Fix the margin calculation for FRD/data based margins.

repagh · repagh · commit ae0910469a3c · 2015-07-08T01:12:09.000+02:00
TODO: verify the stability margin
diff --git 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
@@ -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)
 
