StateSpace.zero() now supports MIMO systems

calcmogul · calcmogul · commit 8607fb0b52f1 · 2018-05-27T19:02:47.000-07:00
scipy.linalg.eigvals() is used to solve the generalized eigenvalue
problem. The zero locations used in the unit test were obtained via the
zero() function in MATLAB R2017b with two more digits of precision added
based on the results of StateSpace.zero().
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,22 @@ 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.")
+        # 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`.
+        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")
 
-        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)
+        if self.states:
+            return np.array([x for x in sp.linalg.eigvals(L, M,
+                                                          overwrite_a=True)
+                             if not isinf(x)])
+        else:
+            return np.array([])
 
     # 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,12 @@ 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."""
 
-        sys = StateSpace(self.sys1.A, [[3.], [-2.], [4.]], [[-1., 3., 2.]], [[-4.]])
-        z = sys.zero()
+        z = np.sort(self.sys1.zero())
+        true_z = np.sort([44.41465, -0.490252, -5.924398])
 
-        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."""
