Skip to content

Commit d2f9a9c

Browse files
murrayrmmurrayrm
authored
Merge pull request #1116 from murrayrm/update_ssmatrix-01Feb2025
Update _ssmatrix and _check_shape for consistent usage
2 parents 2cb0520 + 4fc70f7 commit d2f9a9c

File tree

7 files changed

+133
-131
lines changed

7 files changed

+133
-131
lines changed

‎control/mateqn.py‎

Lines changed: 36 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,6 @@
4444

4545
from .exception import ControlSlycot, ControlArgument, ControlDimension, \
4646
slycot_check
47-
from .statesp import _ssmatrix
4847

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

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

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

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

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

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

213-
return _ssmatrix(X)
212+
return X
214213

215214

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

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

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

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

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

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

336-
return _ssmatrix(X)
335+
return X
337336

338337

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

409408
# Check to make sure input matrices are the right shape and type
410-
_check_shape(_As, A, n, n, square=True)
411-
_check_shape(_Bs, B, n, m)
412-
_check_shape(_Qs, Q, n, n, square=True, symmetric=True)
413-
_check_shape(_Rs, R, m, m, square=True, symmetric=True)
409+
_check_shape(A, n, n, square=True, name=_As)
410+
_check_shape(B, n, m, name=_Bs)
411+
_check_shape(Q, n, n, square=True, symmetric=True, name=_Qs)
412+
_check_shape(R, m, m, square=True, symmetric=True, name=_Rs)
414413

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

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

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

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

459458
# Check to make sure input matrices are the right shape and type
460-
_check_shape(_Es, E, n, n, square=True)
461-
_check_shape(_Ss, S, n, m)
459+
_check_shape(E, n, n, square=True, name=_Es)
460+
_check_shape(S, n, m, name=_Ss)
462461

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

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

495494
# Return the solution X, the closed-loop eigenvalues L and
496495
# the gain matrix G
497-
return _ssmatrix(X), L, _ssmatrix(G)
496+
return X, L, G
498497

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

566565
# Check to make sure input matrices are the right shape and type
567-
_check_shape(_As, A, n, n, square=True)
568-
_check_shape(_Bs, B, n, m)
569-
_check_shape(_Qs, Q, n, n, square=True, symmetric=True)
570-
_check_shape(_Rs, R, m, m, square=True, symmetric=True)
566+
_check_shape(A, n, n, square=True, name=_As)
567+
_check_shape(B, n, m, name=_Bs)
568+
_check_shape(Q, n, n, square=True, symmetric=True, name=_Qs)
569+
_check_shape(R, m, m, square=True, symmetric=True, name=_Rs)
571570
if E is not None:
572-
_check_shape(_Es, E, n, n, square=True)
571+
_check_shape(E, n, n, square=True, name=_Es)
573572
if S is not None:
574-
_check_shape(_Ss, S, n, m)
573+
_check_shape(S, n, m, name=_Ss)
575574

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

592-
return _ssmatrix(X), L, _ssmatrix(G)
591+
return X, L, G
593592

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

619618
# Return the solution X, the closed-loop eigenvalues L and
620619
# the gain matrix G
621-
return _ssmatrix(X), L, _ssmatrix(G)
620+
return X, L, G
622621

623622

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

633632

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

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

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

645646

646647
# Utility function to check if a matrix is symmetric

‎control/statefbk.py‎

Lines changed: 26 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,7 @@
5050
ControlSlycot
5151
from .iosys import _process_indices, _process_labels, isctime, isdtime
5252
from .lti import LTI
53-
from .mateqn import _check_shape, care, dare
53+
from .mateqn import care, dare
5454
from .nlsys import NonlinearIOSystem, interconnect
5555
from .statesp import StateSpace, _ssmatrix, ss
5656

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

132132
# Convert the system inputs to NumPy arrays
133-
A_mat = np.array(A)
134-
B_mat = np.array(B)
135-
if (A_mat.shape[0] != A_mat.shape[1]):
136-
raise ControlDimension("A must be a square matrix")
137-
138-
if (A_mat.shape[0] != B_mat.shape[0]):
139-
err_str = "The number of rows of A must equal the number of rows in B"
140-
raise ControlDimension(err_str)
133+
A_mat = _ssmatrix(A, square=True, name="A")
134+
B_mat = _ssmatrix(B, axis=0, rows=A_mat.shape[0])
141135

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

145139
result = place_poles(A_mat, B_mat, placed_eigs, method='YT')
146140
K = result.gain_matrix
147-
return _ssmatrix(K)
141+
return K
148142

149143

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

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

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

248240
# Return the gain matrix, with MATLAB gain convention
249-
return _ssmatrix(-F)
241+
return -F
250242

251243

252244
# Contributed by Roberto Bucher <roberto.bucher@supsi.ch>
@@ -274,12 +266,12 @@ def acker(A, B, poles):
274266
275267
"""
276268
# Convert the inputs to matrices
277-
a = _ssmatrix(A)
278-
b = _ssmatrix(B)
269+
A = _ssmatrix(A, square=True, name="A")
270+
B = _ssmatrix(B, axis=0, rows=A.shape[0], name="B")
279271

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

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

296-
K = K[-1][:] # Extract the last row
297-
return _ssmatrix(K)
288+
K = K[-1, :] # Extract the last row
289+
return K
298290

299291

300292
def lqr(*args, **kwargs):
@@ -577,7 +569,7 @@ def dlqr(*args, **kwargs):
577569

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

582574

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

11001092
# Convert input parameters to matrices (if they aren't already)
1101-
A = _ssmatrix(A)
1102-
if np.asarray(B).ndim == 1 and len(B) == A.shape[0]:
1103-
B = _ssmatrix(B, axis=0)
1104-
else:
1105-
B = _ssmatrix(B)
1106-
1093+
A = _ssmatrix(A, square=True, name="A")
11071094
n = A.shape[0]
1108-
m = B.shape[1]
11091095

1110-
_check_shape('A', A, n, n, square=True)
1111-
_check_shape('B', B, n, m)
1096+
B = _ssmatrix(B, axis=0, rows=n, name="B")
1097+
m = B.shape[1]
11121098

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

1122-
return _ssmatrix(ctrb)
1108+
return ctrb
11231109

11241110

11251111
def obsv(A, C, t=None):
@@ -1145,16 +1131,12 @@ def obsv(A, C, t=None):
11451131
np.int64(2)
11461132
11471133
"""
1148-
11491134
# Convert input parameters to matrices (if they aren't already)
1150-
A = _ssmatrix(A)
1151-
C = _ssmatrix(C)
1152-
1153-
n = np.shape(A)[0]
1154-
p = np.shape(C)[0]
1135+
A = _ssmatrix(A, square=True, name="A")
1136+
n = A.shape[0]
11551137

1156-
_check_shape('A', A, n, n, square=True)
1157-
_check_shape('C', C, p, n)
1138+
C = _ssmatrix(C, cols=n, name="C")
1139+
p = C.shape[0]
11581140

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

1169-
return _ssmatrix(obsv)
1151+
return obsv
11701152

11711153

11721154
def gram(sys, type):
@@ -1246,7 +1228,7 @@ def gram(sys, type):
12461228
X, scale, sep, ferr, w = sb03md(
12471229
n, C, A, U, dico, job='X', fact='N', trana=tra)
12481230
gram = X
1249-
return _ssmatrix(gram)
1231+
return gram
12501232

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

0 commit comments

Comments
 (0)