partial success with frd. Still need to investigate whether interpolation

repagh · repagh · commit 10e6fe1d6524 · 2012-10-22T16:44:08.000Z
should be fixed
diff --git a/src/frdata.py b/src/frdata.py
@@ -74,8 +74,9 @@
 
 # External function declarations
 from numpy import angle, any, array, empty, finfo, insert, ndarray, ones, \
-    polyadd, polymul, polyval, roots, sort, sqrt, zeros, squeeze, inner
-from scipy.interpolate import interp1d
+    polyadd, polymul, polyval, roots, sort, sqrt, zeros, squeeze, inner, \
+    real, imag, matrix, absolute
+from scipy.interpolate import splprep, splev
 from copy import deepcopy
 from lti import Lti
 import statesp
@@ -84,7 +85,7 @@ class FRD(Lti):
 
     """The FRD class represents (measured?) frequency response 
     TF instances and functions.
-    4
+    
     The FRD class is derived from the Lti parent class.  It is used
     throughout the python-control library to represent systems in frequency
     response data form. 
@@ -127,7 +128,6 @@ def __init__(self, *args):
                 # not an FRD, but still a system, second argument should be
                 # the frequency range
                 otherlti = args[0]
-                print args[1]
                 self.omega = array(args[1], dtype=float)
                 self.omega.sort()
                 numfreq = len(self.omega)
@@ -166,10 +166,13 @@ def __init__(self, *args):
 
         # create interpolation functions
         self.ifunc = empty((self.fresp.shape[1], self.fresp.shape[0]), 
-                           dtype=interp1d)
+                           dtype=tuple)
         for i in range(self.fresp.shape[1]):
             for j in range(self.fresp.shape[0]):
-                self.ifunc[i,j] = interp1d(self.omega, self.fresp[i, j, :])
+                self.ifunc[i,j],u = splprep(
+                    u=self.omega, x=[real(self.fresp[i, j, :]), 
+                                     imag(self.fresp[i, j, :])], 
+                    w=1.0/(absolute(self.fresp[i, j, :])+0.001), s=0.01)
 
         Lti.__init__(self, self.fresp.shape[1], self.fresp.shape[0])
         
@@ -179,14 +182,15 @@ def __str__(self):
         mimo = self.inputs > 1 or self.outputs > 1  
         outstr = [ 'frequency response data ' ]
         
+        mt, pt, wt = self.freqresp(self.omega)
         for i in range(self.inputs):
             for j in range(self.outputs):
                 if mimo:
                     outstr.append("Input %i to output %i:" % (i + 1, j + 1))
                 outstr.append('Freq [rad/s]  Magnitude   Phase')
                 outstr.extend(
                     [ '%f %f %f' % (w, m, p*180.0)
-                      for m, p, w in self.freqresp(self.omega) ])
+                      for m, p, w in zip(mt[i][j], pt[i][j], wt) ])
 
         return '\n'.join(outstr)
     
@@ -335,7 +339,8 @@ def evalfr(self, omega):
 
         for i in range(self.outputs):
             for j in range(self.inputs):
-                out[i,j] = self.ifunc[i,j](omega)
+                out[i,j] = inner(array([1, 1j]), 
+                                 splev(omega, self.ifunc[i,j], der=0))
 
         return out
 
@@ -360,7 +365,7 @@ def freqresp(self, omega):
 
         for i in range(self.outputs):
             for j in range(self.inputs):
-                fresp = self.ifunc[i,j](omega)
+                fresp = matrix([[1,1j]])*splev(omega, self.ifunc[i,j]) 
                 mag[i, j, :] = abs(fresp)
                 phase[i, j, :] = angle(fresp)
 
@@ -375,18 +380,18 @@ def feedback(self, other, sign=-1):
             self.inputs != other.outputs):
             raise ValueError(
                 "FRD.feedback, inputs/outputs mismatch")
-        fresp = empty(self.outputs, self.inputs, len(other.omega))
+        fresp = empty((self.outputs, self.inputs, len(other.omega)), 
+                      dtype=complex)
         # TODO: vectorize this
         # TODO: handle omega re-mapping
         for k, w in enumerate(other.omega):
-            for i in range(0, self.inputs):
-                for j in range(0, self.outputs):
+            for i in range(self.inputs):
+                for j in range(self.outputs):
                     fresp[i, j, k] = \
                         self.fresp[i, j, k] / \
-                        (1.0 + inner(self.fresp[i, :, k], other.fresp[:, i, k]))
+                        (1.0-sign*inner(self.fresp[:, j, k], other.fresp[i, :, k]))
 
         return FRD(fresp, other.omega)
-
  
 def _convertToFRD(sys, omega, inputs=1, outputs=1):
     """Convert a system to frequency response data form (if needed).
@@ -442,9 +447,20 @@ def _convertToFRD(sys, omega, inputs=1, outputs=1):
         return FRD(fresp, omega)
 
     elif isinstance(sys, (int, long, float, complex)):
