still not perfect

repagh · repagh · commit 17b9d6086adf · 2012-10-25T21:28:03.000Z
diff --git a/src/frdata.py b/src/frdata.py
@@ -75,7 +75,7 @@
 # External function declarations
 from numpy import angle, any, array, empty, finfo, insert, ndarray, ones, \
     polyadd, polymul, polyval, roots, sort, sqrt, zeros, squeeze, inner, \
-    real, imag, matrix, absolute, eye
+    real, imag, matrix, absolute, eye, linalg
 from scipy.interpolate import splprep, splev
 from copy import deepcopy
 from lti import Lti
@@ -339,10 +339,8 @@ def evalfr(self, omega):
 
         for i in range(self.outputs):
             for j in range(self.inputs):
-                print array([1, 1j])
-                print splev(omega, self.ifunc[i,j], der=0)
-                out[i,j] = inner(array([1, 1j]), 
-                                 splev(omega, self.ifunc[i,j], der=0))
+                frraw = splev(omega, self.ifunc[i,j], der=0)
+                out[i,j] = frraw[0] + 1.0j*frraw[1]
 
         return out
 
@@ -366,7 +364,7 @@ def freqresp(self, omega):
         omega.sort()
 
         for k, w in enumerate(omega):
-            fresp = self.evalfr(omega)
+            fresp = self.evalfr(w)
             mag[:, :, k] = abs(fresp)
             phase[:, :, k] = angle(fresp)
 
@@ -386,10 +384,10 @@ def feedback(self, other, sign=-1):
         # TODO: vectorize this
         # TODO: handle omega re-mapping
         for k, w in enumerate(other.omega):
-            fresp[:, :, k] = (
+            fresp[:, :, k] = linalg.solve(
                 eye(self.inputs) + other.fresp[:, :, k].view(type=matrix) * 
-                    self.fresp[:, :, k].view(type=matrix)).I * \
-            self.fresp[:, :, k].view(type=matrix)
+                self.fresp[:, :, k].view(type=matrix), 
+                eye(self.inputs))*self.fresp[:, :, k].view(type=matrix)
             
         #    for i in range(self.inputs):
         #        for j in range(self.outputs):
diff --git a/tests/frd_test.py b/tests/frd_test.py
@@ -130,15 +130,12 @@ def testMIMOfb(self):
         omega = np.logspace(-1, 2, 10)
         f1 = FRD(sys, omega).feedback([[0.1, 0.3],[0.0, 1.0]])
         f2 = FRD(sys.feedback([[0.1, 0.3],[0.0, 1.0]]), omega)
-        print f1 - f2
-        print f1
-        print f2
-        #np.testing.assert_array_almost_equal(
-        #    sys.feedback([[0.1, 0.3],[0.0, 1.0]]).freqresp([0.1, 1.0, 10])[0],
-        #    f1.feedback([[0.1, 0.3],[0.0, 1.0]]).freqresp([0.1, 1.0, 10])[0])
-        #np.testing.assert_array_almost_equal(
-        #    sys.feedback([[0.1, 0.3],[0.0, 1.0]]).freqresp([0.1, 1.0, 10])[1],
-        #    f1.feedback([[0.1, 0.3],[0.0, 1.0]]).freqresp([0.1, 1.0, 10])[1])
+        np.testing.assert_array_almost_equal(
+            f1.freqresp([0.1, 1.0, 10])[0],
+            f2.freqresp([0.1, 1.0, 10])[0])
+        np.testing.assert_array_almost_equal(
+            f1.freqresp([0.1, 1.0, 10])[1],
+            f2.freqresp([0.1, 1.0, 10])[1])
 if __name__ == "__main__":
     unittest.main()
     sys.exit(0)
