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) 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