Skip to content

Commit a33bcc5

Browse files
murrayrmmurrayrm
authored
Merge pull request #670 from sawyerbfuller/dlqr2
Add dlqr() and dlqe() functions
2 parents 8356044 + 0d4ff4c commit a33bcc5

File tree

3 files changed

+541
-208
lines changed

3 files changed

+541
-208
lines changed

‎control/mateqn.py‎

Lines changed: 55 additions & 103 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
# Implementation of the functions lyap, dlyap, care and dare
44
# for solution of Lyapunov and Riccati equations.
55
#
6-
# Author: Bjorn Olofsson
6+
# Original author: Bjorn Olofsson
77

88
# Copyright (c) 2011, All rights reserved.
99

@@ -162,6 +162,7 @@ def lyap(A, Q, C=None, E=None, method=None):
162162
_check_shape("Q", Q, n, n, square=True, symmetric=True)
163163

164164
if method == 'scipy':
165+
# Solve the Lyapunov equation using SciPy
165166
return sp.linalg.solve_continuous_lyapunov(A, -Q)
166167

167168
# Solve the Lyapunov equation by calling Slycot function sb03md
@@ -177,6 +178,7 @@ def lyap(A, Q, C=None, E=None, method=None):
177178
_check_shape("C", C, n, m)
178179

179180
if method == 'scipy':
181+
# Solve the Sylvester equation using SciPy
180182
return sp.linalg.solve_sylvester(A, Q, -C)
181183

182184
# Solve the Sylvester equation by calling the Slycot function sb04md
@@ -293,6 +295,7 @@ def dlyap(A, Q, C=None, E=None, method=None):
293295
_check_shape("Q", Q, n, n, square=True, symmetric=True)
294296

295297
if method == 'scipy':
298+
# Solve the Lyapunov equation using SciPy
296299
return sp.linalg.solve_discrete_lyapunov(A, Q)
297300

298301
# Solve the Lyapunov equation by calling the Slycot function sb03md
@@ -343,7 +346,8 @@ def dlyap(A, Q, C=None, E=None, method=None):
343346
# Riccati equation solvers care and dare
344347
#
345348

