diff --git a/control/statesp.py b/control/statesp.py index bff14d241..8f7861434 100644 --- 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 index 5f2d7d244..1677f6afe 100644 --- 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."""