Merge pull request python-control#68 from repagh/master

murrayrm · murrayrm · commit c14b880b66a4 · 2016-05-30T09:27:59.000-07:00
Fixes to margin calculation
diff --git 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
 
diff --git a/control/margins.py b/control/margins.py
@@ -84,7 +84,10 @@ def _polysqr(pol):
 #
 # RvP, July 8, 2014, corrected to exclude phase=0 crossing for the gain
 #                    margin polynomial
-def stability_margins(sysdata, returnall=False, epsw=1e-10):
+# 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-8):
     """Calculate stability margins and associated crossover frequencies.
 
     Parameters
@@ -101,7 +104,7 @@ def stability_margins(sysdata, returnall=False, epsw=1e-10):
         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
@@ -127,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:
@@ -147,23 +150,27 @@ 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)
+        #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)),
@@ -175,58 +182,91 @@ 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
-        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),
             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:
         # 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]
+        #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]
+        #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('wstabf0', wstab)
+        wstab = wstab[(wstab >= sys.omega[0]) * 
+                      (wstab <= sys.omega[-1])]
+        #print ('wstabf', wstab)
+        
 
     # 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]))
+    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[0],
-            (PM.shape[0] or None) and 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],
-            (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)][0])
 
 
 # Contributed by Steffen Waldherr <waldherr@ist.uni-stuttgart.de>
@@ -291,11 +331,12 @@ def margin(*args):
     Returns
     -------
     gm, pm, Wcg, Wcp : float
-        Gain margin gm, phase margin pm (in deg), gain crossover frequency
-        (corresponding to phase margin) and phase crossover frequency
-        (corresponding to gain margin), in rad/sec of SISO open-loop.
-        If more than one crossover frequency is detected, returns the lowest
-        corresponding margin.
+        Gain margin gm, phase margin pm (in deg), phase crossover frequency
+        (corresponding to gain margin, where phase=-180) and gain crossover
+        frequency (corresponding to phase margin, where gain is 0dB),
+        in rad/sec of SISO open-loop.
+        If more than one crossover frequency is detected for gain or phase,
+        this returns one with the lowest corresponding margin.
 
     Examples
     --------
diff --git a/control/tests/margin_test.py b/control/tests/margin_test.py