346-
def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None):
349+
def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None,
350+
A_s="A", B_s="B", Q_s="Q", R_s="R", S_s="S", E_s="E"):
347351
"""X, L, G = care(A, B, Q, R=None) solves the continuous-time
348352
algebraic Riccati equation
349353
@@ -395,24 +399,6 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None):
395399
# Decide what method to use
396400
method = _slycot_or_scipy(method)
397401

398-
if method == 'slycot':
399-
# Make sure we can import required slycot routines
400-
try:
401-
from slycot import sb02md
402-
except ImportError:
403-
raise ControlSlycot("Can't find slycot module 'sb02md'")
404-
405-
try:
406-
from slycot import sb02mt
407-
except ImportError:
408-
raise ControlSlycot("Can't find slycot module 'sb02mt'")
409-
410-
# Make sure we can find the required slycot routine
411-
try:
412-
from slycot import sg02ad
413-
except ImportError:
414-
raise ControlSlycot("Can't find slycot module 'sg02ad'")
415-
416402
# Reshape input arrays
417403
A = np.array(A, ndmin=2)
418404
B = np.array(B, ndmin=2)
@@ -428,10 +414,10 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None):
428414
m = B.shape[1]
429415

430416
# Check to make sure input matrices are the right shape and type
431-
_check_shape("A", A, n, n, square=True)
432-
_check_shape("B", B, n, m)
433-
_check_shape("Q", Q, n, n, square=True, symmetric=True)
434-
_check_shape("R", R, m, m, square=True, symmetric=True)
417+
_check_shape(A_s, A, n, n, square=True)
418+
_check_shape(B_s, B, n, m)
419+
_check_shape(Q_s, Q, n, n, square=True, symmetric=True)
420+
_check_shape(R_s, R, m, m, square=True, symmetric=True)
435421

436422
# Solve the standard algebraic Riccati equation
437423
if S is None and E is None:
@@ -446,9 +432,16 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None):
446432
E, _ = np.linalg.eig(A - B @ K)
447433
return _ssmatrix(X), E, _ssmatrix(K)
448434

449-
# Create back-up of arrays needed for later computations
450-
R_ba = copy(R)
451-
B_ba = copy(B)
435+
# Make sure we can import required slycot routines
436+
try:
437+
from slycot import sb02md
438+
except ImportError:
439+
raise ControlSlycot("Can't find slycot module 'sb02md'")
440+
441+
try:
442+
from slycot import sb02mt
443+
except ImportError:
444+
raise ControlSlycot("Can't find slycot module 'sb02mt'")
452445

453446
# Solve the standard algebraic Riccati equation by calling Slycot
454447
# functions sb02mt and sb02md
@@ -458,7 +451,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None):
458451
X, rcond, w, S_o, U, A_inv = sb02md(n, A, G, Q, 'C', sort=sort)
459452

460453
# Calculate the gain matrix G
461-
G = solve(R_ba, B_ba.T) @ X
454+
G = solve(R, B.T) @ X
462455

463456
# Return the solution X, the closed-loop eigenvalues L and
464457
# the gain matrix G
@@ -471,8 +464,8 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None):
471464
E = np.eye(A.shape[0]) if E is None else np.array(E, ndmin=2)
472465

473466
# Check to make sure input matrices are the right shape and type
474-
_check_shape("E", E, n, n, square=True)
475-
_check_shape("S", S, n, m)
467+
_check_shape(E_s, E, n, n, square=True)
468+
_check_shape(S_s, S, n, m)
476469

477470
# See if we should solve this using SciPy
478471
if method == 'scipy':
@@ -485,11 +478,11 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None):
485478
eigs, _ = sp.linalg.eig(A - B @ K, E)
486479
return _ssmatrix(X), eigs, _ssmatrix(K)
487480

488-
# Create back-up of arrays needed for later computations
489-
R_b = copy(R)
490-
B_b = copy(B)
491-
E_b = copy(E)
492-
S_b = copy(S)
481+
# Make sure we can find the required slycot routine
482+
try:
483+
from slycot import sg02ad
484+
except ImportError:
485+
raise ControlSlycot("Can't find slycot module 'sg02ad'")
493486

494487
# Solve the generalized algebraic Riccati equation by calling the
495488
# Slycot function sg02ad
@@ -504,14 +497,15 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None):
504497
L = np.array([(alfar[i] + alfai[i]*1j) / beta[i] for i in range(n)])
505498

506499
# Calculate the gain matrix G
507-
G = solve(R_b, B_b.T @ X @ E_b + S_b.T)
500+
G = solve(R, B.T @ X @ E + S.T)
508501

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

513-
def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None):
514-
"""(X, L, G) = dare(A, B, Q, R) solves the discrete-time algebraic Riccati
506+
def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None,
507+
A_s="A", B_s="B", Q_s="Q", R_s="R", S_s="S", E_s="E"):
508+
"""X, L, G = dare(A, B, Q, R) solves the discrete-time algebraic Riccati
515509
equation
516510
517511
:math:`A^T X A - X - A^T X B (B^T X B + R)^{-1} B^T X A + Q = 0`
@@ -521,15 +515,17 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None):
521515
matrix G = (B^T X B + R)^-1 B^T X A and the closed loop eigenvalues L,
522516
i.e., the eigenvalues of A - B G.
523517
524-
(X, L, G) = dare(A, B, Q, R, S, E) solves the generalized discrete-time
518+
X, L, G = dare(A, B, Q, R, S, E) solves the generalized discrete-time
525519
algebraic Riccati equation
526520
527521
:math:`A^T X A - E^T X E - (A^T X B + S) (B^T X B + R)^{-1} (B^T X A + S^T) + Q = 0`
528522
529-
where A, Q and E are square matrices of the same dimension. Further, Q and
530-
R are symmetric matrices. The function returns the solution X, the gain
531-
matrix :math:`G = (B^T X B + R)^{-1} (B^T X A + S^T)` and the closed loop
532-
eigenvalues L, i.e., the eigenvalues of A - B G , E.
523+
where A, Q and E are square matrices of the same dimension. Further, Q
524+
and R are symmetric matrices. If R is None, it is set to the identity
525+
matrix. The function returns the solution X, the gain matrix :math:`G =
526+
(B^T X B + R)^{-1} (B^T X A + S^T)` and the closed loop eigenvalues L,
527+
i.e., the (generalized) eigenvalues of A - B G (with respect to E, if
528+
specified).
533529
534530
Parameters
535531
----------
@@ -575,87 +571,43 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None):
575571
m = B.shape[1]
576572

577573
# Check to make sure input matrices are the right shape and type
578-
_check_shape("A", A, n, n, square=True)
574+
_check_shape(A_s, A, n, n, square=True)
575+
_check_shape(B_s, B, n, m)
576+
_check_shape(Q_s, Q, n, n, square=True, symmetric=True)
577+
_check_shape(R_s, R, m, m, square=True, symmetric=True)
578+
if E is not None:
579+
_check_shape(E_s, E, n, n, square=True)
580+
if S is not None:
581+
_check_shape(S_s, S, n, m)
579582

580583
# Figure out how to solve the problem
581-
if method == 'scipy' and not stabilizing:
582-
raise ControlArgument(
583-
"method='scipy' not valid when stabilizing is not True")
584-
585-
elif method == 'slycot':
586-
return _dare_slycot(A, B, Q, R, S, E, stabilizing)
584+
if method == 'scipy':
585+
if not stabilizing:
586+
raise ControlArgument(
587+
"method='scipy' not valid when stabilizing is not True")
587588

588-
else:
589-
_check_shape("B", B, n, m)
590-
_check_shape("Q", Q, n, n, square=True, symmetric=True)
591-
_check_shape("R", R, m, m, square=True, symmetric=True)
592-
if E is not None:
593-
_check_shape("E", E, n, n, square=True)
594-
if S is not None:
595-
_check_shape("S", S, n, m)
596-
597-
Rmat = _ssmatrix(R)
598-
Qmat = _ssmatrix(Q)
599-
X = sp.linalg.solve_discrete_are(A, B, Qmat, Rmat, e=E, s=S)
589+
X = sp.linalg.solve_discrete_are(A, B, Q, R, e=E, s=S)
600590
if S is None:
601-
G = solve(B.T @ X @ B + Rmat, B.T @ X @ A)
591+
G = solve(B.T @ X @ B + R, B.T @ X @ A)
602592
else:
603-
G = solve(B.T @ X @ B + Rmat, B.T @ X @ A + S.T)
593+
G = solve(B.T @ X @ B + R, B.T @ X @ A + S.T)
604594
if E is None:
605595
L = eigvals(A - B @ G)
606596
else:
607597
L, _ = sp.linalg.eig(A - B @ G, E)
608598

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

611-
612-
def _dare_slycot(A, B, Q, R, S=None, E=None, stabilizing=True):
613601
# Make sure we can import required slycot routine
614-
try:
615-
from slycot import sb02md
616-
except ImportError:
617-
raise ControlSlycot("Can't find slycot module 'sb02md'")
618-
619-
try:
620-
from slycot import sb02mt
621-
except ImportError:
622-
raise ControlSlycot("Can't find slycot module 'sb02mt'")
623-
624-
# Make sure we can find the required slycot routine
625602
try:
626603
from slycot import sg02ad
627604
except ImportError:
628605
raise ControlSlycot("Can't find slycot module 'sg02ad'")
629606

630-
# Reshape input arrays
631-
A = np.array(A, ndmin=2)
632-
B = np.array(B, ndmin=2)
633-
Q = np.array(Q, ndmin=2)
634-
R = np.eye(B.shape[1]) if R is None else np.array(R, ndmin=2)
635-
636-
# Determine main dimensions
637-
n = A.shape[0]
638-
m = B.shape[1]
639-
640607
# Initialize optional matrices
641608
S = np.zeros((n, m)) if S is None else np.array(S, ndmin=2)
642609
E = np.eye(A.shape[0]) if E is None else np.array(E, ndmin=2)
643610

644-
# Check to make sure input matrices are the right shape and type
645-
_check_shape("A", A, n, n, square=True)
646-
_check_shape("B", B, n, m)
647-
_check_shape("Q", Q, n, n, square=True, symmetric=True)
648-
_check_shape("R", R, m, m, square=True, symmetric=True)
649-
_check_shape("E", E, n, n, square=True)
650-
_check_shape("S", S, n, m)
651-
652-
# Create back-up of arrays needed for later computations
653-
A_b = copy(A)
654-
R_b = copy(R)
655-
B_b = copy(B)
656-
E_b = copy(E)
657-
S_b = copy(S)
658-
659611
# Solve the generalized algebraic Riccati equation by calling the
660612
# Slycot function sg02ad
661613
sort = 'S' if stabilizing else 'U'
@@ -669,7 +621,7 @@ def _dare_slycot(A, B, Q, R, S=None, E=None, stabilizing=True):
669621
L = np.array([(alfar[i] + alfai[i]*1j) / beta[i] for i in range(n)])
670622

671623
# Calculate the gain matrix G
672-
G = solve(B_b.T @ X @ B_b + R_b, B_b.T @ X @ A_b + S_b.T)
624+
G = solve(B.T @ X @ B + R, B.T @ X @ A + S.T)
673625

674626
# Return the solution X, the closed-loop eigenvalues L and
675627
# the gain matrix G

0 commit comments

Comments
 (0)