diff --git a/control/mateqn.py b/control/mateqn.py index 3a723591f..23ae1e64e 100644 --- a/control/mateqn.py +++ b/control/mateqn.py @@ -3,7 +3,7 @@ # Implementation of the functions lyap, dlyap, care and dare # for solution of Lyapunov and Riccati equations. # -# Author: Bjorn Olofsson +# Original author: Bjorn Olofsson # Copyright (c) 2011, All rights reserved. @@ -162,6 +162,7 @@ def lyap(A, Q, C=None, E=None, method=None): _check_shape("Q", Q, n, n, square=True, symmetric=True) if method == 'scipy': + # Solve the Lyapunov equation using SciPy return sp.linalg.solve_continuous_lyapunov(A, -Q) # Solve the Lyapunov equation by calling Slycot function sb03md @@ -177,6 +178,7 @@ def lyap(A, Q, C=None, E=None, method=None): _check_shape("C", C, n, m) if method == 'scipy': + # Solve the Sylvester equation using SciPy return sp.linalg.solve_sylvester(A, Q, -C) # Solve the Sylvester equation by calling the Slycot function sb04md @@ -293,6 +295,7 @@ def dlyap(A, Q, C=None, E=None, method=None): _check_shape("Q", Q, n, n, square=True, symmetric=True) if method == 'scipy': + # Solve the Lyapunov equation using SciPy return sp.linalg.solve_discrete_lyapunov(A, Q) # Solve the Lyapunov equation by calling the Slycot function sb03md @@ -343,7 +346,8 @@ def dlyap(A, Q, C=None, E=None, method=None): # Riccati equation solvers care and dare # -def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None): +def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, + A_s="A", B_s="B", Q_s="Q", R_s="R", S_s="S", E_s="E"): """X, L, G = care(A, B, Q, R=None) solves the continuous-time algebraic Riccati equation @@ -395,24 +399,6 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None): # Decide what method to use method = _slycot_or_scipy(method) - if method == 'slycot': - # Make sure we can import required slycot routines - try: - from slycot import sb02md - except ImportError: - raise ControlSlycot("Can't find slycot module 'sb02md'") - - try: - from slycot import sb02mt - except ImportError: - raise ControlSlycot("Can't find slycot module 'sb02mt'") - - # Make sure we can find the required slycot routine - try: - from slycot import sg02ad - except ImportError: - raise ControlSlycot("Can't find slycot module 'sg02ad'") - # Reshape input arrays A = np.array(A, ndmin=2) 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): m = B.shape[1] # Check to make sure input matrices are the right shape and type - _check_shape("A", A, n, n, square=True) - _check_shape("B", B, n, m) - _check_shape("Q", Q, n, n, square=True, symmetric=True) - _check_shape("R", R, m, m, square=True, symmetric=True) + _check_shape(A_s, A, n, n, square=True) + _check_shape(B_s, B, n, m) + _check_shape(Q_s, Q, n, n, square=True, symmetric=True) + _check_shape(R_s, R, m, m, square=True, symmetric=True) # Solve the standard algebraic Riccati equation 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): E, _ = np.linalg.eig(A - B @ K) return _ssmatrix(X), E, _ssmatrix(K) - # Create back-up of arrays needed for later computations - R_ba = copy(R) - B_ba = copy(B) + # Make sure we can import required slycot routines + try: + from slycot import sb02md + except ImportError: + raise ControlSlycot("Can't find slycot module 'sb02md'") + + try: + from slycot import sb02mt + except ImportError: + raise ControlSlycot("Can't find slycot module 'sb02mt'") # Solve the standard algebraic Riccati equation by calling Slycot # functions sb02mt and sb02md @@ -458,7 +451,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None): X, rcond, w, S_o, U, A_inv = sb02md(n, A, G, Q, 'C', sort=sort) # Calculate the gain matrix G - G = solve(R_ba, B_ba.T) @ X + G = solve(R, B.T) @ X # Return the solution X, the closed-loop eigenvalues L and # the gain matrix G @@ -471,8 +464,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("E", E, n, n, square=True) - _check_shape("S", S, n, m) + _check_shape(E_s, E, n, n, square=True) + _check_shape(S_s, S, n, m) # See if we should solve this using SciPy if method == 'scipy': @@ -485,11 +478,11 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None): eigs, _ = sp.linalg.eig(A - B @ K, E) return _ssmatrix(X), eigs, _ssmatrix(K) - # Create back-up of arrays needed for later computations - R_b = copy(R) - B_b = copy(B) - E_b = copy(E) - S_b = copy(S) + # Make sure we can find the required slycot routine + try: + from slycot import sg02ad + except ImportError: + raise ControlSlycot("Can't find slycot module 'sg02ad'") # Solve the generalized algebraic Riccati equation by calling the # Slycot function sg02ad @@ -504,14 +497,15 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None): L = np.array([(alfar[i] + alfai[i]*1j) / beta[i] for i in range(n)]) # Calculate the gain matrix G - G = solve(R_b, B_b.T @ X @ E_b + S_b.T) + G = solve(R, B.T @ X @ E + S.T) # Return the solution X, the closed-loop eigenvalues L and # the gain matrix G return _ssmatrix(X), L, _ssmatrix(G) -def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None): - """(X, L, G) = dare(A, B, Q, R) solves the discrete-time algebraic Riccati +def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None, + A_s="A", B_s="B", Q_s="Q", R_s="R", S_s="S", E_s="E"): + """X, L, G = dare(A, B, Q, R) solves the discrete-time algebraic Riccati equation :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): matrix G = (B^T X B + R)^-1 B^T X A and the closed loop eigenvalues L, i.e., the eigenvalues of A - B G. - (X, L, G) = dare(A, B, Q, R, S, E) solves the generalized discrete-time + X, L, G = dare(A, B, Q, R, S, E) solves the generalized discrete-time algebraic Riccati equation :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` - where A, Q and E are square matrices of the same dimension. Further, Q and - R are symmetric matrices. The function returns the solution X, the gain - matrix :math:`G = (B^T X B + R)^{-1} (B^T X A + S^T)` and the closed loop - eigenvalues L, i.e., the eigenvalues of A - B G , E. + where A, Q and E are square matrices of the same dimension. Further, Q + and R are symmetric matrices. If R is None, it is set to the identity + matrix. The function returns the solution X, the gain matrix :math:`G = + (B^T X B + R)^{-1} (B^T X A + S^T)` and the closed loop eigenvalues L, + i.e., the (generalized) eigenvalues of A - B G (with respect to E, if + specified). Parameters ---------- @@ -575,32 +571,26 @@ 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("A", A, n, n, square=True) + _check_shape(A_s, A, n, n, square=True) + _check_shape(B_s, B, n, m) + _check_shape(Q_s, Q, n, n, square=True, symmetric=True) + _check_shape(R_s, R, m, m, square=True, symmetric=True) + if E is not None: + _check_shape(E_s, E, n, n, square=True) + if S is not None: + _check_shape(S_s, S, n, m) # Figure out how to solve the problem - if method == 'scipy' and not stabilizing: - raise ControlArgument( - "method='scipy' not valid when stabilizing is not True") - - elif method == 'slycot': - return _dare_slycot(A, B, Q, R, S, E, stabilizing) + if method == 'scipy': + if not stabilizing: + raise ControlArgument( + "method='scipy' not valid when stabilizing is not True") - else: - _check_shape("B", B, n, m) - _check_shape("Q", Q, n, n, square=True, symmetric=True) - _check_shape("R", R, m, m, square=True, symmetric=True) - if E is not None: - _check_shape("E", E, n, n, square=True) - if S is not None: - _check_shape("S", S, n, m) - - Rmat = _ssmatrix(R) - Qmat = _ssmatrix(Q) - X = sp.linalg.solve_discrete_are(A, B, Qmat, Rmat, e=E, s=S) + X = sp.linalg.solve_discrete_are(A, B, Q, R, e=E, s=S) if S is None: - G = solve(B.T @ X @ B + Rmat, B.T @ X @ A) + G = solve(B.T @ X @ B + R, B.T @ X @ A) else: - G = solve(B.T @ X @ B + Rmat, B.T @ X @ A + S.T) + G = solve(B.T @ X @ B + R, B.T @ X @ A + S.T) if E is None: L = eigvals(A - B @ G) else: @@ -608,54 +598,16 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None): return _ssmatrix(X), L, _ssmatrix(G) - -def _dare_slycot(A, B, Q, R, S=None, E=None, stabilizing=True): # Make sure we can import required slycot routine - try: - from slycot import sb02md - except ImportError: - raise ControlSlycot("Can't find slycot module 'sb02md'") - - try: - from slycot import sb02mt - except ImportError: - raise ControlSlycot("Can't find slycot module 'sb02mt'") - - # Make sure we can find the required slycot routine try: from slycot import sg02ad except ImportError: raise ControlSlycot("Can't find slycot module 'sg02ad'") - # Reshape input arrays - A = np.array(A, ndmin=2) - B = np.array(B, ndmin=2) - Q = np.array(Q, ndmin=2) - R = np.eye(B.shape[1]) if R is None else np.array(R, ndmin=2) - - # Determine main dimensions - n = A.shape[0] - m = B.shape[1] - # Initialize optional matrices S = np.zeros((n, m)) if S is None else np.array(S, ndmin=2) 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("A", A, n, n, square=True) - _check_shape("B", B, n, m) - _check_shape("Q", Q, n, n, square=True, symmetric=True) - _check_shape("R", R, m, m, square=True, symmetric=True) - _check_shape("E", E, n, n, square=True) - _check_shape("S", S, n, m) - - # Create back-up of arrays needed for later computations - A_b = copy(A) - R_b = copy(R) - B_b = copy(B) - E_b = copy(E) - S_b = copy(S) - # Solve the generalized algebraic Riccati equation by calling the # Slycot function sg02ad sort = 'S' if stabilizing else 'U' @@ -669,7 +621,7 @@ def _dare_slycot(A, B, Q, R, S=None, E=None, stabilizing=True): L = np.array([(alfar[i] + alfai[i]*1j) / beta[i] for i in range(n)]) # Calculate the gain matrix G - G = solve(B_b.T @ X @ B_b + R_b, B_b.T @ X @ A_b + S_b.T) + G = solve(B.T @ X @ B + R, B.T @ X @ A + S.T) # Return the solution X, the closed-loop eigenvalues L and # the gain matrix G diff --git a/control/statefbk.py b/control/statefbk.py index e710f6b13..ef16cbfff 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -43,9 +43,9 @@ import numpy as np from . import statesp -from .mateqn import care -from .statesp import _ssmatrix, _convert_to_statespace -from .lti import LTI +from .mateqn import care, dare, _check_shape +from .statesp import StateSpace, _ssmatrix, _convert_to_statespace +from .lti import LTI, isdtime, isctime from .exception import ControlSlycot, ControlArgument, ControlDimension, \ ControlNotImplemented @@ -69,7 +69,7 @@ def sb03md(n, C, A, U, dico, job='X',fact='N',trana='N',ldwork=None): __all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr', 'lqe', - 'acker'] + 'dlqr', 'dlqe', 'acker'] # Pole placement @@ -260,7 +260,7 @@ def place_varga(A, B, p, dtime=False, alpha=None): # contributed by Sawyer B. Fuller def lqe(*args, **keywords): - """lqe(A, G, C, Q, R, [, N]) + """lqe(A, G, C, QN, RN, [, NN]) Linear quadratic estimator design (Kalman filter) for continuous-time systems. Given the system @@ -272,7 +272,7 @@ def lqe(*args, **keywords): with unbiased process noise w and measurement noise v with covariances - .. math:: E{ww'} = Q, E{vv'} = R, E{wv'} = N + .. math:: E{ww'} = QN, E{vv'} = RN, E{wv'} = NN The lqe() function computes the observer gain matrix L such that the stationary (non-time-varying) Kalman filter @@ -280,18 +280,18 @@ def lqe(*args, **keywords): .. math:: x_e = A x_e + B u + L(y - C x_e - D u) produces a state estimate x_e that minimizes the expected squared error - using the sensor measurements y. The noise cross-correlation `N` is + using the sensor measurements y. The noise cross-correlation `NN` is set to zero when omitted. The function can be called with either 3, 4, 5, or 6 arguments: - * ``L, P, E = lqe(sys, Q, R)`` - * ``L, P, E = lqe(sys, Q, R, N)`` - * ``L, P, E = lqe(A, G, C, Q, R)`` - * ``L, P, E = lqe(A, B, C, Q, R, N)`` + * ``L, P, E = lqe(sys, QN, RN)`` + * ``L, P, E = lqe(sys, QN, RN, NN)`` + * ``L, P, E = lqe(A, G, C, QN, RN)`` + * ``L, P, E = lqe(A, G, C, QN, RN, NN)`` - where `sys` is an `LTI` object, and `A`, `G`, `C`, `Q`, `R`, and `N` are - 2D arrays or matrices of appropriate dimension. + where `sys` is an `LTI` object, and `A`, `G`, `C`, `QN`, `RN`, and `NN` + are 2D arrays or matrices of appropriate dimension. Parameters ---------- @@ -300,9 +300,9 @@ def lqe(*args, **keywords): sys : LTI (StateSpace or TransferFunction) Linear I/O system, with the process noise input taken as the system input. - Q, R : 2D array_like + QN, RN : 2D array_like Process and sensor noise covariance matrices - N : 2D array, optional + NN : 2D array, optional Cross covariance matrix. Not currently implemented. method : str, optional Set the method used for computing the result. Current methods are @@ -325,17 +325,22 @@ def lqe(*args, **keywords): Notes ----- - The return type for 2D arrays depends on the default class set for - state space operations. See :func:`~control.use_numpy_matrix`. + 1. If the first argument is an LTI object, then this object will be used + to define the dynamics, noise and output matrices. Furthermore, if + the LTI object corresponds to a discrete time system, the ``dlqe()`` + function will be called. + + 2. The return type for 2D arrays depends on the default class set for + state space operations. See :func:`~control.use_numpy_matrix`. Examples -------- - >>> L, P, E = lqe(A, G, C, Q, R) - >>> L, P, E = lqe(A, G, C, Q, R, N) + >>> L, P, E = lqe(A, G, C, QN, RN) + >>> L, P, E = lqe(A, G, C, Q, RN, NN) See Also -------- - lqr + lqr, dlqe, dlqr """ @@ -356,19 +361,23 @@ def lqe(*args, **keywords): if (len(args) < 3): raise ControlArgument("not enough input arguments") - try: - sys = args[0] # Treat the first argument as a system - if isinstance(sys, LTI): - # Convert LTI system to state space - sys = _convert_to_statespace(sys) - - # Extract A, G (assume disturbances come through input), and C - A = np.array(sys.A, ndmin=2, dtype=float) - G = np.array(sys.B, ndmin=2, dtype=float) - C = np.array(sys.C, ndmin=2, dtype=float) + # If we were passed a discrete time system as the first arg, use dlqe() + if isinstance(args[0], LTI) and isdtime(args[0], strict=True): + # Call dlqe + return dlqe(*args, **keywords) + + # If we were passed a state space system, use that to get system matrices + if isinstance(args[0], StateSpace): + A = np.array(args[0].A, ndmin=2, dtype=float) + G = np.array(args[0].B, ndmin=2, dtype=float) + C = np.array(args[0].C, ndmin=2, dtype=float) index = 1 - except AttributeError: + elif isinstance(args[0], LTI): + # Don't allow other types of LTI systems + raise ControlArgument("LTI system must be in state space form") + + else: # Arguments should be A and B matrices A = np.array(args[0], ndmin=2, dtype=float) G = np.array(args[1], ndmin=2, dtype=float) @@ -376,34 +385,149 @@ def lqe(*args, **keywords): index = 3 # Get the weighting matrices (converting to matrices, if needed) - Q = np.array(args[index], ndmin=2, dtype=float) - R = np.array(args[index+1], ndmin=2, dtype=float) + QN = np.array(args[index], ndmin=2, dtype=float) + RN = np.array(args[index+1], ndmin=2, dtype=float) # Get the cross-covariance matrix, if given if (len(args) > index + 2): - N = np.array(args[index+2], ndmin=2, dtype=float) + NN = np.array(args[index+2], ndmin=2, dtype=float) raise ControlNotImplemented("cross-covariance not implemented") else: - N = np.zeros((Q.shape[0], R.shape[1])) - - # Check dimensions for consistency - nstates = A.shape[0] - ninputs = G.shape[1] - noutputs = C.shape[0] - if (A.shape[0] != nstates or A.shape[1] != nstates or - G.shape[0] != nstates or C.shape[1] != nstates): - raise ControlDimension("inconsistent system dimensions") + # For future use (not currently used below) + NN = np.zeros((QN.shape[0], RN.shape[1])) - elif (Q.shape[0] != ninputs or Q.shape[1] != ninputs or - R.shape[0] != noutputs or R.shape[1] != noutputs or - N.shape[0] != ninputs or N.shape[1] != noutputs): - raise ControlDimension("incorrect covariance matrix dimensions") + # Check dimensions of G (needed before calling care()) + _check_shape("QN", QN, G.shape[1], G.shape[1]) - P, E, LT = care(A.T, C.T, G @ Q @ G.T, R, method=method) + # Compute the result (dimension and symmetry checking done in care()) + P, E, LT = care(A.T, C.T, G @ QN @ G.T, RN, method=method, + B_s="C", Q_s="QN", R_s="RN", S_s="NN") return _ssmatrix(LT.T), _ssmatrix(P), E +# contributed by Sawyer B. Fuller +def dlqe(*args, **keywords): + """dlqe(A, G, C, QN, RN, [, N]) + + Linear quadratic estimator design (Kalman filter) for discrete-time + systems. Given the system + + .. math:: + + x[n+1] &= Ax[n] + Bu[n] + Gw[n] \\\\ + y[n] &= Cx[n] + Du[n] + v[n] + + with unbiased process noise w and measurement noise v with covariances + + .. math:: E{ww'} = QN, E{vv'} = RN, E{wv'} = NN + + The dlqe() function computes the observer gain matrix L such that the + stationary (non-time-varying) Kalman filter + + .. math:: x_e[n+1] = A x_e[n] + B u[n] + L(y[n] - C x_e[n] - D u[n]) + + produces a state estimate x_e[n] that minimizes the expected squared error + using the sensor measurements y. The noise cross-correlation `NN` is + set to zero when omitted. + + Parameters + ---------- + A, G : 2D array_like + Dynamics and noise input matrices + QN, RN : 2D array_like + Process and sensor noise covariance matrices + NN : 2D array, optional + Cross covariance matrix (not yet supported) + method : str, optional + Set the method used for computing the result. Current methods are + 'slycot' and 'scipy'. If set to None (default), try 'slycot' first + and then 'scipy'. + + Returns + ------- + L : 2D array (or matrix) + Kalman estimator gain + P : 2D array (or matrix) + Solution to Riccati equation + + .. math:: + + A P + P A^T - (P C^T + G N) R^{-1} (C P + N^T G^T) + G Q G^T = 0 + + E : 1D array + Eigenvalues of estimator poles eig(A - L C) + + Notes + ----- + The return type for 2D arrays depends on the default class set for + state space operations. See :func:`~control.use_numpy_matrix`. + + Examples + -------- + >>> L, P, E = dlqe(A, G, C, QN, RN) + >>> L, P, E = dlqe(A, G, C, QN, RN, NN) + + See Also + -------- + dlqr, lqe, lqr + + """ + + # + # Process the arguments and figure out what inputs we received + # + + # Get the method to use (if specified as a keyword) + method = keywords.get('method', None) + + # Get the system description + if (len(args) < 3): + raise ControlArgument("not enough input arguments") + + # If we were passed a continus time system as the first arg, raise error + if isinstance(args[0], LTI) and isctime(args[0], strict=True): + raise ControlArgument("dlqr() called with a continuous time system") + + # If we were passed a state space system, use that to get system matrices + if isinstance(args[0], StateSpace): + A = np.array(args[0].A, ndmin=2, dtype=float) + G = np.array(args[0].B, ndmin=2, dtype=float) + C = np.array(args[0].C, ndmin=2, dtype=float) + index = 1 + + elif isinstance(args[0], LTI): + # Don't allow other types of LTI systems + raise ControlArgument("LTI system must be in state space form") + + else: + # Arguments should be A and B matrices + A = np.array(args[0], ndmin=2, dtype=float) + G = np.array(args[1], ndmin=2, dtype=float) + C = np.array(args[2], ndmin=2, dtype=float) + index = 3 + + # Get the weighting matrices (converting to matrices, if needed) + QN = np.array(args[index], ndmin=2, dtype=float) + RN = np.array(args[index+1], ndmin=2, dtype=float) + + # TODO: incorporate cross-covariance NN, something like this, + # which doesn't work for some reason + # if NN is None: + # NN = np.zeros(QN.size(0),RN.size(1)) + # NG = G @ NN + if len(args) > index + 2: + NN = np.array(args[index+2], ndmin=2, dtype=float) + raise ControlNotImplemented("cross-covariance not yet implememented") + + # Check dimensions of G (needed before calling care()) + _check_shape("QN", QN, G.shape[1], G.shape[1]) + + # Compute the result (dimension and symmetry checking done in dare()) + P, E, LT = dare(A.T, C.T, G @ QN @ G.T, RN, method=method, + B_s="C", Q_s="QN", R_s="RN", S_s="NN") + return _ssmatrix(LT.T), _ssmatrix(P), E + # Contributed by Roberto Bucher def acker(A, B, poles): """Pole placement using Ackermann method @@ -458,7 +582,7 @@ def lqr(*args, **keywords): Linear quadratic regulator design The lqr() function computes the optimal state feedback controller - that minimizes the quadratic cost + u = -K x that minimizes the quadratic cost .. math:: J = \\int_0^\\infty (x' Q x + u' R u + 2 x' N u) dt @@ -476,8 +600,8 @@ def lqr(*args, **keywords): ---------- A, B : 2D array_like Dynamics and input matrices - sys : LTI (StateSpace or TransferFunction) - Linear I/O system + sys : LTI StateSpace system + Linear system Q, R : 2D array State and input weight matrices N : 2D array, optional @@ -498,12 +622,17 @@ def lqr(*args, **keywords): See Also -------- - lqe + lqe, dlqr, dlqe Notes ----- - The return type for 2D arrays depends on the default class set for - state space operations. See :func:`~control.use_numpy_matrix`. + 1. If the first argument is an LTI object, then this object will be used + to define the dynamics and input matrices. Furthermore, if the LTI + object corresponds to a discrete time system, the ``dlqr()`` function + will be called. + + 2. The return type for 2D arrays depends on the default class set for + state space operations. See :func:`~control.use_numpy_matrix`. Examples -------- @@ -511,7 +640,6 @@ def lqr(*args, **keywords): >>> K, S, E = lqr(A, B, Q, R, [N]) """ - # # Process the arguments and figure out what inputs we received # @@ -523,13 +651,22 @@ def lqr(*args, **keywords): if (len(args) < 3): raise ControlArgument("not enough input arguments") - try: - # If this works, we were (probably) passed a system as the - # first argument; extract A and B + # If we were passed a discrete time system as the first arg, use dlqr() + if isinstance(args[0], LTI) and isdtime(args[0], strict=True): + # Call dlqr + return dlqr(*args, **keywords) + + # If we were passed a state space system, use that to get system matrices + if isinstance(args[0], StateSpace): A = np.array(args[0].A, ndmin=2, dtype=float) B = np.array(args[0].B, ndmin=2, dtype=float) index = 1 - except AttributeError: + + elif isinstance(args[0], LTI): + # Don't allow other types of LTI systems + raise ControlArgument("LTI system must be in state space form") + + else: # Arguments should be A and B matrices A = np.array(args[0], ndmin=2, dtype=float) B = np.array(args[1], ndmin=2, dtype=float) @@ -543,11 +680,111 @@ def lqr(*args, **keywords): else: N = None - # Solve continuous algebraic Riccati equation - X, L, G = care(A, B, Q, R, N, None, method=method) + # Compute the result (dimension and symmetry checking done in care()) + X, L, G = care(A, B, Q, R, N, None, method=method, S_s="N") return G, X, L +def dlqr(*args, **keywords): + """dlqr(A, B, Q, R[, N]) + + Discrete-time linear quadratic regulator design + + The dlqr() function computes the optimal state feedback controller + u[n] = - K x[n] that minimizes the quadratic cost + + .. math:: J = \\Sum_0^\\infty (x[n]' Q x[n] + u[n]' R u[n] + 2 x[n]' N u[n]) + + The function can be called with either 3, 4, or 5 arguments: + + * ``dlqr(dsys, Q, R)`` + * ``dlqr(dsys, Q, R, N)`` + * ``dlqr(A, B, Q, R)`` + * ``dlqr(A, B, Q, R, N)`` + + where `dsys` is a discrete-time :class:`StateSpace` system, and `A`, `B`, + `Q`, `R`, and `N` are 2d arrays of appropriate dimension (`dsys.dt` must + not be 0.) + + Parameters + ---------- + A, B : 2D array + Dynamics and input matrices + dsys : LTI :class:`StateSpace` + Discrete-time linear system + Q, R : 2D array + State and input weight matrices + N : 2D array, optional + Cross weight matrix + + Returns + ------- + K : 2D array (or matrix) + State feedback gains + S : 2D array (or matrix) + Solution to Riccati equation + E : 1D array + Eigenvalues of the closed loop system + + See Also + -------- + lqr, lqe, dlqe + + Notes + ----- + The return type for 2D arrays depends on the default class set for + state space operations. See :func:`~control.use_numpy_matrix`. + + Examples + -------- + >>> K, S, E = dlqr(dsys, Q, R, [N]) + >>> K, S, E = dlqr(A, B, Q, R, [N]) + """ + + # + # Process the arguments and figure out what inputs we received + # + + # Get the method to use (if specified as a keyword) + method = keywords.get('method', None) + + # Get the system description + if (len(args) < 3): + raise ControlArgument("not enough input arguments") + + # If we were passed a continus time system as the first arg, raise error + if isinstance(args[0], LTI) and isctime(args[0], strict=True): + raise ControlArgument("dsys must be discrete time (dt != 0)") + + # If we were passed a state space system, use that to get system matrices + if isinstance(args[0], StateSpace): + A = np.array(args[0].A, ndmin=2, dtype=float) + B = np.array(args[0].B, ndmin=2, dtype=float) + index = 1 + + elif isinstance(args[0], LTI): + # Don't allow other types of LTI systems + raise ControlArgument("LTI system must be in state space form") + + else: + # Arguments should be A and B matrices + A = np.array(args[0], ndmin=2, dtype=float) + B = np.array(args[1], ndmin=2, dtype=float) + index = 2 + + # Get the weighting matrices (converting to matrices, if needed) + Q = np.array(args[index], ndmin=2, dtype=float) + R = np.array(args[index+1], ndmin=2, dtype=float) + if (len(args) > index + 2): + N = np.array(args[index+2], ndmin=2, dtype=float) + else: + N = np.zeros((Q.shape[0], R.shape[1])) + + # Compute the result (dimension and symmetry checking done in dare()) + S, E, K = dare(A, B, Q, R, N, method=method, S_s="N") + return _ssmatrix(K), _ssmatrix(S), E + + def ctrb(A, B): """Controllabilty matrix diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index fad848da2..73410312f 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -11,7 +11,8 @@ from control.exception import ControlDimension, ControlSlycot, \ ControlArgument, slycot_check from control.mateqn import care, dare -from control.statefbk import ctrb, obsv, place, place_varga, lqr, gram, acker +from control.statefbk import (ctrb, obsv, place, place_varga, lqr, dlqr, + lqe, dlqe, gram, acker) from control.tests.conftest import (slycotonly, check_deprecated_matrix, ismatarrayout, asmatarrayout) @@ -77,7 +78,7 @@ def testCtrbObsvDuality(self, matarrayin): Wc = ctrb(A, B) A = np.transpose(A) C = np.transpose(B) - Wo = np.transpose(obsv(A, C)); + Wo = np.transpose(obsv(A, C)) np.testing.assert_array_almost_equal(Wc,Wo) @slycotonly @@ -165,7 +166,7 @@ def testAcker(self, fixedseed): continue # Place the poles at random locations - des = rss(states, 1, 1); + des = rss(states, 1, 1) poles = pole(des) # Now place the poles using acker @@ -297,7 +298,6 @@ def testPlace_varga_discrete_partial_eigs(self, matarrayin): P_placed = np.linalg.eigvals(A - B @ K) self.checkPlaced(P_expected, P_placed) - def check_LQR(self, K, S, poles, Q, R): S_expected = asmatarrayout(np.sqrt(Q @ R)) K_expected = asmatarrayout(S_expected / R) @@ -306,6 +306,14 @@ def check_LQR(self, K, S, poles, Q, R): np.testing.assert_array_almost_equal(K, K_expected) np.testing.assert_array_almost_equal(poles, poles_expected) + def check_DLQR(self, K, S, poles, Q, R): + S_expected = asmatarrayout(Q) + K_expected = asmatarrayout(0) + poles_expected = -np.squeeze(np.asarray(K_expected)) + np.testing.assert_array_almost_equal(S, S_expected) + np.testing.assert_array_almost_equal(K, K_expected) + np.testing.assert_array_almost_equal(poles, poles_expected) + @pytest.mark.parametrize("method", [None, 'slycot', 'scipy']) def test_LQR_integrator(self, matarrayin, matarrayout, method): if method == 'slycot' and not slycot_check(): @@ -323,16 +331,32 @@ def test_LQR_3args(self, matarrayin, matarrayout, method): K, S, poles = lqr(sys, Q, R, method=method) self.check_LQR(K, S, poles, Q, R) - def test_lqr_badmethod(self): + @pytest.mark.parametrize("method", [None, 'slycot', 'scipy']) + def test_DLQR_3args(self, matarrayin, matarrayout, method): + if method == 'slycot' and not slycot_check(): + return + dsys = ss(0., 1., 1., 0., .1) + Q, R = (matarrayin([[X]]) for X in [10., 2.]) + K, S, poles = dlqr(dsys, Q, R, method=method) + self.check_DLQR(K, S, poles, Q, R) + + def test_DLQR_4args(self, matarrayin, matarrayout): + A, B, Q, R = (matarrayin([[X]]) for X in [0., 1., 10., 2.]) + K, S, poles = dlqr(A, B, Q, R) + self.check_DLQR(K, S, poles, Q, R) + + @pytest.mark.parametrize("cdlqr", [lqr, dlqr]) + def test_lqr_badmethod(self, cdlqr): A, B, Q, R = 0, 1, 10, 2 with pytest.raises(ControlArgument, match="Unknown method"): - K, S, poles = lqr(A, B, Q, R, method='nosuchmethod') + K, S, poles = cdlqr(A, B, Q, R, method='nosuchmethod') - def test_lqr_slycot_not_installed(self): + @pytest.mark.parametrize("cdlqr", [lqr, dlqr]) + def test_lqr_slycot_not_installed(self, cdlqr): A, B, Q, R = 0, 1, 10, 2 if not slycot_check(): with pytest.raises(ControlSlycot, match="Can't find slycot"): - K, S, poles = lqr(A, B, Q, R, method='slycot') + K, S, poles = cdlqr(A, B, Q, R, method='slycot') @pytest.mark.xfail(reason="warning not implemented") def testLQR_warning(self): @@ -353,10 +377,11 @@ def testLQR_warning(self): with pytest.warns(UserWarning): (K, S, E) = lqr(A, B, Q, R, N) - @slycotonly - def test_lqr_call_format(self): + @pytest.mark.parametrize("cdlqr", [lqr, dlqr]) + def test_lqr_call_format(self, cdlqr): # Create a random state space system for testing sys = rss(2, 3, 2) + sys.dt = None # treat as either continuous or discrete time # Weighting matrices Q = np.eye(sys.nstates) @@ -364,27 +389,56 @@ def test_lqr_call_format(self): N = np.zeros((sys.nstates, sys.ninputs)) # Standard calling format - Kref, Sref, Eref = lqr(sys.A, sys.B, Q, R) + Kref, Sref, Eref = cdlqr(sys.A, sys.B, Q, R) # Call with system instead of matricees - K, S, E = lqr(sys, Q, R) + K, S, E = cdlqr(sys, Q, R) np.testing.assert_array_almost_equal(Kref, K) np.testing.assert_array_almost_equal(Sref, S) np.testing.assert_array_almost_equal(Eref, E) # Pass a cross-weighting matrix - K, S, E = lqr(sys, Q, R, N) + K, S, E = cdlqr(sys, Q, R, N) np.testing.assert_array_almost_equal(Kref, K) np.testing.assert_array_almost_equal(Sref, S) np.testing.assert_array_almost_equal(Eref, E) # Inconsistent system dimensions with pytest.raises(ct.ControlDimension, match="Incompatible dimen"): - K, S, E = lqr(sys.A, sys.C, Q, R) + K, S, E = cdlqr(sys.A, sys.C, Q, R) - # incorrect covariance matrix dimensions + # Incorrect covariance matrix dimensions with pytest.raises(ct.ControlDimension, match="Q must be a square"): - K, S, E = lqr(sys.A, sys.B, sys.C, R, Q) + K, S, E = cdlqr(sys.A, sys.B, sys.C, R, Q) + + # Too few input arguments + with pytest.raises(ct.ControlArgument, match="not enough input"): + K, S, E = cdlqr(sys.A, sys.B) + + # First argument is the wrong type (use SISO for non-slycot tests) + sys_tf = tf(rss(3, 1, 1)) + sys_tf.dt = None # treat as either continuous or discrete time + with pytest.raises(ct.ControlArgument, match="LTI system must be"): + K, S, E = cdlqr(sys_tf, Q, R) + + @pytest.mark.xfail(reason="warning not implemented") + def testDLQR_warning(self): + """Test dlqr() + + Make sure we get a warning if [Q N;N' R] is not positive semi-definite + """ + # from matlab_test siso.ss2 (testLQR); probably not referenced before + # not yet implemented check + A = np.array([[-2, 3, 1], + [-1, 0, 0], + [0, 1, 0]]) + B = np.array([[-1, 0, 0]]).T + Q = np.eye(3) + R = np.eye(1) + N = np.array([[1, 1, 2]]).T + # assert any(np.linalg.eigvals(np.block([[Q, N], [N.T, R]])) < 0) + with pytest.warns(UserWarning): + (K, S, E) = dlqr(A, B, Q, R, N) def check_LQE(self, L, P, poles, G, QN, RN): P_expected = asmatarrayout(np.sqrt(G @ QN @ G @ RN)) @@ -394,14 +448,20 @@ def check_LQE(self, L, P, poles, G, QN, RN): np.testing.assert_array_almost_equal(L, L_expected) np.testing.assert_array_almost_equal(poles, poles_expected) - def test_LQE(self, matarrayin): + @pytest.mark.parametrize("method", [None, 'slycot', 'scipy']) + def test_LQE(self, matarrayin, method): + if method == 'slycot' and not slycot_check(): + return + A, G, C, QN, RN = (matarrayin([[X]]) for X in [0., .1, 1., 10., 2.]) - L, P, poles = lqe(A, G, C, QN, RN) + L, P, poles = lqe(A, G, C, QN, RN, method=method) self.check_LQE(L, P, poles, G, QN, RN) - def test_lqe_call_format(self): + @pytest.mark.parametrize("cdlqe", [lqe, dlqe]) + def test_lqe_call_format(self, cdlqe): # Create a random state space system for testing sys = rss(4, 3, 2) + sys.dt = None # treat as either continuous or discrete time # Covariance matrices Q = np.eye(sys.ninputs) @@ -409,34 +469,55 @@ def test_lqe_call_format(self): N = np.zeros((sys.ninputs, sys.noutputs)) # Standard calling format - Lref, Pref, Eref = lqe(sys.A, sys.B, sys.C, Q, R) + Lref, Pref, Eref = cdlqe(sys.A, sys.B, sys.C, Q, R) # Call with system instead of matricees - L, P, E = lqe(sys, Q, R) + L, P, E = cdlqe(sys, Q, R) np.testing.assert_array_almost_equal(Lref, L) np.testing.assert_array_almost_equal(Pref, P) np.testing.assert_array_almost_equal(Eref, E) - # Compare state space and transfer function (SISO only) - sys_siso = rss(4, 1, 1) - L_ss, P_ss, E_ss = lqe(sys_siso, np.eye(1), np.eye(1)) - L_tf, P_tf, E_tf = lqe(tf(sys_siso), np.eye(1), np.eye(1)) - np.testing.assert_array_almost_equal(np.sort(E_ss), np.sort(E_tf)) - # Make sure we get an error if we specify N with pytest.raises(ct.ControlNotImplemented): - L, P, E = lqe(sys, Q, R, N) + L, P, E = cdlqe(sys, Q, R, N) # Inconsistent system dimensions - with pytest.raises(ct.ControlDimension, match="inconsistent system"): - L, P, E = lqe(sys.A, sys.C, sys.B, Q, R) + with pytest.raises(ct.ControlDimension, match="Incompatible"): + L, P, E = cdlqe(sys.A, sys.C, sys.B, Q, R) + + # Incorrect covariance matrix dimensions + with pytest.raises(ct.ControlDimension, match="Incompatible"): + L, P, E = cdlqe(sys.A, sys.B, sys.C, R, Q) + + # Too few input arguments + with pytest.raises(ct.ControlArgument, match="not enough input"): + L, P, E = cdlqe(sys.A, sys.C) + + # First argument is the wrong type (use SISO for non-slycot tests) + sys_tf = tf(rss(3, 1, 1)) + sys_tf.dt = None # treat as either continuous or discrete time + with pytest.raises(ct.ControlArgument, match="LTI system must be"): + L, P, E = cdlqe(sys_tf, Q, R) + + def check_DLQE(self, L, P, poles, G, QN, RN): + P_expected = asmatarrayout(G.dot(QN).dot(G)) + L_expected = asmatarrayout(0) + poles_expected = -np.squeeze(np.asarray(L_expected)) + np.testing.assert_array_almost_equal(P, P_expected) + np.testing.assert_array_almost_equal(L, L_expected) + np.testing.assert_array_almost_equal(poles, poles_expected) - # incorrect covariance matrix dimensions - with pytest.raises(ct.ControlDimension, match="incorrect covariance"): - L, P, E = lqe(sys.A, sys.B, sys.C, R, Q) + @pytest.mark.parametrize("method", [None, 'slycot', 'scipy']) + def test_DLQE(self, matarrayin, method): + if method == 'slycot' and not slycot_check(): + return + + A, G, C, QN, RN = (matarrayin([[X]]) for X in [0., .1, 1., 10., 2.]) + L, P, poles = dlqe(A, G, C, QN, RN, method=method) + self.check_DLQE(L, P, poles, G, QN, RN) def test_care(self, matarrayin): - """Test stabilizing and anti-stabilizing feedbacks, continuous""" + """Test stabilizing and anti-stabilizing feedback, continuous""" A = matarrayin(np.diag([1, -1])) B = matarrayin(np.identity(2)) Q = matarrayin(np.identity(2)) @@ -454,8 +535,11 @@ def test_care(self, matarrayin): with pytest.raises(ControlArgument, match="'scipy' not valid"): X, L, G = care(A, B, Q, R, S, E, stabilizing=False) - def test_dare(self, matarrayin): - """Test stabilizing and anti-stabilizing feedbacks, discrete""" + @pytest.mark.parametrize( + "stabilizing", + [True, pytest.param(False, marks=slycotonly)]) + def test_dare(self, matarrayin, stabilizing): + """Test stabilizing and anti-stabilizing feedback, discrete""" A = matarrayin(np.diag([0.5, 2])) B = matarrayin(np.identity(2)) Q = matarrayin(np.identity(2)) @@ -463,12 +547,72 @@ def test_dare(self, matarrayin): S = matarrayin(np.zeros((2, 2))) E = matarrayin(np.identity(2)) - X, L, G = dare(A, B, Q, R, S, E, stabilizing=True) - assert np.all(np.abs(L) < 1) + X, L, G = dare(A, B, Q, R, S, E, stabilizing=stabilizing) + sgn = {True: -1, False: 1}[stabilizing] + assert np.all(sgn * (np.abs(L) - 1) > 0) - if slycot_check(): - X, L, G = dare(A, B, Q, R, S, E, stabilizing=False) - assert np.all(np.abs(L) > 1) - else: - with pytest.raises(ControlArgument, match="'scipy' not valid"): - X, L, G = dare(A, B, Q, R, S, E, stabilizing=False) + def test_lqr_discrete(self): + """Test overloading of lqr operator for discrete time systems""" + csys = ct.rss(2, 1, 1) + dsys = ct.drss(2, 1, 1) + Q = np.eye(2) + R = np.eye(1) + + # Calling with a system versus explicit A, B should be the sam + K_csys, S_csys, E_csys = ct.lqr(csys, Q, R) + K_expl, S_expl, E_expl = ct.lqr(csys.A, csys.B, Q, R) + np.testing.assert_almost_equal(K_csys, K_expl) + np.testing.assert_almost_equal(S_csys, S_expl) + np.testing.assert_almost_equal(E_csys, E_expl) + + # Calling lqr() with a discrete time system should call dlqr() + K_lqr, S_lqr, E_lqr = ct.lqr(dsys, Q, R) + K_dlqr, S_dlqr, E_dlqr = ct.dlqr(dsys, Q, R) + np.testing.assert_almost_equal(K_lqr, K_dlqr) + np.testing.assert_almost_equal(S_lqr, S_dlqr) + np.testing.assert_almost_equal(E_lqr, E_dlqr) + + # Calling lqr() with no timebase should call lqr() + asys = ct.ss(csys.A, csys.B, csys.C, csys.D, dt=None) + K_asys, S_asys, E_asys = ct.lqr(asys, Q, R) + K_expl, S_expl, E_expl = ct.lqr(csys.A, csys.B, Q, R) + np.testing.assert_almost_equal(K_asys, K_expl) + np.testing.assert_almost_equal(S_asys, S_expl) + np.testing.assert_almost_equal(E_asys, E_expl) + + # Calling dlqr() with a continuous time system should raise an error + with pytest.raises(ControlArgument, match="dsys must be discrete"): + K, S, E = ct.dlqr(csys, Q, R) + + def test_lqe_discrete(self): + """Test overloading of lqe operator for discrete time systems""" + csys = ct.rss(2, 1, 1) + dsys = ct.drss(2, 1, 1) + Q = np.eye(1) + R = np.eye(1) + + # Calling with a system versus explicit A, B should be the sam + K_csys, S_csys, E_csys = ct.lqe(csys, Q, R) + K_expl, S_expl, E_expl = ct.lqe(csys.A, csys.B, csys.C, Q, R) + np.testing.assert_almost_equal(K_csys, K_expl) + np.testing.assert_almost_equal(S_csys, S_expl) + np.testing.assert_almost_equal(E_csys, E_expl) + + # Calling lqe() with a discrete time system should call dlqe() + K_lqe, S_lqe, E_lqe = ct.lqe(dsys, Q, R) + K_dlqe, S_dlqe, E_dlqe = ct.dlqe(dsys, Q, R) + np.testing.assert_almost_equal(K_lqe, K_dlqe) + np.testing.assert_almost_equal(S_lqe, S_dlqe) + np.testing.assert_almost_equal(E_lqe, E_dlqe) + + # Calling lqe() with no timebase should call lqe() + asys = ct.ss(csys.A, csys.B, csys.C, csys.D, dt=None) + K_asys, S_asys, E_asys = ct.lqe(asys, Q, R) + K_expl, S_expl, E_expl = ct.lqe(csys.A, csys.B, csys.C, Q, R) + np.testing.assert_almost_equal(K_asys, K_expl) + np.testing.assert_almost_equal(S_asys, S_expl) + np.testing.assert_almost_equal(E_asys, E_expl) + + # Calling dlqe() with a continuous time system should raise an error + with pytest.raises(ControlArgument, match="called with a continuous"): + K, S, E = ct.dlqe(csys, Q, R)