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

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 self.A.shape[0] != self.A.shape[1]:

raise NotImplementedError("StateSpace.zero only supports systems "

"with square A matrices.")

# 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")

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):