Skip to content

Commit b284c47

Browse files
murrayrmmurrayrm
committed
use care() for lqr()
1 parent 4baea13 commit b284c47

File tree

5 files changed

+43
-80
lines changed

5 files changed

+43
-80
lines changed

‎control/mateqn.py‎

Lines changed: 9 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -206,7 +206,7 @@ def lyap(A, Q, C=None, E=None, method=None):
206206
sg03ad('C', 'B', 'N', 'T', 'L', n,
207207
A, E, eye(n, n), eye(n, n), -Q)
208208

209-
# Invalid set of input parameters
209+
# Invalid set of input parameters (C and E specified)
210210
else:
211211
raise ControlArgument("Invalid set of input parameters")
212212

@@ -331,7 +331,7 @@ def dlyap(A, Q, C=None, E=None, method=None):
331331
sg03ad('D', 'B', 'N', 'T', 'L', n,
332332
A, E, eye(n, n), eye(n, n), -Q)
333333

334-
# Invalid set of input parameters
334+
# Invalid set of input parameters (C and E specified)
335335
else:
336336
raise ControlArgument("Invalid set of input parameters")
337337

@@ -464,7 +464,11 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None):
464464
return _ssmatrix(X), w[:n], _ssmatrix(G)
465465

466466
# Solve the generalized algebraic Riccati equation
467-
elif S is not None and E is not None:
467+
else:
468+
# Initialize optional matrices
469+
S = np.zeros((n, m)) if S is None else np.array(S, ndmin=2)
470+
E = np.eye(A.shape[0]) if E is None else np.array(E, ndmin=2)
471+
468472
# Check to make sure input matrices are the right shape and type
469473
_check_shape("E", E, n, n, square=True)
470474
_check_shape("S", S, n, m)
@@ -508,11 +512,6 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None):
508512
# the gain matrix G
509513
return _ssmatrix(X), L, _ssmatrix(G)
510514

511-
# Invalid set of input parameters
512-
else:
513-
raise ControlArgument("Invalid set of input parameters")
514-
515-
516515
def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None):
517516
"""(X, L, G) = dare(A, B, Q, R) solves the discrete-time algebraic Riccati
518517
equation
@@ -597,10 +596,6 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None):
597596
if S is not None:
598597
_check_shape("S", S, n, m)
599598

600-
# For consistency with dare_slycot(), don't allow just S or E
601-
if (S is None and E is not None) or (E is None and S is not None):
602-
raise ControlArgument("Invalid set of input parameters")
603-
604599
Rmat = _ssmatrix(R)
605600
Qmat = _ssmatrix(Q)
606601
X = solve_discrete_are(A, B, Qmat, Rmat, e=E, s=S)
@@ -687,9 +682,9 @@ def _dare_slycot(A, B, Q, R, S=None, E=None, stabilizing=True):
687682

688683
# Utility function to decide on method to use
689684
def _slycot_or_scipy(method):
690-
if (method is None and slycot_check()) or method == 'slycot':
685+
if method == 'slycot' or (method is None and slycot_check()):
691686
return 'slycot'
692-
elif method == 'scipy' or not slycot_check():
687+
elif method == 'scipy' or (method is None and not slycot_check()):
693688
return 'scipy'
694689
else:
695690
raise ValueError("unknown method %s" % method)

‎control/statefbk.py‎

Lines changed: 17 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -304,6 +304,10 @@ def lqe(*args, **keywords):
304304
Process and sensor noise covariance matrices
305305
N : 2D array, optional
306306
Cross covariance matrix. Not currently implemented.
307+
method : str, optional
308+
Set the method used for computing the result. Current methods are
309+
'slycot' and 'scipy'. If set to None (default), try 'slycot' first
310+
and then 'scipy'.
307311
308312
Returns
309313
-------
@@ -326,8 +330,8 @@ def lqe(*args, **keywords):
326330
327331
Examples
328332
--------
329-
>>> L, P, E = lqe(A, G, C, QN, RN)
330-
>>> L, P, E = lqe(A, G, C, QN, RN, NN)
333+
>>> L, P, E = lqe(A, G, C, Q, R)
334+
>>> L, P, E = lqe(A, G, C, Q, R, N)
331335
332336
See Also
333337
--------
@@ -345,6 +349,9 @@ def lqe(*args, **keywords):
345349
# Process the arguments and figure out what inputs we received
346350
#
347351

352+
# Get the method to use (if specified as a keyword)
353+
method = keywords.get('method', None)
354+
348355
# Get the system description
349356
if (len(args) < 3):
350357
raise ControlArgument("not enough input arguments")
@@ -393,7 +400,7 @@ def lqe(*args, **keywords):
393400
N.shape[0] != ninputs or N.shape[1] != noutputs):
394401
raise ControlDimension("incorrect covariance matrix dimensions")
395402

396-
P, E, LT = care(A.T, C.T, G @ Q @ G.T, R)
403+
P, E, LT = care(A.T, C.T, G @ Q @ G.T, R, method=method)
397404
return _ssmatrix(LT.T), _ssmatrix(P), E
398405

399406

@@ -505,25 +512,13 @@ def lqr(*args, **keywords):
505512
506513
"""
507514

