"""Convert a system into canonical form

# Call the appropriate tranformation function

if form == 'reachable':

return reachable_form(xsys)

elif form == 'observable':

return observable_form(xsys)

elif form == 'modal':

return modal_form(xsys)

else:

raise ControlNotImplemented(

"""Convert a system into reachable canonical form

# Check to make sure we have a SISO system

if not issiso(xsys):

raise ControlNotImplemented(

"Canonical forms for MIMO systems not yet supported")

# Create a new system, starting with a copy of the old one

zsys = StateSpace(xsys)

# Generate the system matrices for the desired canonical form

zsys.B = zeros(shape(xsys.B))

zsys.B[0, 0] = 1.0

zsys.A = zeros(shape(xsys.A))

Apoly = poly(xsys.A) # characteristic polynomial

for i in range(0, xsys.states):

zsys.A[0, i] = -Apoly[i+1] / Apoly[0]

if (i+1 < xsys.states):

zsys.A[i+1, i] = 1.0

# Compute the reachability matrices for each set of states

Wrx = ctrb(xsys.A, xsys.B)

Wrz = ctrb(zsys.A, zsys.B)

if matrix_rank(Wrx) != xsys.states:

raise ValueError("System not controllable to working precision.")

# Transformation from one form to another

Tzx = solve(Wrx.T, Wrz.T).T # matrix right division, Tzx = Wrz * inv(Wrx)

if matrix_rank(Tzx) != xsys.states:

raise ValueError("Transformation matrix singular to working precision.")

# Finally, compute the output matrix

zsys.C = solve(Tzx.T, xsys.C.T).T # matrix right division, zsys.C = xsys.C * inv(Tzx)

"""Convert a system into observable canonical form

# Check to make sure we have a SISO system

if not issiso(xsys):

raise ControlNotImplemented(

"Canonical forms for MIMO systems not yet supported")

# Create a new system, starting with a copy of the old one

zsys = StateSpace(xsys)

# Generate the system matrices for the desired canonical form

zsys.C = zeros(shape(xsys.C))

zsys.C[0, 0] = 1

zsys.A = zeros(shape(xsys.A))

Apoly = poly(xsys.A) # characteristic polynomial

for i in range(0, xsys.states):

zsys.A[i, 0] = -Apoly[i+1] / Apoly[0]

if (i+1 < xsys.states):

zsys.A[i, i+1] = 1

# Compute the observability matrices for each set of states

Wrx = obsv(xsys.A, xsys.C)

Wrz = obsv(zsys.A, zsys.C)

# Transformation from one form to another

Tzx = solve(Wrz, Wrx) # matrix left division, Tzx = inv(Wrz) * Wrx

if matrix_rank(Tzx) != xsys.states:

raise ValueError("Transformation matrix singular to working precision.")

# Finally, compute the output matrix

zsys.B = Tzx * xsys.B

"""Convert a system into modal canonical form

# Check to make sure we have a SISO system

if not issiso(xsys):

raise ControlNotImplemented(

"Canonical forms for MIMO systems not yet supported")

# Create a new system, starting with a copy of the old one

zsys = StateSpace(xsys)

# Calculate eigenvalues and matrix of eigenvectors Tzx,

eigval, eigvec = eig(xsys.A)

# Eigenvalues and according eigenvectors are not sorted,

# thus modal transformation is ambiguous

# Sorting eigenvalues and respective vectors by largest to smallest eigenvalue

idx = eigval.argsort()[::-1]

eigval = eigval[idx]

eigvec = eigvec[:,idx]

# If all eigenvalues are real, the matrix of eigenvectors is Tzx directly

if not iscomplex(eigval).any():

Tzx = eigvec

else:

# A is an arbitrary semisimple matrix

# Keep track of complex conjugates (need only one)

lst_conjugates = []

Tzx = None

for val, vec in zip(eigval, eigvec.T):

if iscomplex(val):

if val not in lst_conjugates:

lst_conjugates.append(val.conjugate())

if Tzx is not None:

Tzx = hstack((Tzx, hstack((vec.real.T, vec.imag.T))))

else:

Tzx = hstack((vec.real.T, vec.imag.T))

else:

# if conjugate has already been seen, skip this eigenvalue

lst_conjugates.remove(val)

else:

if Tzx is not None:

Tzx = hstack((Tzx, vec.real.T))

else:

Tzx = vec.real.T

# Generate the system matrices for the desired canonical form

zsys.A = solve(Tzx, xsys.A).dot(Tzx)

zsys.B = solve(Tzx, xsys.B)

zsys.C = xsys.C.dot(Tzx)