From 6e6507058851656e0608422459fdce612a104dd7 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 27 May 2018 16:22:08 -0700 Subject: [PATCH] StateSpace.zero() now supports MIMO systems 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(). --- control/statesp.py | 46 ++++++++++++++++++++++++++--------- control/tests/statesp_test.py | 33 ++++++++++++++++++++----- 2 files changed, 61 insertions(+), 18 deletions(-) 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."""