working on matrix feedback with state-space and frd

repagh · repagh · commit ee0bd9c1f2c2 · 2012-10-24T16:01:01.000Z
diff --git a/src/frdata.py b/src/frdata.py
@@ -389,7 +389,8 @@ def feedback(self, other, sign=-1):
                 for j in range(self.outputs):
                     fresp[i, j, k] = \
                         self.fresp[i, j, k] / \
-                        (1.0-sign*inner(self.fresp[:, j, k], other.fresp[i, :, k]))
+                        (1.0-sign*inner(self.fresp[:, j, k], 
+                                        other.fresp[i, :, k]))
 
         return FRD(fresp, other.omega)
  
diff --git a/src/statesp.py b/src/statesp.py
@@ -80,6 +80,7 @@
 from numpy.linalg import inv, det, solve
 from numpy.linalg.linalg import LinAlgError
 from scipy.signal import lti
+from exceptions import Exception
 import warnings
 from lti import Lti
 import xferfcn
@@ -505,8 +506,18 @@ def _convertToStateSpace(sys, **kw):
         #   return StateSpace([[]], [[]], [[]], eye(outputs, inputs))
         return StateSpace(0., zeros((1, inputs)), zeros((outputs, 1)), 
             sys * ones((outputs, inputs)))
-    else:
-        raise TypeError("Can't convert given type to StateSpace system.")
+
+    # If this is a matrix, try to create a constant feedthrough
+    try:
+        D = matrix(sys)
+        outputs, inputs = D.shape
+        
+        return StateSpace(0., zeros((1, inputs)), zeros((outputs, 1)), D)
+    except Exception, e: 
+        print "Failure to assume argument is matrix-like in" \
+            " _convertToStateSpace, result %s" % e
+        
+    raise TypeError("Can't convert given type to StateSpace system.")
     
 def _rss_generate(states, inputs, outputs, type):
     """Generate a random state space.
diff --git a/tests/frd_test.py b/tests/frd_test.py
@@ -98,15 +98,47 @@ def testAuto(self):
         f2 = _convertToFRD(np.matrix([[1, 0], [0.1, -1]]), omega)
         f2 = _convertToFRD([[1, 0], [0.1, -1]], omega)
 
-    def testBode(self):
+    def testNyquist(self):
         h1 = TransferFunction([1], [1, 2, 2])
         omega = np.logspace(-1, 2, 40)
         f1 = FRD(h1, omega)
         control.freqplot.nyquist(f1, np.logspace(-1, 2, 100))
+        plt.savefig('/dev/null', format='svg')
         plt.figure(2)
         control.freqplot.nyquist(f1, f1.omega)
-        plt.show()
+        plt.savefig('/dev/null', format='svg')
 
+    def testMIMO(self):
+        sys = StateSpace([[-0.5, 0.0], [0.0, -1.0]], 
+                         [[1.0, 0.0], [0.0, 1.0]], 
+                         [[1.0, 0.0], [0.0, 1.0]], 
+                         [[0.0, 0.0], [0.0, 0.0]])
+        omega = np.logspace(-1, 2, 10)
+        f1 = FRD(sys, omega)
+        np.testing.assert_array_almost_equal(
+            sys.freqresp([0.1, 1.0, 10])[0],
+            f1.freqresp([0.1, 1.0, 10])[0])
+        np.testing.assert_array_almost_equal(
+            sys.freqresp([0.1, 1.0, 10])[1],
+            f1.freqresp([0.1, 1.0, 10])[1])
+
+    def testMIMOfb(self):
+        sys = StateSpace([[-0.5, 0.0], [0.0, -1.0]], 
+                         [[1.0, 0.0], [0.0, 1.0]], 
+                         [[1.0, 0.0], [0.0, 1.0]], 
+                         [[0.0, 0.0], [0.0, 0.0]])
+        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])
 if __name__ == "__main__":
     unittest.main()
     sys.exit(0)
