remove redundant argument process in care/dare + fix up error strings

murrayrm · murrayrm · commit cf3c9b2749d1 · 2021-12-26T16:33:20.000-08:00
diff --git a/control/mateqn.py b/control/mateqn.py
@@ -343,7 +343,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
 
@@ -428,10 +429,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:
@@ -471,8 +472,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':
@@ -510,8 +511,9 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None):
         # 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,16 +523,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. 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 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
     ----------
@@ -576,7 +579,14 @@ 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:
@@ -587,21 +597,11 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None):
         return _dare_slycot(A, B, Q, R, S, E, stabilizing)
 
     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:
@@ -611,7 +611,7 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None):
 
 
 def _dare_slycot(A, B, Q, R, S=None, E=None, stabilizing=True):
-    # Make sure we can import required slycot routine
+    # Make sure we can import required slycot routines
     try:
         from slycot import sb02md
     except ImportError:
@@ -622,18 +622,11 @@ def _dare_slycot(A, B, Q, R, S=None, E=None, stabilizing=True):
     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]
@@ -642,21 +635,6 @@ def _dare_slycot(A, B, Q, R, S=None, E=None, stabilizing=True):
     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'
@@ -670,7 +648,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
@@ -43,7 +43,7 @@
 import numpy as np
 
 from . import statesp
-from .mateqn import care, dare
+from .mateqn import care, dare, _check_shape
 from .statesp import _ssmatrix, _convert_to_statespace
 from .lti import LTI, isdtime, isctime
 from .exception import ControlSlycot, ControlArgument, ControlDimension, \
@@ -260,7 +260,7 @@ def place_varga(A, B, p, dtime=False, alpha=None):
 
 # contributed by Sawyer B. Fuller <minster@uw.edu>
 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,26 +272,26 @@ 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
 
-    .. math:: x_e = A x_e + B u + L(y - C x_e - D u)
+    .. math:: x_e = A x_e + G 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
@@ -335,8 +335,8 @@ def lqe(*args, **keywords):
 
     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
     --------
@@ -386,31 +386,24 @@ 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
 
 
@@ -524,7 +517,9 @@ def dlqe(*args, **keywords):
         NN = np.array(args[index+2], ndmin=2, dtype=float)
         raise ControlNotImplemented("cross-covariance not yet implememented")
 
-    P, E, LT = dare(A.T, C.T, np.dot(np.dot(G, QN), G.T), RN, method=method)
+    # 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 <roberto.bucher@supsi.ch>
@@ -675,8 +670,8 @@ 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
 
 
@@ -771,19 +766,8 @@ def dlqr(*args, **keywords):
     else:
         N = np.zeros((Q.shape[0], R.shape[1]))
 
-    # Check dimensions for consistency
-    nstates = B.shape[0]
-    ninputs = B.shape[1]
-    if (A.shape[0] != nstates or A.shape[1] != nstates):
-        raise ControlDimension("inconsistent system dimensions")
-
-    elif (Q.shape[0] != nstates or Q.shape[1] != nstates or
-          R.shape[0] != ninputs or R.shape[1] != ninputs or
-          N.shape[0] != nstates or N.shape[1] != ninputs):
-        raise ControlDimension("incorrect weighting matrix dimensions")
-
-    # compute the result
-    S, E, K = dare(A, B, Q, R, N, method=method)
+    # 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
 
 
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
@@ -472,11 +472,11 @@ def test_lqe_call_format(self):
             L, P, E = lqe(sys, Q, R, N)
 
         # Inconsistent system dimensions
-        with pytest.raises(ct.ControlDimension, match="inconsistent system"):
+        with pytest.raises(ct.ControlDimension, match="Incompatible"):
             L, P, E = lqe(sys.A, sys.C, sys.B, Q, R)
 
         # incorrect covariance matrix dimensions
-        with pytest.raises(ct.ControlDimension, match="incorrect covariance"):
+        with pytest.raises(ct.ControlDimension, match="Incompatible"):
             L, P, E = lqe(sys.A, sys.B, sys.C, R, Q)
 
     def check_DLQE(self, L, P, poles, G, QN, RN):