508-
# Figure out what method to use
509-
method = keywords.get('method', None)
510-
if method == 'slycot' or method is None:
511-
# Make sure that SLICOT is installed
512-
try:
513-
from slycot import sb02md
514-
from slycot import sb02mt
515-
method = 'slycot'
516-
except ImportError:
517-
if method == 'slycot':
518-
raise ControlSlycot(
519-
"can't find slycot module 'sb02md' or 'sb02nt'")
520-
else:
521-
method = 'scipy'
522-
523515
#
524516
# Process the arguments and figure out what inputs we received
525517
#
526518

519+
# Get the method to use (if specified as a keyword)
520+
method = keywords.get('method', None)
521+
527522
# Get the system description
528523
if (len(args) < 3):
529524
raise ControlArgument("not enough input arguments")
@@ -546,43 +541,11 @@ def lqr(*args, **keywords):
546541
if (len(args) > index + 2):
547542
N = np.array(args[index+2], ndmin=2, dtype=float)
548543
else:
549-
N = np.zeros((Q.shape[0], R.shape[1]))
550-
551-
# Check dimensions for consistency
552-
nstates = B.shape[0]
553-
ninputs = B.shape[1]
554-
if (A.shape[0] != nstates or A.shape[1] != nstates):
555-
raise ControlDimension("inconsistent system dimensions")
556-
557-
elif (Q.shape[0] != nstates or Q.shape[1] != nstates or
558-
R.shape[0] != ninputs or R.shape[1] != ninputs or
559-
N.shape[0] != nstates or N.shape[1] != ninputs):
560-
raise ControlDimension("incorrect weighting matrix dimensions")
561-
562-
if method == 'slycot':
563-
# Compute the G matrix required by SB02MD
564-
A_b, B_b, Q_b, R_b, L_b, ipiv, oufact, G = \
565-
sb02mt(nstates, ninputs, B, R, A, Q, N, jobl='N')
566-
567-
# Call the SLICOT function
568-
X, rcond, w, S, U, A_inv = sb02md(nstates, A_b, G, Q_b, 'C')
569-
570-
# Now compute the return value
571-
# We assume that R is positive definite and, hence, invertible
572-
K = np.linalg.solve(R, np.dot(B.T, X) + N.T)
573-
S = X
574-
E = w[0:nstates]
575-
576-
elif method == 'scipy':
577-
import scipy as sp
578-
S = sp.linalg.solve_continuous_are(A, B, Q, R, s=N)
579-
K = np.linalg.solve(R, B.T @ S + N.T)
580-
E, _ = np.linalg.eig(A - B @ K)
581-
582-
else:
583-
raise ValueError("unknown method: %s" % method)
544+
N = None
584545

585-
return _ssmatrix(K), _ssmatrix(S), E
546+
# Solve continuous algebraic Riccati equation
547+
X, L, G = care(A, B, Q, R, N, None, method=method)
548+
return G, X, L
586549

587550

588551
def ctrb(A, B):

‎control/tests/mateqn_test.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -120,7 +120,7 @@ def test_dlyap_g(self):
120120
A = array([[-0.6, 0],[-0.1, -0.4]])
121121
Q = array([[3, 1],[1, 1]])
122122
E = array([[1, 1],[2, 1]])
123-
X = dlyap(A,Q,None,E)
123+
X = dlyap(A, Q, None, E)
124124
# print("The solution obtained is ", X)
125125
assert_array_almost_equal(A @ X @ A.T - E @ X @ E.T + Q,
126126
zeros((2,2)))

‎control/tests/matlab_test.py‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -507,7 +507,6 @@ def testAcker(self, siso):
507507
"""Call acker()"""
508508
acker(siso.ss1.A, siso.ss1.B, [-2, -2.5])
509509

