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)
pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy