"""Wrapper around numpy.matrix that reshapes empty matrices to be 0x0

from numpy import matrix

am = matrix(a, dtype=float)

if (1, 0) == am.shape:

am.shape = (0, 0)

"""StateSpace(A, B, C, D[, dt])

def __init__(self, *args):

"""

if len(args) == 4:

# The user provided A, B, C, and D matrices.

(A, B, C, D) = args

dt = None

elif len(args) == 5:

# Discrete time system

(A, B, C, D, dt) = args

elif len(args) == 1:

# Use the copy constructor.

if not isinstance(args[0], StateSpace):

raise TypeError("The one-argument constructor can only take in a StateSpace "

"object. Received %s." % type(args[0]))

A = args[0].A

B = args[0].B

C = args[0].C

D = args[0].D

try:

dt = args[0].dt

except NameError:

dt = None

else:

raise ValueError("Needs 1 or 4 arguments; received %i." % len(args))

A, B, C, D = [_matrix(M) for M in (A, B, C, D)]

# TODO: use super here?

LTI.__init__(self, inputs=D.shape[1], outputs=D.shape[0], dt=dt)

self.A = A

self.B = B

self.C = C

self.D = D

self.states = A.shape[1]

if 0 == self.states:

# static gain

# matrix's default "empty" shape is 1x0

A.shape = (0,0)

B.shape = (0,self.inputs)

C.shape = (self.outputs,0)

# Check that the matrix sizes are consistent.

if self.states != A.shape[0]:

raise ValueError("A must be square.")

if self.states != B.shape[0]:

raise ValueError("A and B must have the same number of rows.")

if self.states != C.shape[1]:

raise ValueError("A and C must have the same number of columns.")

if self.inputs != B.shape[1]:

raise ValueError("B and D must have the same number of columns.")

if self.outputs != C.shape[0]:

raise ValueError("C and D must have the same number of rows.")

# Check for states that don't do anything, and remove them.

self._remove_useless_states()

def _remove_useless_states(self):

"""Check for states that don't do anything, and remove them.

# Search for useless states and get the indices of these states

# as an array.

ax1_A = np.where(~self.A.any(axis=1))[0]

ax1_B = np.where(~self.B.any(axis=1))[0]

ax0_A = np.where(~self.A.any(axis=0))[1]

ax0_C = np.where(~self.C.any(axis=0))[1]

useless_1 = np.intersect1d(ax1_A, ax1_B, assume_unique=True)

useless_2 = np.intersect1d(ax0_A, ax0_C, assume_unique=True)

useless = np.union1d(useless_1, useless_2)

# Remove the useless states.

self.A = delete(self.A, useless, 0)

self.A = delete(self.A, useless, 1)

self.B = delete(self.B, useless, 0)

self.C = delete(self.C, useless, 1)

self.states = self.A.shape[0]

self.inputs = self.B.shape[1]

self.outputs = self.C.shape[0]

def __str__(self):

"""String representation of the state space."""

str = "A = " + self.A.__str__() + "\n\n"

str += "B = " + self.B.__str__() + "\n\n"

str += "C = " + self.C.__str__() + "\n\n"

str += "D = " + self.D.__str__() + "\n"

# TODO: replace with standard calls to lti functions

if (type(self.dt) == bool and self.dt is True):

str += "\ndt unspecified\n"

elif (not (self.dt is None) and type(self.dt) != bool and self.dt > 0):

str += "\ndt = " + self.dt.__str__() + "\n"

return str

# represent as string, makes display work for IPython

__repr__ = __str__

# Negation of a system

def __neg__(self):

"""Negate a state space system."""

return StateSpace(self.A, self.B, -self.C, -self.D, self.dt)

# Addition of two state space systems (parallel interconnection)

def __add__(self, other):

"""Add two LTI systems (parallel connection)."""

# Check for a couple of special cases

if isinstance(other, (int, float, complex, np.number)):

# Just adding a scalar; put it in the D matrix

A, B, C = self.A, self.B, self.C

D = self.D + other