510-
@slycotonly
511510
def testLQR(self, siso):
512511
"""Call lqr()"""
513512
(K, S, E) = lqr(siso.ss1.A, siso.ss1.B, np.eye(2), np.eye(1))

‎control/tests/statefbk_test.py‎

Lines changed: 16 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -305,7 +305,6 @@ def check_LQR(self, K, S, poles, Q, R):
305305
np.testing.assert_array_almost_equal(K, K_expected)
306306
np.testing.assert_array_almost_equal(poles, poles_expected)
307307

308-
309308
@pytest.mark.parametrize("method", [None, 'slycot', 'scipy'])
310309
def test_LQR_integrator(self, matarrayin, matarrayout, method):
311310
if method == 'slycot' and not slycot_check():
@@ -334,7 +333,6 @@ def test_lqr_slycot_not_installed(self):
334333
with pytest.raises(ControlSlycot, match="can't find slycot"):
335334
K, S, poles = lqr(A, B, Q, R, method='slycot')
336335

337-
@slycotonly
338336
@pytest.mark.xfail(reason="warning not implemented")
339337
def testLQR_warning(self):
340338
"""Test lqr()
@@ -395,13 +393,11 @@ def check_LQE(self, L, P, poles, G, QN, RN):
395393
np.testing.assert_array_almost_equal(L, L_expected)
396394
np.testing.assert_array_almost_equal(poles, poles_expected)
397395

398-
@slycotonly
399396
def test_LQE(self, matarrayin):
400397
A, G, C, QN, RN = (matarrayin([[X]]) for X in [0., .1, 1., 10., 2.])
401398
L, P, poles = lqe(A, G, C, QN, RN)
402399
self.check_LQE(L, P, poles, G, QN, RN)
403400

404-
@slycotonly
405401
def test_lqe_call_format(self):
406402
# Create a random state space system for testing
407403
sys = rss(4, 3, 2)
@@ -438,7 +434,6 @@ def test_lqe_call_format(self):
438434
with pytest.raises(ct.ControlDimension, match="incorrect covariance"):
439435
L, P, E = lqe(sys.A, sys.B, sys.C, R, Q)
440436

441-
@slycotonly
442437
def test_care(self, matarrayin):
443438
"""Test stabilizing and anti-stabilizing feedbacks, continuous"""
444439
A = matarrayin(np.diag([1, -1]))
@@ -447,12 +442,17 @@ def test_care(self, matarrayin):
447442
R = matarrayin(np.identity(2))
448443
S = matarrayin(np.zeros((2, 2)))
449444
E = matarrayin(np.identity(2))
445+
450446
X, L, G = care(A, B, Q, R, S, E, stabilizing=True)
451447
assert np.all(np.real(L) < 0)
452-
X, L, G = care(A, B, Q, R, S, E, stabilizing=False)
453-
assert np.all(np.real(L) > 0)
454448

455-
@slycotonly
449+
if slycot_check():
450+
X, L, G = care(A, B, Q, R, S, E, stabilizing=False)
451+
assert np.all(np.real(L) > 0)
452+
else:
453+
with pytest.raises(ValueError, match="'scipy' not valid"):
454+
X, L, G = care(A, B, Q, R, S, E, stabilizing=False)
455+
456456
def test_dare(self, matarrayin):
457457
"""Test stabilizing and anti-stabilizing feedbacks, discrete"""
458458
A = matarrayin(np.diag([0.5, 2]))
@@ -461,7 +461,13 @@ def test_dare(self, matarrayin):
461461
R = matarrayin(np.identity(2))
462462
S = matarrayin(np.zeros((2, 2)))
463463
E = matarrayin(np.identity(2))
464+
464465
X, L, G = dare(A, B, Q, R, S, E, stabilizing=True)
465466
assert np.all(np.abs(L) < 1)
466-
X, L, G = dare(A, B, Q, R, S, E, stabilizing=False)
467-
assert np.all(np.abs(L) > 1)
467+
468+
if slycot_check():
469+
X, L, G = care(A, B, Q, R, S, E, stabilizing=False)
470+
assert np.all(np.real(L) > 0)
471+
else:
472+
with pytest.raises(ValueError, match="'scipy' not valid"):
473+
X, L, G = care(A, B, Q, R, S, E, stabilizing=False)

0 commit comments

Comments
 (0)