Thanks to visit codestin.com
Credit goes to github.com

Skip to content

Update _ssmatrix and _check_shape for consistent usage #1116

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Feb 2, 2025
Merged

Update _ssmatrix and _check_shape for consistent usage #1116

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
71 changes: 36 additions & 35 deletions control/mateqn.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,6 @@

from .exception import ControlSlycot, ControlArgument, ControlDimension, \
slycot_check
from .statesp import _ssmatrix

# Make sure we have access to the right slycot routines
try:
Expand Down Expand Up @@ -151,12 +150,12 @@ def lyap(A, Q, C=None, E=None, method=None):
m = Q.shape[0]

# Check to make sure input matrices are the right shape and type
_check_shape("A", A, n, n, square=True)
_check_shape(A, n, n, square=True, name="A")

# Solve standard Lyapunov equation
if C is None and E is None:
# Check to make sure input matrices are the right shape and type
_check_shape("Q", Q, n, n, square=True, symmetric=True)
_check_shape(Q, n, n, square=True, symmetric=True, name="Q")

if method == 'scipy':
# Solve the Lyapunov equation using SciPy
Expand All @@ -171,8 +170,8 @@ def lyap(A, Q, C=None, E=None, method=None):
# Solve the Sylvester equation
elif C is not None and E is None:
# Check to make sure input matrices are the right shape and type
_check_shape("Q", Q, m, m, square=True)
_check_shape("C", C, n, m)
_check_shape(Q, m, m, square=True, name="Q")
_check_shape(C, n, m, name="C")

if method == 'scipy':
# Solve the Sylvester equation using SciPy
Expand All @@ -184,8 +183,8 @@ def lyap(A, Q, C=None, E=None, method=None):
# Solve the generalized Lyapunov equation
elif C is None and E is not None:
# Check to make sure input matrices are the right shape and type
_check_shape("Q", Q, n, n, square=True, symmetric=True)
_check_shape("E", E, n, n, square=True)
_check_shape(Q, n, n, square=True, symmetric=True, name="Q")
_check_shape(E, n, n, square=True, name="E")