dt = self.dt

else:

other = _convertToStateSpace(other)

# Check to make sure the dimensions are OK

if ((self.inputs != other.inputs) or

(self.outputs != other.outputs)):

raise ValueError("Systems have different shapes.")

# Figure out the sampling time to use

if self.dt is None and other.dt is not None:

dt = other.dt # use dt from second argument

elif (other.dt is None and self.dt is not None) or \

(timebaseEqual(self, other)):

dt = self.dt # use dt from first argument

else:

raise ValueError("Systems have different sampling times")

# Concatenate the various arrays

A = concatenate((

concatenate((self.A, zeros((self.A.shape[0],

other.A.shape[-1]))),axis=1),

concatenate((zeros((other.A.shape[0], self.A.shape[-1])),

other.A),axis=1)

),axis=0)

B = concatenate((self.B, other.B), axis=0)

C = concatenate((self.C, other.C), axis=1)

D = self.D + other.D

return StateSpace(A, B, C, D, dt)

# Right addition - just switch the arguments

def __radd__(self, other):

"""Right add two LTI systems (parallel connection)."""

return self + other

# Subtraction of two state space systems (parallel interconnection)

def __sub__(self, other):

"""Subtract two LTI systems."""

return self + (-other)

def __rsub__(self, other):

"""Right subtract two LTI systems."""

return other + (-self)

# Multiplication of two state space systems (series interconnection)

def __mul__(self, other):

"""Multiply two LTI objects (serial connection)."""

# Check for a couple of special cases

if isinstance(other, (int, float, complex, np.number)):

# Just multiplying by a scalar; change the output

A, B = self.A, self.B

C = self.C * other

D = self.D * other

dt = self.dt

else:

other = _convertToStateSpace(other)

# Check to make sure the dimensions are OK

if self.inputs != other.outputs:

raise ValueError("C = A * B: A has %i column(s) (input(s)), \

but B has %i row(s)\n(output(s))." % (self.inputs, other.outputs))

# Figure out the sampling time to use

if (self.dt == None and other.dt != None):

dt = other.dt # use dt from second argument

elif (other.dt == None and self.dt != None) or \

(timebaseEqual(self, other)):

dt = self.dt # use dt from first argument

else:

raise ValueError("Systems have different sampling times")

# Concatenate the various arrays

A = concatenate(

(concatenate((other.A, zeros((other.A.shape[0], self.A.shape[1]))),

axis=1),

concatenate((self.B * other.C, self.A), axis=1)), axis=0)

B = concatenate((other.B, self.B * other.D), axis=0)

C = concatenate((self.D * other.C, self.C),axis=1)

D = self.D * other.D

return StateSpace(A, B, C, D, dt)

# Right multiplication of two state space systems (series interconnection)

# Just need to convert LH argument to a state space object

# TODO: __rmul__ only works for special cases (??)

def __rmul__(self, other):

"""Right multiply two LTI objects (serial connection)."""

# Check for a couple of special cases

if isinstance(other, (int, float, complex, np.number)):

# Just multiplying by a scalar; change the input

A, C = self.A, self.C

B = self.B * other

D = self.D * other

return StateSpace(A, B, C, D, self.dt)

# is lti, and convertible?

if isinstance(other, LTI):

return _convertToStateSpace(other) * self

# try to treat this as a matrix

try:

X = _matrix(other)

C = X * self.C

D = X * self.D

return StateSpace(self.A, self.B, C, D, self.dt)

except Exception as e:

print(e)

pass

raise TypeError("can't interconnect systems")

# TODO: __div__ and __rdiv__ are not written yet.

def __div__(self, other):

"""Divide two LTI systems."""

raise NotImplementedError("StateSpace.__div__ is not implemented yet.")

def __rdiv__(self, other):

"""Right divide two LTI systems."""

raise NotImplementedError("StateSpace.__rdiv__ is not implemented yet.")

def evalfr(self, omega):

"""Evaluate a SS system's transfer function at a single frequency.

warn("StateSpace.evalfr(omega) will be deprecated in a future "

"release of python-control; use evalfr(sys, omega*1j) instead",

PendingDeprecationWarning)

return self._evalfr(omega)

def _evalfr(self, omega):

"""Evaluate a SS system's transfer function at a single frequency"""

# Figure out the point to evaluate the transfer function

if isdtime(self, strict=True):

dt = timebase(self)

s = exp(1.j * omega * dt)

if omega * dt > math.pi:

warn("_evalfr: frequency evaluation above Nyquist frequency")

else:

s = omega * 1.j

return self.horner(s)

def horner(self, s):

"""Evaluate the systems's transfer function for a complex variable

resp = self.C * solve(s * eye(self.states) - self.A,

self.B) + self.D

return array(resp)

# Method for generating the frequency response of the system

def freqresp(self, omega):

"""

# In case omega is passed in as a list, rather than a proper array.

omega = np.asarray(omega)

numFreqs = len(omega)

Gfrf = np.empty((self.outputs, self.inputs, numFreqs),

dtype=np.complex128)

# Sort frequency and calculate complex frequencies on either imaginary

# axis (continuous time) or unit circle (discrete time).

omega.sort()

if isdtime(self, strict=True):

dt = timebase(self)

cmplx_freqs = exp(1.j * omega * dt)

if max(np.abs(omega)) * dt > math.pi:

warn("freqresp: frequency evaluation above Nyquist frequency")

else:

cmplx_freqs = omega * 1.j

# Do the frequency response evaluation. Use TB05AD from Slycot

# if it's available, otherwise use the built-in horners function.

try:

from slycot import tb05ad

n = np.shape(self.A)[0]

m = self.inputs

p = self.outputs

# The first call both evaluates C(sI-A)^-1 B and also returns

# Hessenberg transformed matrices at, bt, ct.

result = tb05ad(n, m, p, cmplx_freqs[0], self.A,

self.B, self.C, job='NG')

# When job='NG', result = (at, bt, ct, g_i, hinvb, info)

at = result[0]

bt = result[1]

ct = result[2]

# TB05AD frequency evaluation does not include direct feedthrough.

Gfrf[:, :, 0] = result[3] + self.D

# Now, iterate through the remaining frequencies using the

# transformed state matrices, at, bt, ct.

# Start at the second frequency, already have the first.

for kk, cmplx_freqs_kk in enumerate(cmplx_freqs[1:numFreqs]):

result = tb05ad(n, m, p, cmplx_freqs_kk, at,

bt, ct, job='NH')

# When job='NH', result = (g_i, hinvb, info)

# kk+1 because enumerate starts at kk = 0.

# but zero-th spot is already filled.

Gfrf[:, :, kk+1] = result[0] + self.D

except ImportError: # Slycot unavailable. Fall back to horner.

for kk, cmplx_freqs_kk in enumerate(cmplx_freqs):

Gfrf[:, :, kk] = self.horner(cmplx_freqs_kk)

# mag phase omega

return np.abs(Gfrf), np.angle(Gfrf), omega

# Compute poles and zeros

def pole(self):

"""Compute the poles of a state space system."""

return eigvals(self.A) if self.states else np.array([])

def zero(self):

"""Compute the zeros of a state space system."""

if not self.states:

return np.array([])

# 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]

if nu == 0:

return np.array([])

else:

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 lambda

# for which there exist nontrivial solutions of the equation

# `Lz - lamba 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):

"""Feedback interconnection between two LTI systems."""

other = _convertToStateSpace(other)

# Check to make sure the dimensions are OK

if (self.inputs != other.outputs) or (self.outputs != other.inputs):

raise ValueError("State space systems don't have compatible inputs/outputs for "

"feedback.")

# Figure out the sampling time to use

if self.dt is None and other.dt is not None:

dt = other.dt # use dt from second argument

elif other.dt is None and self.dt is not None or timebaseEqual(self, other):

dt = self.dt # use dt from first argument

else:

raise ValueError("Systems have different sampling times")

A1 = self.A

B1 = self.B

C1 = self.C

D1 = self.D

A2 = other.A

B2 = other.B

C2 = other.C

D2 = other.D

F = eye(self.inputs) - sign * D2 * D1

if matrix_rank(F) != self.inputs:

raise ValueError("I - sign * D2 * D1 is singular to working precision.")

# Precompute F\D2 and F\C2 (E = inv(F))

# We can solve two linear systems in one pass, since the

# coefficients matrix F is the same. Thus, we perform the LU

# decomposition (cubic runtime complexity) of F only once!

# The remaining back substitutions are only quadratic in runtime.

E_D2_C2 = solve(F, concatenate((D2, C2), axis=1))

E_D2 = E_D2_C2[:, :other.inputs]

E_C2 = E_D2_C2[:, other.inputs:]

T1 = eye(self.outputs) + sign * D1 * E_D2

T2 = eye(self.inputs) + sign * E_D2 * D1

A = concatenate((concatenate((A1 + sign * B1 * E_D2 * C1, sign * B1 * E_C2), axis=1),

concatenate((B2 * T1 * C1, A2 + sign * B2 * D1 * E_C2), axis=1)),

axis=0)

B = concatenate((B1 * T2, B2 * D1 * T2), axis=0)

C = concatenate((T1 * C1, sign * D1 * E_C2), axis=1)

D = D1 * T2

return StateSpace(A, B, C, D, dt)

def lft(self, other, nu=-1, ny=-1):

"""Return the Linear Fractional Transformation.

other = _convertToStateSpace(other)

# maximal values for nu, ny

if ny == -1:

ny = min(other.inputs, self.outputs)

if nu == -1:

nu = min(other.outputs, self.inputs)

# dimension check

# TODO

# Figure out the sampling time to use

if (self.dt == None and other.dt != None):

dt = other.dt # use dt from second argument

elif (other.dt == None and self.dt != None) or \

timebaseEqual(self, other):

dt = self.dt # use dt from first argument

else:

raise ValueError("Systems have different time bases")

# submatrices

A = self.A

B1 = self.B[:, :self.inputs - nu]

B2 = self.B[:, self.inputs - nu:]

C1 = self.C[:self.outputs - ny, :]

C2 = self.C[self.outputs - ny:, :]

D11 = self.D[:self.outputs - ny, :self.inputs - nu]

D12 = self.D[:self.outputs - ny, self.inputs - nu:]

D21 = self.D[self.outputs - ny:, :self.inputs - nu]

D22 = self.D[self.outputs - ny:, self.inputs - nu:]

# submatrices

Abar = other.A

Bbar1 = other.B[:, :ny]

Bbar2 = other.B[:, ny:]

Cbar1 = other.C[:nu, :]

Cbar2 = other.C[nu:, :]

Dbar11 = other.D[:nu, :ny]

Dbar12 = other.D[:nu, ny:]

Dbar21 = other.D[nu:, :ny]

Dbar22 = other.D[nu:, ny:]

# well-posed check

F = np.block([[np.eye(ny), -D22], [-Dbar11, np.eye(nu)]])

if matrix_rank(F) != ny + nu:

raise ValueError("lft not well-posed to working precision.")

# solve for the resulting ss by solving for [y, u] using [x,

# xbar] and [w1, w2].

TH = np.linalg.solve(F, np.block(

[[C2, np.zeros((ny, other.states)), D21, np.zeros((ny, other.inputs - ny))],

[np.zeros((nu, self.states)), Cbar1, np.zeros((nu, self.inputs - nu)), Dbar12]]

))

T11 = TH[:ny, :self.states]

T12 = TH[:ny, self.states: self.states + other.states]

T21 = TH[ny:, :self.states]

T22 = TH[ny:, self.states: self.states + other.states]

H11 = TH[:ny, self.states + other.states: self.states + other.states + self.inputs - nu]

H12 = TH[:ny, self.states + other.states + self.inputs - nu:]

H21 = TH[ny:, self.states + other.states: self.states + other.states + self.inputs - nu]

H22 = TH[ny:, self.states + other.states + self.inputs - nu:]

Ares = np.block([

[A + B2.dot(T21), B2.dot(T22)],

[Bbar1.dot(T11), Abar + Bbar1.dot(T12)]

])

Bres = np.block([

[B1 + B2.dot(H21), B2.dot(H22)],

[Bbar1.dot(H11), Bbar2 + Bbar1.dot(H12)]

])

Cres = np.block([

[C1 + D12.dot(T21), D12.dot(T22)],

[Dbar21.dot(T11), Cbar2 + Dbar21.dot(T12)]

])

Dres = np.block([

[D11 + D12.dot(H21), D12.dot(H22)],

[Dbar21.dot(H11), Dbar22 + Dbar21.dot(H12)]

])

return StateSpace(Ares, Bres, Cres, Dres, dt)

def minreal(self, tol=0.0):

"""Calculate a minimal realization, removes unobservable and

if self.states:

try:

from slycot import tb01pd

B = empty((self.states, max(self.inputs, self.outputs)))

B[:,:self.inputs] = self.B

C = empty((max(self.outputs, self.inputs), self.states))

C[:self.outputs,:] = self.C

A, B, C, nr = tb01pd(self.states, self.inputs, self.outputs,

self.A, B, C, tol=tol)

return StateSpace(A[:nr,:nr], B[:nr,:self.inputs],

C[:self.outputs,:nr], self.D)

except ImportError:

raise TypeError("minreal requires slycot tb01pd")

else:

return StateSpace(self)

# TODO: add discrete time check

def returnScipySignalLTI(self):

"""Return a list of a list of scipy.signal.lti objects.

# Preallocate the output.

out = [[[] for _ in range(self.inputs)] for _ in range(self.outputs)]

for i in range(self.outputs):

for j in range(self.inputs):

out[i][j] = lti(asarray(self.A), asarray(self.B[:, j]),

asarray(self.C[i, :]), asarray(self.D[i, j]))

return out

def append(self, other):

"""Append a second model to the present model. The second

if not isinstance(other, StateSpace):

other = _convertToStateSpace(other)

if self.dt != other.dt:

raise ValueError("Systems must have the same time step")

n = self.states + other.states

m = self.inputs + other.inputs

p = self.outputs + other.outputs

A = zeros((n, n))

B = zeros((n, m))

C = zeros((p, n))

D = zeros((p, m))

A[:self.states, :self.states] = self.A

A[self.states:, self.states:] = other.A

B[:self.states, :self.inputs] = self.B

B[self.states:, self.inputs:] = other.B

C[:self.outputs, :self.states] = self.C

C[self.outputs:, self.states:] = other.C

D[:self.outputs, :self.inputs] = self.D

D[self.outputs:, self.inputs:] = other.D

return StateSpace(A, B, C, D, self.dt)

def __getitem__(self, indices):

"""Array style access"""

if len(indices) != 2:

raise IOError('must provide indices of length 2 for state space')

i = indices[0]

j = indices[1]

return StateSpace(self.A, self.B[:, j], self.C[i, :], self.D[i, j], self.dt)

def sample(self, Ts, method='zoh', alpha=None):

"""Convert a continuous time system to discrete time

if not self.isctime():

raise ValueError("System must be continuous time system")

sys = (self.A, self.B, self.C, self.D)

Ad, Bd, C, D, dt = cont2discrete(sys, Ts, method, alpha)

return StateSpace(Ad, Bd, C, D, dt)

def dcgain(self):

"""Return the zero-frequency gain

try:

if self.isctime():

gain = np.asarray(self.D-self.C.dot(np.linalg.solve(self.A, self.B)))

else:

gain = self.horner(1)

except LinAlgError:

# eigenvalue at DC

gain = np.tile(np.nan, (self.outputs, self.inputs))

"""Convert a system to state space form (if needed).

from .xferfcn import TransferFunction

import itertools

if isinstance(sys, StateSpace):

if len(kw):

raise TypeError("If sys is a StateSpace, _convertToStateSpace \

cannot take keywords.")

# Already a state space system; just return it

return sys

elif isinstance(sys, TransferFunction):

try:

from slycot import td04ad

if len(kw):

raise TypeError("If sys is a TransferFunction, "

"_convertToStateSpace cannot take keywords.")

# Change the numerator and denominator arrays so that the transfer

# function matrix has a common denominator.

# matrices are also sized/padded to fit td04ad

num, den, denorder = sys.minreal()._common_den()

# transfer function to state space conversion now should work!

ssout = td04ad('C', sys.inputs, sys.outputs,

denorder, den, num, tol=0)

states = ssout[0]

return StateSpace(ssout[1][:states, :states], ssout[2][:states, :sys.inputs],

ssout[3][:sys.outputs, :states], ssout[4], sys.dt)

except ImportError:

# No Slycot. Scipy tf->ss can't handle MIMO, but static

# MIMO is an easy special case we can check for here

maxn = max(max(len(n) for n in nrow)

for nrow in sys.num)

maxd = max(max(len(d) for d in drow)

for drow in sys.den)

if 1 == maxn and 1 == maxd:

D = empty((sys.outputs, sys.inputs), dtype=float)

for i, j in itertools.product(range(sys.outputs), range(sys.inputs)):

D[i, j] = sys.num[i][j][0] / sys.den[i][j][0]

return StateSpace([], [], [], D, sys.dt)

else:

if sys.inputs != 1 or sys.outputs != 1:

raise TypeError("No support for MIMO without slycot")

# TODO: do we want to squeeze first and check dimenations?

# I think this will fail if num and den aren't 1-D after

# the squeeze

A, B, C, D = sp.signal.tf2ss(squeeze(sys.num), squeeze(sys.den))

return StateSpace(A, B, C, D, sys.dt)

elif isinstance(sys, (int, float, complex, np.number)):

if "inputs" in kw:

inputs = kw["inputs"]

else:

inputs = 1

if "outputs" in kw:

outputs = kw["outputs"]

else:

outputs = 1

# Generate a simple state space system of the desired dimension

# The following Doesn't work due to inconsistencies in ltisys:

# return StateSpace([[]], [[]], [[]], eye(outputs, inputs))

return StateSpace(0., zeros((1, inputs)), zeros((outputs, 1)),

sys * ones((outputs, inputs)))

# If this is a matrix, try to create a constant feedthrough

try:

D = _matrix(sys)

return StateSpace([], [], [], D)

except Exception as e:

print("Failure to assume argument is matrix-like in" \

" _convertToStateSpace, result %s" % e)

"""Generate a random state space.

# Probability of repeating a previous root.

pRepeat = 0.05

# Probability of choosing a real root. Note that when choosing a complex

# root, the conjugate gets chosen as well. So the expected proportion of

# real roots is pReal / (pReal + 2 * (1 - pReal)).

pReal = 0.6

# Probability that an element in B or C will not be masked out.

pBCmask = 0.8

# Probability that an element in D will not be masked out.

pDmask = 0.3

# Probability that D = 0.

pDzero = 0.5

# Check for valid input arguments.

if states < 1 or states % 1:

raise ValueError("states must be a positive integer. states = %g." %

states)

if inputs < 1 or inputs % 1:

raise ValueError("inputs must be a positive integer. inputs = %g." %

inputs)

if outputs < 1 or outputs % 1:

raise ValueError("outputs must be a positive integer. outputs = %g." %

outputs)

# Make some poles for A. Preallocate a complex array.

poles = zeros(states) + zeros(states) * 0.j

i = 0

while i < states:

if rand() < pRepeat and i != 0 and i != states - 1:

# Small chance of copying poles, if we're not at the first or last

# element.

if poles[i-1].imag == 0:

# Copy previous real pole.

poles[i] = poles[i-1]

i += 1

else:

# Copy previous complex conjugate pair of poles.

poles[i:i+2] = poles[i-2:i]

i += 2

elif rand() < pReal or i == states - 1:

# No-oscillation pole.