Merge pull request #205 from calcmogul/mimo-zeros

murrayrm · web-flow · commit 115223e67a4d · 2018-07-01T19:03:37.000-07:00
StateSpace.zero() now supports MIMO systems
diff --git a/control/statesp.py b/control/statesp.py
@@ -54,8 +54,7 @@
 import math
 import numpy as np
 from numpy import all, angle, any, array, asarray, concatenate, cos, delete, \
-    dot, empty, exp, eye, matrix, ones, poly, poly1d, roots, shape, sin, \
-    zeros, squeeze
+    dot, empty, exp, eye, isinf, matrix, ones, pad, shape, sin, zeros, squeeze
 from numpy.random import rand, randn
 from numpy.linalg import solve, eigvals, matrix_rank
 from numpy.linalg.linalg import LinAlgError
@@ -516,17 +515,40 @@ def pole(self):
     def zero(self):
         """Compute the zeros of a state space system."""
 
-        if self.inputs > 1 or self.outputs > 1:
-            raise NotImplementedError("StateSpace.zeros is currently \
-implemented only for SISO systems.")
+        if not self.states:
+            return np.array([])
 
-        den = poly1d(poly(self.A))
-        # Compute the numerator based on zeros
-        #! TODO: This is currently limited to SISO systems
-        num = poly1d(poly(self.A - dot(self.B, self.C)) + ((self.D[0, 0] - 1) *
-            den))
-
-        return roots(num)
+        # Use AB08ND from Slycot if it's available, otherwise use
+        # scipy.lingalg.eigvals().
+        try:
+            from slycot import ab08nd
+
+            out = ab08nd(self.A.shape[0], self.B.shape[1], self.C.shape[0],
+                         self.A, self.B, self.C, self.D)
+            nu = out[0]
+            return sp.linalg.eigvals(out[8][0:nu,0:nu], out[9][0:nu,0:nu])
+        except ImportError:  # Slycot unavailable. Fall back to scipy.
+            if self.C.shape[0] != self.D.shape[1]:
+                raise NotImplementedError("StateSpace.zero only supports "
+                                          "systems with the same number of "
+                                          "inputs as outputs.")
+
+            # This implements the QZ algorithm for finding transmission zeros
+            # from
+            # https://dspace.mit.edu/bitstream/handle/1721.1/841/P-0802-06587335.pdf.
+            # The QZ algorithm solves the generalized eigenvalue problem: given
+            # `L = [A, B; C, D]` and `M = [I_nxn 0]`, find all finite λ for
+            # which there exist nontrivial solutions of the equation `Lz - λMz`.
+            #
+            # The generalized eigenvalue problem is only solvable if its
+            # arguments are square matrices.
+            L = concatenate((concatenate((self.A, self.B), axis=1),
+                             concatenate((self.C, self.D), axis=1)), axis=0)
+            M = pad(eye(self.A.shape[0]), ((0, self.C.shape[0]),
+                                           (0, self.B.shape[1])), "constant")
+            return np.array([x for x in sp.linalg.eigvals(L, M,
+                                                          overwrite_a=True)
+                             if not isinf(x)])
 
     # Feedback around a state space system
     def feedback(self, other=1, sign=-1):
diff --git a/control/tests/statesp_test.py b/control/tests/statesp_test.py
@@ -43,14 +43,35 @@ def testPole(self):
         np.testing.assert_array_almost_equal(p, true_p)
 
     def testZero(self):
-        """Evaluate the zeros of a SISO system."""
+        """Evaluate the zeros of a MIMO system."""
+
+        z = np.sort(self.sys1.zero())
+        true_z = np.sort([44.41465, -0.490252, -5.924398])
+
+        np.testing.assert_array_almost_equal(z, true_z)
+
+        A = np.array([[1, 0, 0, 0, 0, 0],
+                      [0, 1, 0, 0, 0, 0],
+                      [0, 0, 3, 0, 0, 0],
+                      [0, 0, 0,-4, 0, 0],
+                      [0, 0, 0, 0,-1, 0],
+                      [0, 0, 0, 0, 0, 3]])
+        B = np.array([[0,-1],
+                      [-1,0],
+                      [1,-1],
+                      [0, 0],
+                      [0, 1],
+                      [-1,-1]])
+        C = np.array([[1, 0, 0, 1, 0, 0],
+                      [0, 1, 0, 1, 0, 1],
+                      [0, 0, 1, 0, 0, 1]])
+        D = np.zeros((3,2))
+        sys = StateSpace(A, B, C, D)
 
-        sys = StateSpace(self.sys1.A, [[3.], [-2.], [4.]], [[-1., 3., 2.]], [[-4.]])
-        z = sys.zero()
+        z = np.sort(sys.zero())
+        true_z = np.sort([2., -1.])
 
-        np.testing.assert_array_almost_equal(z, [4.26864638637134,
-            -3.75932319318567 + 1.10087776649554j,
-            -3.75932319318567 - 1.10087776649554j])
+        np.testing.assert_array_almost_equal(z, true_z)
 
     def testAdd(self):
         """Add two MIMO systems."""