if method == 'scipy':
raise ControlArgument(
Expand All @@ -210,7 +209,7 @@ def lyap(A, Q, C=None, E=None, method=None):
else:
raise ControlArgument("Invalid set of input parameters")

return _ssmatrix(X)
return X


def dlyap(A, Q, C=None, E=None, method=None):
Expand Down Expand Up @@ -281,12 +280,12 @@ def dlyap(A, Q, C=None, E=None, method=None):
m = Q.shape[0]

# Check to make sure input matrices are the right shape and type
_check_shape("A", A, n, n, square=True)
_check_shape(A, n, n, square=True, name="A")

# Solve standard Lyapunov equation
if C is None and E is None:
# Check to make sure input matrices are the right shape and type
_check_shape("Q", Q, n, n, square=True, symmetric=True)
_check_shape(Q, n, n, square=True, symmetric=True, name="Q")

if method == 'scipy':
# Solve the Lyapunov equation using SciPy
Expand All @@ -301,8 +300,8 @@ def dlyap(A, Q, C=None, E=None, method=None):
# Solve the Sylvester equation
elif C is not None and E is None:
# Check to make sure input matrices are the right shape and type
_check_shape("Q", Q, m, m, square=True)
_check_shape("C", C, n, m)
_check_shape(Q, m, m, square=True, name="Q")
_check_shape(C, n, m, name="C")

if method == 'scipy':
raise ControlArgument(
Expand All @@ -314,8 +313,8 @@ def dlyap(A, Q, C=None, E=None, method=None):
# Solve the generalized Lyapunov equation
elif C is None and E is not None:
# Check to make sure input matrices are the right shape and type
_check_shape("Q", Q, n, n, square=True, symmetric=True)
_check_shape("E", E, n, n, square=True)
_check_shape(Q, n, n, square=True, symmetric=True, name="Q")
_check_shape(E, n, n, square=True, name="E")

if method == 'scipy':
raise ControlArgument(
Expand All @@ -333,7 +332,7 @@ def dlyap(A, Q, C=None, E=None, method=None):
else:
raise ControlArgument("Invalid set of input parameters")

return _ssmatrix(X)
return X


#
Expand Down Expand Up @@ -407,10 +406,10 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None,
m = B.shape[1]

# Check to make sure input matrices are the right shape and type
_check_shape(_As, A, n, n, square=True)
_check_shape(_Bs, B, n, m)
_check_shape(_Qs, Q, n, n, square=True, symmetric=True)
_check_shape(_Rs, R, m, m, square=True, symmetric=True)
_check_shape(A, n, n, square=True, name=_As)
_check_shape(B, n, m, name=_Bs)
_check_shape(Q, n, n, square=True, symmetric=True, name=_Qs)
_check_shape(R, m, m, square=True, symmetric=True, name=_Rs)

# Solve the standard algebraic Riccati equation
if S is None and E is None:
Expand All @@ -423,7 +422,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None,
X = sp.linalg.solve_continuous_are(A, B, Q, R)
K = np.linalg.solve(R, B.T @ X)
E, _ = np.linalg.eig(A - B @ K)
return _ssmatrix(X), E, _ssmatrix(K)
return X, E, K

# Make sure we can import required slycot routines
try:
Expand All @@ -448,7 +447,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None,

# Return the solution X, the closed-loop eigenvalues L and
# the gain matrix G
return _ssmatrix(X), w[:n], _ssmatrix(G)
return X, w[:n], G

# Solve the generalized algebraic Riccati equation
else:
Expand All @@ -457,8 +456,8 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None,
E = np.eye(A.shape[0]) if E is None else np.array(E, ndmin=2)

# Check to make sure input matrices are the right shape and type
_check_shape(_Es, E, n, n, square=True)
_check_shape(_Ss, S, n, m)
_check_shape(E, n, n, square=True, name=_Es)
_check_shape(S, n, m, name=_Ss)

# See if we should solve this using SciPy
if method == 'scipy':
Expand All @@ -469,7 +468,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None,
X = sp.linalg.solve_continuous_are(A, B, Q, R, s=S, e=E)
K = np.linalg.solve(R, B.T @ X @ E + S.T)
eigs, _ = sp.linalg.eig(A - B @ K, E)
return _ssmatrix(X), eigs, _ssmatrix(K)
return X, eigs, K

# Make sure we can find the required slycot routine
try:
Expand All @@ -494,7 +493,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None,

# Return the solution X, the closed-loop eigenvalues L and
# the gain matrix G
return _ssmatrix(X), L, _ssmatrix(G)
return X, L, G

def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None,
_As="A", _Bs="B", _Qs="Q", _Rs="R", _Ss="S", _Es="E"):
Expand Down Expand Up @@ -564,14 +563,14 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None,
m = B.shape[1]

# Check to make sure input matrices are the right shape and type
_check_shape(_As, A, n, n, square=True)
_check_shape(_Bs, B, n, m)
_check_shape(_Qs, Q, n, n, square=True, symmetric=True)
_check_shape(_Rs, R, m, m, square=True, symmetric=True)
_check_shape(A, n, n, square=True, name=_As)
_check_shape(B, n, m, name=_Bs)
_check_shape(Q, n, n, square=True, symmetric=True, name=_Qs)
_check_shape(R, m, m, square=True, symmetric=True, name=_Rs)
if E is not None:
_check_shape(_Es, E, n, n, square=True)
_check_shape(E, n, n, square=True, name=_Es)
if S is not None:
_check_shape(_Ss, S, n, m)
_check_shape(S, n, m, name=_Ss)

# Figure out how to solve the problem
if method == 'scipy':
Expand All @@ -589,7 +588,7 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None,
else:
L, _ = sp.linalg.eig(A - B @ G, E)

return _ssmatrix(X), L, _ssmatrix(G)
return X, L, G

# Make sure we can import required slycot routine
try:
Expand Down Expand Up @@ -618,7 +617,7 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None,

# Return the solution X, the closed-loop eigenvalues L and
# the gain matrix G
return _ssmatrix(X), L, _ssmatrix(G)
return X, L, G


# Utility function to decide on method to use
Expand All @@ -632,15 +631,17 @@ def _slycot_or_scipy(method):


# Utility function to check matrix dimensions
def _check_shape(name, M, n, m, square=False, symmetric=False):
def _check_shape(M, n, m, square=False, symmetric=False, name="??"):
if square and M.shape[0] != M.shape[1]:
raise ControlDimension("%s must be a square matrix" % name)

if symmetric and not _is_symmetric(M):
raise ControlArgument("%s must be a symmetric matrix" % name)

if M.shape[0] != n or M.shape[1] != m:
raise ControlDimension("Incompatible dimensions of %s matrix" % name)
raise ControlDimension(
f"Incompatible dimensions of {name} matrix; "
f"expected ({n}, {m}) but found {M.shape}")


# Utility function to check if a matrix is symmetric
Expand Down
70 changes: 26 additions & 44 deletions control/statefbk.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@
ControlSlycot
from .iosys import _process_indices, _process_labels, isctime, isdtime
from .lti import LTI
from .mateqn import _check_shape, care, dare
from .mateqn import care, dare
from .nlsys import NonlinearIOSystem, interconnect
from .statesp import StateSpace, _ssmatrix, ss

Expand Down Expand Up @@ -130,21 +130,15 @@ def place(A, B, p):
from scipy.signal import place_poles

# Convert the system inputs to NumPy arrays
A_mat = np.array(A)
B_mat = np.array(B)
if (A_mat.shape[0] != A_mat.shape[1]):
raise ControlDimension("A must be a square matrix")

if (A_mat.shape[0] != B_mat.shape[0]):
err_str = "The number of rows of A must equal the number of rows in B"
raise ControlDimension(err_str)
A_mat = _ssmatrix(A, square=True, name="A")
B_mat = _ssmatrix(B, axis=0, rows=A_mat.shape[0])

# Convert desired poles to numpy array
placed_eigs = np.atleast_1d(np.squeeze(np.asarray(p)))

result = place_poles(A_mat, B_mat, placed_eigs, method='YT')
K = result.gain_matrix
return _ssmatrix(K)
return K


def place_varga(A, B, p, dtime=False, alpha=None):
Expand Down Expand Up @@ -206,10 +200,8 @@ def place_varga(A, B, p, dtime=False, alpha=None):
raise ControlSlycot("can't find slycot module 'sb01bd'")

# Convert the system inputs to NumPy arrays
A_mat = np.array(A)
B_mat = np.array(B)
if (A_mat.shape[0] != A_mat.shape[1] or A_mat.shape[0] != B_mat.shape[0]):
raise ControlDimension("matrix dimensions are incorrect")
A_mat = _ssmatrix(A, square=True, name="A")
B_mat = _ssmatrix(B, axis=0, rows=A_mat.shape[0])

# Compute the system eigenvalues and convert poles to numpy array
system_eigs = np.linalg.eig(A_mat)[0]
Expand Down Expand Up @@ -246,7 +238,7 @@ def place_varga(A, B, p, dtime=False, alpha=None):
A_mat, B_mat, placed_eigs, DICO)

# Return the gain matrix, with MATLAB gain convention
return _ssmatrix(-F)
return -F


# Contributed by Roberto Bucher <[email protected]>
Expand Down Expand Up @@ -274,12 +266,12 @@ def acker(A, B, poles):

"""
# Convert the inputs to matrices
a = _ssmatrix(A)
b = _ssmatrix(B)
A = _ssmatrix(A, square=True, name="A")
B = _ssmatrix(B, axis=0, rows=A.shape[0], name="B")

# Make sure the system is controllable
ct = ctrb(A, B)
if np.linalg.matrix_rank(ct) != a.shape[0]:
if np.linalg.matrix_rank(ct) != A.shape[0]:
raise ValueError("System not reachable; pole placement invalid")

# Compute the desired characteristic polynomial
Expand All @@ -288,13 +280,13 @@ def acker(A, B, poles):
# Place the poles using Ackermann's method
# TODO: compute pmat using Horner's method (O(n) instead of O(n^2))
n = np.size(p)
pmat = p[n-1] * np.linalg.matrix_power(a, 0)
pmat = p[n-1] * np.linalg.matrix_power(A, 0)
for i in np.arange(1, n):
pmat = pmat + p[n-i-1] * np.linalg.matrix_power(a, i)
pmat = pmat + p[n-i-1] * np.linalg.matrix_power(A, i)
K = np.linalg.solve(ct, pmat)

K = K[-1][:] # Extract the last row
return _ssmatrix(K)
K = K[-1, :] # Extract the last row
return K


def lqr(*args, **kwargs):
Expand Down Expand Up @@ -577,7 +569,7 @@ def dlqr(*args, **kwargs):

# Compute the result (dimension and symmetry checking done in dare())
S, E, K = dare(A, B, Q, R, N, method=method, _Ss="N")
return _ssmatrix(K), _ssmatrix(S), E
return K, S, E


# Function to create an I/O sytems representing a state feedback controller
Expand Down Expand Up @@ -1098,17 +1090,11 @@ def ctrb(A, B, t=None):
"""

# Convert input parameters to matrices (if they aren't already)
A = _ssmatrix(A)
if np.asarray(B).ndim == 1 and len(B) == A.shape[0]:
B = _ssmatrix(B, axis=0)
else:
B = _ssmatrix(B)

A = _ssmatrix(A, square=True, name="A")
n = A.shape[0]
m = B.shape[1]

_check_shape('A', A, n, n, square=True)
_check_shape('B', B, n, m)
B = _ssmatrix(B, axis=0, rows=n, name="B")
m = B.shape[1]

if t is None or t > n:
t = n
Expand All @@ -1119,7 +1105,7 @@ def ctrb(A, B, t=None):
for k in range(1, t):
ctrb[:, k * m:(k + 1) * m] = np.dot(A, ctrb[:, (k - 1) * m:k * m])

return _ssmatrix(ctrb)
return ctrb


def obsv(A, C, t=None):
Expand All @@ -1145,16 +1131,12 @@ def obsv(A, C, t=None):
np.int64(2)

"""

# Convert input parameters to matrices (if they aren't already)
A = _ssmatrix(A)
C = _ssmatrix(C)

n = np.shape(A)[0]
p = np.shape(C)[0]
A = _ssmatrix(A, square=True, name="A")
n = A.shape[0]

_check_shape('A', A, n, n, square=True)
_check_shape('C', C, p, n)
C = _ssmatrix(C, cols=n, name="C")
p = C.shape[0]

if t is None or t > n:
t = n
Expand All @@ -1166,7 +1148,7 @@ def obsv(A, C, t=None):
for k in range(1, t):
obsv[k * p:(k + 1) * p, :] = np.dot(obsv[(k - 1) * p:k * p, :], A)

return _ssmatrix(obsv)
return obsv


def gram(sys, type):
Expand Down Expand Up @@ -1246,7 +1228,7 @@ def gram(sys, type):
X, scale, sep, ferr, w = sb03md(
n, C, A, U, dico, job='X', fact='N', trana=tra)
gram = X
return _ssmatrix(gram)
return gram

elif type == 'cf' or type == 'of':
# Compute cholesky factored gramian from slycot routine sb03od
Expand All @@ -1269,4 +1251,4 @@ def gram(sys, type):
X, scale, w = sb03od(
n, m, A, Q, C.transpose(), dico, fact='N', trans=tra)
gram = X
return _ssmatrix(gram)
return gram
Loading
Loading