-        
         fresp = ones((outputs, inputs, len(omega)), dtype=float)*sys
-        
         return FRD(fresp, omega)
-    else:
-        raise TypeError("Can't convert given type to FRD system.")
+
+    # try converting constant matrices
+    try:
+        sys = array(sys)
+        outputs,inputs = sys.shape
+        fresp = empty((outputs, inputs, len(omega)), dtype=float)
+        for i in range(outputs):
+            for j in range(inputs):
+                fresp[i,j,:] = sys[i,j]
+        return FRD(fresp, omega)
+    except:
+        pass
+
+    raise TypeError('''Can't convert given type "%s" to FRD system.''' %
+                    sys.__class__)
diff --git a/src/freqplot.py b/src/freqplot.py
@@ -353,9 +353,12 @@ def default_frequency_range(syslist):
     if (not getattr(syslist, '__iter__', False)):
         syslist = (syslist,)
     for sys in syslist:
-        # Add new features to the list
-        features = np.concatenate((features, np.abs(sys.pole())))
-        features = np.concatenate((features, np.abs(sys.zero())))
+        try:
+            # Add new features to the list
+            features = np.concatenate((features, np.abs(sys.pole())))
+            features = np.concatenate((features, np.abs(sys.zero())))
+        except:
+            pass
 
     # Get rid of poles and zeros at the origin
     features = features[features != 0];
diff --git a/tests/frd_test.py b/tests/frd_test.py
@@ -7,8 +7,11 @@
 import unittest
 import numpy as np
 from control.statesp import StateSpace
-from control.xferfcn import TransferFunction, _convertToTransferFunction
-from control.frdata import FRD
+from control.xferfcn import TransferFunction
+from control.frdata import FRD, _convertToFRD
+from control.matlab import bode
+import control.freqplot
+import matplotlib.pyplot as plt
 
 class TestFRD(unittest.TestCase):
     """These are tests for functionality and correct reporting of the
@@ -54,7 +57,57 @@ def testOperators(self):
             (f1 - f2).freqresp([0.1, 1.0, 10])[1],
             (h1 - h2).freqresp([0.1, 1.0, 10])[1])
         
+    def testOperatorsTf(self):
+        # get two SISO transfer functions
+        h1 = TransferFunction([1], [1, 2, 2])
+        h2 = TransferFunction([1], [0.1, 1])
+        omega = np.logspace(-1, 2, 10)
+        f1 = FRD(h1, omega)
+        f2 = FRD(h2, omega)
+
+        np.testing.assert_array_almost_equal(
+            (f1 + h2).freqresp([0.1, 1.0, 10])[0],
+            (h1 + h2).freqresp([0.1, 1.0, 10])[0])
+        np.testing.assert_array_almost_equal(
+            (f1 + h2).freqresp([0.1, 1.0, 10])[1],
+            (h1 + h2).freqresp([0.1, 1.0, 10])[1])
+        np.testing.assert_array_almost_equal(
+            (f1 - h2).freqresp([0.1, 1.0, 10])[0],
+            (h1 - h2).freqresp([0.1, 1.0, 10])[0])
+        np.testing.assert_array_almost_equal(
+            (f1 - h2).freqresp([0.1, 1.0, 10])[1],
+            (h1 - h2).freqresp([0.1, 1.0, 10])[1])
+        # the reverse does not work
+        
+    def testFeedback(self):
+        h1 = TransferFunction([1], [1, 2, 2])
+        omega = np.logspace(-1, 2, 10)
+        f1 = FRD(h1, omega)
+        np.testing.assert_array_almost_equal(
+            f1.feedback(1).freqresp([0.1, 1.0, 10])[0],
+            h1.feedback(1).freqresp([0.1, 1.0, 10])[0])
+        
+    def testFeedback2(self):
+        h2 = StateSpace([[-1.0, 0], [0, -2.0]], [[0.4], [0.1]],
+                        [[1.0, 0], [0, 1]], [[0.0], [0.0]])
+        #h2.feedback([[0.3, 0.2],[0.1, 0.1]])
+    
+    def testAuto(self):
+        omega = np.logspace(-1, 2, 10)
+        f1 = _convertToFRD(1, omega)
+        f2 = _convertToFRD(np.matrix([[1, 0], [0.1, -1]]), omega)
+        f2 = _convertToFRD([[1, 0], [0.1, -1]], omega)
 
+    def testBode(self):
+        h1 = TransferFunction([1], [1, 2, 2])
+        omega = np.logspace(-1, 2, 10)
+        f1 = FRD(h1, omega)
+        control.freqplot.nyquist(f1, np.logspace(-1, 2, 100))
+        plt.figure(2)
+        control.freqplot.nyquist(f1, f1.omega)
+        plt.show()
 
 if __name__ == "__main__":
     unittest.main()
+    sys.exit(0)
+
