コード例 #1
0
def dlqr(*args, **keywords):
    """Linear quadratic regulator design

    The lqr() function computes the optimal state feedback controller
    that minimizes the quadratic cost

    .. math:: J = \int_0^\infty x' Q x + u' R u + 2 x' N u

    The function can be called with either 3, 4, or 5 arguments:
    
    * ``lqr(sys, Q, R)``
    * ``lqr(sys, Q, R, N)``
    * ``lqr(A, B, Q, R)``
    * ``lqr(A, B, Q, R, N)``
    
    Parameters
    ----------
    A, B: 2-d array
        Dynamics and input matrices
    sys: Lti (StateSpace or TransferFunction)
        Linear I/O system 
    Q, R: 2-d array 
        State and input weight matrices
    N: 2-d array, optional  
        Cross weight matrix

    Returns
    -------
    K: 2-d array 
        State feedback gains
    S: 2-d array
        Solution to Riccati equation
    E: 1-d array 
        Eigenvalues of the closed loop system
    
    Examples
    --------
    >>> K, S, E = lqr(sys, Q, R, [N])
    >>> K, S, E = lqr(A, B, Q, R, [N])

    """

    # Make sure that SLICOT is installed
    try:
        from slycot import sb02md
        from slycot import sb02mt
    except ImportError:
        raise ControlSlycot("can't find slycot module 'sb02md' or 'sb02nt'")

    #
    # Process the arguments and figure out what inputs we received
    #

    # Get the system description
    if (len(args) < 4):
        raise ControlArgument("not enough input arguments")

    elif (ctrlutil.issys(args[0])):
        # We were passed a system as the first argument; extract A and B
        #! TODO: really just need to check for A and B attributes
        A = np.array(args[0].A, ndmin=2, dtype=float)
        B = np.array(args[0].B, ndmin=2, dtype=float)
        index = 1
    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]))

    # 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 G matrix required by SB02MD
    A_b,B_b,Q_b,R_b,L_b,ipiv,oufact,G = \
        sb02mt(nstates, ninputs, B, R, A, Q, N, jobl='N')

    # Call the SLICOT function
    X, rcond, w, S, U, A_inv = sb02md(nstates, A_b, G, Q_b, 'D')

    # Now compute the return value
    K = np.dot(np.linalg.inv(R), (np.dot(B.T, X) + N.T))
    S = X
    E = w[0:nstates]

    return K, S, E
コード例 #2
0
ファイル: slycot.py プロジェクト: prklVIP/pymor
    def solve_ricc_lrcf(A, E, B, C, R=None, S=None, trans=False, options=None):
        """Compute an approximate low-rank solution of a Riccati equation.

        See :func:`pymor.algorithms.riccati.solve_ricc_lrcf` for a
        general description.

        This function uses `slycot.sb02md` (if E and S are `None`),
        `slycot.sb02od` (if E is `None` and S is not `None`) and
        `slycot.sg03ad` (if E is not `None`), which are dense solvers.
        Therefore, we assume all |Operators| and |VectorArrays| can be
        converted to |NumPy arrays| using
        :func:`~pymor.algorithms.to_matrix.to_matrix` and
        :func:`~pymor.vectorarrays.interfaces.VectorArrayInterface.to_numpy`.

        Parameters
        ----------
        A
            The |Operator| A.
        E
            The |Operator| E or `None`.
        B
            The operator B as a |VectorArray| from `A.source`.
        C
            The operator C as a |VectorArray| from `A.source`.
        R
            The operator R as a 2D |NumPy array| or `None`.
        S
            The operator S as a |VectorArray| from `A.source` or `None`.
        trans
            Whether the first |Operator| in the Riccati equation is
            transposed.
        options
            The solver options to use (see
            :func:`ricc_lrcf_solver_options`).

        Returns
        -------
        Z
            Low-rank Cholesky factor of the Riccati equation solution,
            |VectorArray| from `A.source`.
        """

        _solve_ricc_check_args(A, E, B, C, R, S, trans)
        options = _parse_options(options, ricc_lrcf_solver_options(), 'slycot',
                                 None, False)
        if options['type'] != 'slycot':
            raise ValueError(
                f"Unexpected Riccati equation solver ({options['type']}).")

        A_source = A.source
        A = to_matrix(A, format='dense')
        E = to_matrix(E, format='dense') if E else None
        B = B.to_numpy().T
        C = C.to_numpy()
        S = S.to_numpy().T if S else None

        n = A.shape[0]
        dico = 'C'

        if E is None:
            if S is None:
                if not trans:
                    A = A.T
                    G = C.T.dot(C) if R is None else slycot.sb02mt(
                        n, C.shape[0], C.T, R)[-1]
                else:
                    G = B.dot(B.T) if R is None else slycot.sb02mt(
                        n, B.shape[1], B, R)[-1]
                Q = B.dot(B.T) if not trans else C.T.dot(C)
                X, rcond = slycot.sb02md(n, A, G, Q, dico)[:2]
                _ricc_rcond_check('slycot.sb02md', rcond)
            else:
                m = C.shape[0] if not trans else B.shape[1]
                p = B.shape[1] if not trans else C.shape[0]
                if R is None:
                    R = np.eye(m)
                if not trans:
                    A = A.T
                    B, C = C.T, B.T
                X, rcond = slycot.sb02od(n,
                                         m,
                                         A,
                                         B,
                                         C,
                                         R,
                                         dico,
                                         p=p,
                                         L=S,
                                         fact='C')[:2]
                _ricc_rcond_check('slycot.sb02od', rcond)
        else:
            jobb = 'B'
            fact = 'C'
            uplo = 'U'
            jobl = 'Z' if S is None else 'N'
            scal = 'N'
            sort = 'S'
            acc = 'R'
            m = C.shape[0] if not trans else B.shape[1]
            p = B.shape[1] if not trans else C.shape[0]
            if R is None:
                R = np.eye(m)
            if S is None:
                S = np.empty((n, m))
            if not trans:
                A = A.T
                E = E.T
                B, C = C.T, B.T
            out = slycot.sg02ad(dico, jobb, fact, uplo, jobl, scal, sort, acc,
                                n, m, p, A, E, B, C, R, S)
            X = out[1]
            rcond = out[0]
            _ricc_rcond_check('slycot.sg02ad', rcond)

        return A_source.from_numpy(_chol(X).T)
コード例 #3
0
def lqr(*args, **keywords):
    """lqr(A, B, Q, R[, N])

    Linear quadratic regulator design

    The lqr() function computes the optimal state feedback controller
    that minimizes the quadratic cost

    .. math:: J = \\int_0^\\infty (x' Q x + u' R u + 2 x' N u) dt

    The function can be called with either 3, 4, or 5 arguments:

    * ``lqr(sys, Q, R)``
    * ``lqr(sys, Q, R, N)``
    * ``lqr(A, B, Q, R)``
    * ``lqr(A, B, Q, R, N)``

    where `sys` is an `LTI` object, and `A`, `B`, `Q`, `R`, and `N` are
    2d arrays or matrices of appropriate dimension.

    Parameters
    ----------
    A, B : 2D array
        Dynamics and input matrices
    sys : LTI (StateSpace or TransferFunction)
        Linear I/O 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
    --------
    lqe

    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 = lqr(sys, Q, R, [N])
    >>> K, S, E = lqr(A, B, Q, R, [N])
    """

    # Make sure that SLICOT is installed
    try:
        from slycot import sb02md
        from slycot import sb02mt
    except ImportError:
        raise ControlSlycot("can't find slycot module 'sb02md' or 'sb02nt'")

    #
    # Process the arguments and figure out what inputs we received
    #

    # Get the system description
    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
        A = np.array(args[0].A, ndmin=2, dtype=float)
        B = np.array(args[0].B, ndmin=2, dtype=float)
        index = 1
    except AttributeError:
        # 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]))

    # 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 G matrix required by SB02MD
    A_b, B_b, Q_b, R_b, L_b, ipiv, oufact, G = \
        sb02mt(nstates, ninputs, B, R, A, Q, N, jobl='N')

    # Call the SLICOT function
    X, rcond, w, S, U, A_inv = sb02md(nstates, A_b, G, Q_b, 'C')

    # Now compute the return value
    # We assume that R is positive definite and, hence, invertible
    K = np.linalg.solve(R, np.dot(B.T, X) + N.T)
    S = X
    E = w[0:nstates]

    return _ssmatrix(K), _ssmatrix(S), E
コード例 #4
0
def dare(A,B,Q,R,S=None,E=None):
    """ (X,L,G) = dare(A,B,Q,R) solves the discrete-time algebraic Riccati  
    equation

        A^T X A - X - A^T X B (B^T X B + R)^-1 B^T X A + Q = 0

    where A and Q are square matrices of the same dimension. Further, Q 
    is a symmetric matrix. The function returns the solution X, the gain
    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 algebraic
    Riccati equation

        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 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. """

    # 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 1-d arrays
    if len(shape(A)) == 1:
        A = A.reshape(1,A.size)

    if len(shape(B)) == 1:
        B = B.reshape(1,B.size)

    if len(shape(Q)) == 1:
        Q = Q.reshape(1,Q.size)

    if R != None and len(shape(R)) == 1:
        R = R.reshape(1,R.size)

    if S != None and len(shape(S)) == 1:
        S = S.reshape(1,S.size)

    if E != None and len(shape(E)) == 1:
        E = E.reshape(1,E.size)

    # Determine main dimensions
    if size(A) == 1:
        n = 1
    else:
        n = size(A,0)

    if size(B) == 1:
        m = 1
    else:
        m = size(B,1)

    # Solve the standard algebraic Riccati equation
    if S==None and E==None:
        # Check input data for consistency
        if size(A) > 1 and shape(A)[0] != shape(A)[1]:
            raise ControlArgument("A must be a quadratic matrix.")

        if (size(Q) > 1 and shape(Q)[0] != shape(Q)[1]) or \
            (size(Q) > 1 and shape(Q)[0] != n) or \
            size(Q) == 1 and n > 1:
            raise ControlArgument("Q must be a quadratic matrix of the same \
                dimension as A.")

        if (size(B) > 1 and shape(B)[0] != n) or \
            size(B) == 1 and n > 1:
            raise ControlArgument("Incompatible dimensions of B matrix.")

        if not (asarray(Q) == asarray(Q).T).all():
            raise ControlArgument("Q must be a symmetric matrix.")

        if not (asarray(R) == asarray(R).T).all():
            raise ControlArgument("R must be a symmetric matrix.")

        # Create back-up of arrays needed for later computations
        A_ba = copy(A)
        R_ba = copy(R)
        B_ba = copy(B)

        # Solve the standard algebraic Riccati equation by calling Slycot 
        # functions sb02mt and sb02md
        try:
            A_b,B_b,Q_b,R_b,L_b,ipiv,oufact,G = sb02mt(n,m,B,R)    
        except ValueError(ve):
            if ve.info < 0:
                e = ValueError(ve.message)
                e.info = ve.info
            elif ve.info == m+1:
                e = ValueError("The matrix R is numerically singular.")
                e.info = ve.info
            else:
                e = ValueError("The %i-th element of d in the UdU (LdL) \
                     factorization is zero." % ve.info)
                e.info = ve.info
            raise e

        try:
            X,rcond,w,S,U,A_inv = sb02md(n,A,G,Q,'D')
        except ValueError(ve):
            if ve.info < 0 or ve.info > 5:
                e = ValueError(ve.message)
                e.info = ve.info
            elif ve.info == 1:
                e = ValueError("The matrix A is (numerically) singular in \
                    discrete-time case.")
                e.info = ve.info
            elif ve.info == 2:
                e = ValueError("The Hamiltonian or symplectic matrix H cannot \
                    be reduced to real Schur form.")
                e.info = ve.info
            elif ve.info == 3:
                e = ValueError("The real Schur form of the Hamiltonian or \
                     symplectic matrix H cannot be appropriately ordered.")
                e.info = ve.info
            elif ve.info == 4:
                e = ValueError("The Hamiltonian or symplectic matrix H has \
                     less than n stable eigenvalues.")
                e.info = ve.info
            elif ve.info == 5:
                e = ValueError("The N-th order system of linear algebraic \
                     equations is singular to working precision.")
                e.info = ve.info
            raise e

        # Calculate the gain matrix G
        if size(R_b) == 1:
            G = dot( 1/(dot(asarray(B_ba).T,dot(X,B_ba))+R_ba) , \
                dot(asarray(B_ba).T,dot(X,A_ba)) )
        else:
            G = dot( inv(dot(asarray(B_ba).T,dot(X,B_ba))+R_ba) , \
                dot(asarray(B_ba).T,dot(X,A_ba)) )

        # Return the solution X, the closed-loop eigenvalues L and
        # the gain matrix G
        return (X , w[:n] , G)

    # Solve the generalized algebraic Riccati equation
    elif S != None and E != None:
        # Check input data for consistency
        if size(A) > 1 and shape(A)[0] != shape(A)[1]:
            raise ControlArgument("A must be a quadratic matrix.")

        if (size(Q) > 1 and shape(Q)[0] != shape(Q)[1]) or \
            (size(Q) > 1 and shape(Q)[0] != n) or \
            size(Q) == 1 and n > 1:
            raise ControlArgument("Q must be a quadratic matrix of the same \
                dimension as A.")

        if (size(B) > 1 and shape(B)[0] != n) or \
            size(B) == 1 and n > 1:
            raise ControlArgument("Incompatible dimensions of B matrix.")

        if (size(E) > 1 and shape(E)[0] != shape(E)[1]) or \
            (size(E) > 1 and shape(E)[0] != n) or \
            size(E) == 1 and n > 1:
            raise ControlArgument("E must be a quadratic matrix of the same \
                dimension as A.")

        if (size(R) > 1 and shape(R)[0] != shape(R)[1]) or \
            (size(R) > 1 and shape(R)[0] != m) or \
            size(R) == 1 and m > 1:
            raise ControlArgument("R must be a quadratic matrix of the same \
                dimension as the number of columns in the B matrix.")

        if (size(S) > 1 and shape(S)[0] != n) or \
            (size(S) > 1 and shape(S)[1] != m) or \
            size(S) == 1 and n > 1 or \
            size(S) == 1 and m > 1:
            raise ControlArgument("Incompatible dimensions of S matrix.")

        if not (asarray(Q) == asarray(Q).T).all():
            raise ControlArgument("Q must be a symmetric matrix.")

        if not (asarray(R) == asarray(R).T).all():
            raise ControlArgument("R must be a symmetric matrix.")

        # 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
        try:
            rcondu,X,alfar,alfai,beta,S_o,T,U,iwarn = \
                    sg02ad('D','B','N','U','N','N','S','R',n,m,0,A,E,B,Q,R,S)
        except ValueError(ve):
            if ve.info < 0 or ve.info > 7:
                e = ValueError(ve.message)
                e.info = ve.info
            elif ve.info == 1:
                e = ValueError("The computed extended matrix pencil is \
                            singular, possibly due to rounding errors.")
                e.info = ve.info
            elif ve.info == 2:
                e = ValueError("The QZ algorithm failed.")
                e.info = ve.info
            elif ve.info == 3:
                e = ValueError("Reordering of the generalized eigenvalues \
                     failed.")
                e.info = ve.info
            elif ve.info == 4:
                e = ValueError("After reordering, roundoff changed values of \
                            some complex eigenvalues so that leading \
                            eigenvalues in the generalized Schur form no \
                            longer satisfy the stability condition; this \
                            could also be caused due to scaling.")
                e.info = ve.info
            elif ve.info == 5:
                e = ValueError("The computed dimension of the solution does \
                            not equal N.")
                e.info = ve.info
            elif ve.info == 6:
                e = ValueError("The spectrum is too close to the boundary of \
                            the stability domain.")
                e.info = ve.info
            elif ve.info == 7:
                e = ValueError("A singular matrix was encountered during the \
                            computation of the solution matrix X.")
                e.info = ve.info
            raise e

        L = zeros((n,1))
        L.dtype = 'complex64'
        for i in range(n):
            L[i] = (alfar[i] + alfai[i]*1j)/beta[i]

        # Calculate the gain matrix G
        if size(R_b) == 1:
            G = dot( 1/(dot(asarray(B_b).T,dot(X,B_b))+R_b) , \
                dot(asarray(B_b).T,dot(X,A_b)) + asarray(S_b).T)
        else:
            G = dot( inv(dot(asarray(B_b).T,dot(X,B_b))+R_b) , \
                dot(asarray(B_b).T,dot(X,A_b)) + asarray(S_b).T)

        # Return the solution X, the closed-loop eigenvalues L and
        # the gain matrix G
        return (X , L , G)

    # Invalid set of input parameters
    else:
        raise ControlArgument("Invalid set of input parameters.")
コード例 #5
0
ファイル: slycot.py プロジェクト: pymor/pymor
    def solve_ricc_lrcf(A, E, B, C, R=None, S=None, trans=False, options=None):
        """Compute an approximate low-rank solution of a Riccati equation.

        See :func:`pymor.algorithms.riccati.solve_ricc_lrcf` for a
        general description.

        This function uses `slycot.sb02md` (if E and S are `None`),
        `slycot.sb02od` (if E is `None` and S is not `None`) and
        `slycot.sg03ad` (if E is not `None`), which are dense solvers.
        Therefore, we assume all |Operators| and |VectorArrays| can be
        converted to |NumPy arrays| using
        :func:`~pymor.algorithms.to_matrix.to_matrix` and
        :func:`~pymor.vectorarrays.interfaces.VectorArrayInterface.to_numpy`.

        Parameters
        ----------
        A
            The |Operator| A.
        E
            The |Operator| E or `None`.
        B
            The operator B as a |VectorArray| from `A.source`.
        C
            The operator C as a |VectorArray| from `A.source`.
        R
            The operator R as a 2D |NumPy array| or `None`.
        S
            The operator S as a |VectorArray| from `A.source` or `None`.
        trans
            Whether the first |Operator| in the Riccati equation is
            transposed.
        options
            The solver options to use (see
            :func:`ricc_lrcf_solver_options`).

        Returns
        -------
        Z
            Low-rank Cholesky factor of the Riccati equation solution,
            |VectorArray| from `A.source`.
        """

        _solve_ricc_check_args(A, E, B, C, R, S, trans)
        options = _parse_options(options, ricc_lrcf_solver_options(), 'slycot', None, False)
        if options['type'] != 'slycot':
            raise ValueError(f"Unexpected Riccati equation solver ({options['type']}).")

        A_source = A.source
        A = to_matrix(A, format='dense')
        E = to_matrix(E, format='dense') if E else None
        B = B.to_numpy().T
        C = C.to_numpy()
        S = S.to_numpy().T if S else None

        n = A.shape[0]
        dico = 'C'

        if E is None:
            if S is None:
                if not trans:
                    A = A.T
                    G = C.T.dot(C) if R is None else slycot.sb02mt(n, C.shape[0], C.T, R)[-1]
                else:
                    G = B.dot(B.T) if R is None else slycot.sb02mt(n, B.shape[1], B, R)[-1]
                Q = B.dot(B.T) if not trans else C.T.dot(C)
                X, rcond = slycot.sb02md(n, A, G, Q, dico)[:2]
                _ricc_rcond_check('slycot.sb02md', rcond)
            else:
                m = C.shape[0] if not trans else B.shape[1]
                p = B.shape[1] if not trans else C.shape[0]
                if R is None:
                    R = np.eye(m)
                if not trans:
                    A = A.T
                    B, C = C.T, B.T
                X, rcond = slycot.sb02od(n, m, A, B, C, R, dico, p=p, L=S, fact='C')[:2]
                _ricc_rcond_check('slycot.sb02od', rcond)
        else:
            jobb = 'B'
            fact = 'C'
            uplo = 'U'
            jobl = 'Z' if S is None else 'N'
            scal = 'N'
            sort = 'S'
            acc = 'R'
            m = C.shape[0] if not trans else B.shape[1]
            p = B.shape[1] if not trans else C.shape[0]
            if R is None:
                R = np.eye(m)
            if S is None:
                S = np.empty((n, m))
            if not trans:
                A = A.T
                E = E.T
                B, C = C.T, B.T
            out = slycot.sg02ad(dico, jobb, fact, uplo, jobl, scal, sort, acc,
                                n, m, p,
                                A, E, B, C, R, S)
            X = out[1]
            rcond = out[0]
            _ricc_rcond_check('slycot.sg02ad', rcond)

        return A_source.from_numpy(_chol(X).T)
コード例 #6
0
def care(A, B, Q, R=None, S=None, E=None, stabilizing=True):
    """(X, L, G) = care(A, B, Q, R=None) solves the continuous-time
    algebraic Riccati equation

        :math:`A^T X + X A - X B R^{-1} B^T X + Q = 0`

    where A and Q are square matrices of the same dimension. Further,
    Q and R are a symmetric matrices. If R is None, it is set to the
    identity matrix. The function returns the solution X, the gain
    matrix G = B^T X and the closed loop eigenvalues L, i.e., the
    eigenvalues of A - B G.

    (X, L, G) = care(A, B, Q, R, S, E) solves the generalized
    continuous-time algebraic Riccati equation

        :math:`A^T X E + E^T X A - (E^T X B + S) R^{-1} (B^T X E + 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 G = R^-1
    (B^T X E + S^T) and the closed loop eigenvalues L, i.e., the eigenvalues
    of A - B G , E.

    Parameters
    ----------
    A, B, Q : 2D arrays
        Input matrices for the Riccati equation
    R, S, E : 2D arrays, optional
        Input matrices for generalized Riccati equation

    Returns
    -------
    X : 2D array (or matrix)
        Solution to the Ricatti equation
    L : 1D array
        Closed loop eigenvalues
    G : 2D array (or matrix)
        Gain matrix

    Notes
    -----
    The return type for 2D arrays depends on the default class set for
    state space operations.  See :func:`~control.use_numpy_matrix`.

    """

    # 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 1-d arrays
    if len(shape(A)) == 1:
        A = A.reshape(1, A.size)

    if len(shape(B)) == 1:
        B = B.reshape(1, B.size)

    if len(shape(Q)) == 1:
        Q = Q.reshape(1, Q.size)

    if R is not None and len(shape(R)) == 1:
        R = R.reshape(1, R.size)

    if S is not None and len(shape(S)) == 1:
        S = S.reshape(1, S.size)

    if E is not None and len(shape(E)) == 1:
        E = E.reshape(1, E.size)

    # Determine main dimensions
    if size(A) == 1:
        n = 1
    else:
        n = size(A, 0)

    if size(B) == 1:
        m = 1
    else:
        m = size(B, 1)
    if R is None:
        R = eye(m, m)

    # Solve the standard algebraic Riccati equation
    if S is None and E is None:
        # Check input data for consistency
        if size(A) > 1 and shape(A)[0] != shape(A)[1]:
            raise ControlArgument("A must be a quadratic matrix.")

        if (size(Q) > 1 and shape(Q)[0] != shape(Q)[1]) or \
           (size(Q) > 1 and shape(Q)[0] != n) or \
           size(Q) == 1 and n > 1:
            raise ControlArgument("Q must be a quadratic matrix of the same \
                dimension as A.")

        if (size(B) > 1 and shape(B)[0] != n) or \
           size(B) == 1 and n > 1:
            raise ControlArgument("Incompatible dimensions of B matrix.")

        if not _is_symmetric(Q):
            raise ControlArgument("Q must be a symmetric matrix.")

        if not _is_symmetric(R):
            raise ControlArgument("R must be a symmetric matrix.")

        # Create back-up of arrays needed for later computations
        R_ba = copy(R)
        B_ba = copy(B)

        # Solve the standard algebraic Riccati equation by calling Slycot
        # functions sb02mt and sb02md
        try:
            A_b, B_b, Q_b, R_b, L_b, ipiv, oufact, G = sb02mt(n, m, B, R)
        except ValueError as ve:
            if ve.info < 0:
                e = ValueError(ve.message)
                e.info = ve.info
            elif ve.info == m + 1:
                e = ValueError("The matrix R is numerically singular.")
                e.info = ve.info
            else:
                e = ValueError("The %i-th element of d in the UdU (LdL) \
                    factorization is zero." % ve.info)
                e.info = ve.info
            raise e

        try:
            if stabilizing:
                sort = 'S'
            else:
                sort = 'U'
            X, rcond, w, S_o, U, A_inv = sb02md(n, A, G, Q, 'C', sort=sort)
        except ValueError as ve:
            if ve.info < 0 or ve.info > 5:
                e = ValueError(ve.message)
                e.info = ve.info
            elif ve.info == 1:
                e = ValueError("The matrix A is (numerically) singular in \
                    continuous-time case.")
                e.info = ve.info
            elif ve.info == 2:
                e = ValueError("The Hamiltonian or symplectic matrix H cannot \
                    be reduced to real Schur form.")
                e.info = ve.info
            elif ve.info == 3:
                e = ValueError("The real Schur form of the Hamiltonian or \
                    symplectic matrix H cannot be appropriately ordered.")
                e.info = ve.info
            elif ve.info == 4:
                e = ValueError("The Hamiltonian or symplectic matrix H has \
                    less than n stable eigenvalues.")
                e.info = ve.info
            elif ve.info == 5:
                e = ValueError("The N-th order system of linear algebraic \
                         equations is singular to working precision.")
                e.info = ve.info
            raise e

        # Calculate the gain matrix G
        if size(R_b) == 1:
            G = dot(dot(1 / (R_ba), asarray(B_ba).T), X)
        else:
            G = dot(solve(R_ba, asarray(B_ba).T), X)

        # Return the solution X, the closed-loop eigenvalues L and
        # the gain matrix G
        return (_ssmatrix(X), w[:n], _ssmatrix(G))

    # Solve the generalized algebraic Riccati equation
    elif S is not None and E is not None:
        # Check input data for consistency
        if size(A) > 1 and shape(A)[0] != shape(A)[1]:
            raise ControlArgument("A must be a quadratic matrix.")

        if (size(Q) > 1 and shape(Q)[0] != shape(Q)[1]) or \
           (size(Q) > 1 and shape(Q)[0] != n) or \
           size(Q) == 1 and n > 1:
            raise ControlArgument("Q must be a quadratic matrix of the same \
                dimension as A.")

        if (size(B) > 1 and shape(B)[0] != n) or \
           size(B) == 1 and n > 1:
            raise ControlArgument("Incompatible dimensions of B matrix.")

        if (size(E) > 1 and shape(E)[0] != shape(E)[1]) or \
           (size(E) > 1 and shape(E)[0] != n) or \
           size(E) == 1 and n > 1:
            raise ControlArgument("E must be a quadratic matrix of the same \
                dimension as A.")

        if (size(R) > 1 and shape(R)[0] != shape(R)[1]) or \
           (size(R) > 1 and shape(R)[0] != m) or \
           size(R) == 1 and m > 1:
            raise ControlArgument("R must be a quadratic matrix of the same \
                dimension as the number of columns in the B matrix.")

        if (size(S) > 1 and shape(S)[0] != n) or \
           (size(S) > 1 and shape(S)[1] != m) or \
           size(S) == 1 and n > 1 or \
           size(S) == 1 and m > 1:
            raise ControlArgument("Incompatible dimensions of S matrix.")

        if not _is_symmetric(Q):
            raise ControlArgument("Q must be a symmetric matrix.")

        if not _is_symmetric(R):
            raise ControlArgument("R must be a symmetric matrix.")

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

        # Solve the generalized algebraic Riccati equation by calling the
        # Slycot function sg02ad
        try:
            if stabilizing:
                sort = 'S'
            else:
                sort = 'U'
            rcondu, X, alfar, alfai, beta, S_o, T, U, iwarn = \
                sg02ad('C', 'B', 'N', 'U', 'N', 'N', sort,
                       'R', n, m, 0, A, E, B, Q, R, S)
        except ValueError as ve:
            if ve.info < 0 or ve.info > 7:
                e = ValueError(ve.message)
                e.info = ve.info
            elif ve.info == 1:
                e = ValueError("The computed extended matrix pencil is \
                            singular, possibly due to rounding errors.")
                e.info = ve.info
            elif ve.info == 2:
                e = ValueError("The QZ algorithm failed.")
                e.info = ve.info
            elif ve.info == 3:
                e = ValueError("Reordering of the generalized eigenvalues \
                    failed.")
                e.info = ve.info
            elif ve.info == 4:
                e = ValueError("After reordering, roundoff changed values of \
                            some complex eigenvalues so that leading \
                            eigenvalues in the generalized Schur form no \
                            longer satisfy the stability condition; this \
                            could also be caused due to scaling.")
                e.info = ve.info
            elif ve.info == 5:
                e = ValueError("The computed dimension of the solution does \
                            not equal N.")
                e.info = ve.info
            elif ve.info == 6:
                e = ValueError("The spectrum is too close to the boundary of \
                            the stability domain.")
                e.info = ve.info
            elif ve.info == 7:
                e = ValueError("A singular matrix was encountered during the \
                            computation of the solution matrix X.")
                e.info = ve.info
            raise e

        # Calculate the closed-loop eigenvalues L
        L = zeros((n, 1))
        L.dtype = 'complex64'
        for i in range(n):
            L[i] = (alfar[i] + alfai[i] * 1j) / beta[i]

        # Calculate the gain matrix G
        if size(R_b) == 1:
            G = dot(1 / (R_b),
                    dot(asarray(B_b).T, dot(X, E_b)) + asarray(S_b).T)
        else:
            G = solve(R_b, dot(asarray(B_b).T, dot(X, E_b)) + asarray(S_b).T)

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

    # Invalid set of input parameters
    else:
        raise ControlArgument("Invalid set of input parameters.")
コード例 #7
0
def dlqr(*args, **keywords):
    """Linear quadratic regulator design

    The lqr() function computes the optimal state feedback controller
    that minimizes the quadratic cost

    .. math:: J = \int_0^\infty x' Q x + u' R u + 2 x' N u

    The function can be called with either 3, 4, or 5 arguments:
    
    * ``lqr(sys, Q, R)``
    * ``lqr(sys, Q, R, N)``
    * ``lqr(A, B, Q, R)``
    * ``lqr(A, B, Q, R, N)``
    
    Parameters
    ----------
    A, B: 2-d array
        Dynamics and input matrices
    sys: Lti (StateSpace or TransferFunction)
        Linear I/O system 
    Q, R: 2-d array 
        State and input weight matrices
    N: 2-d array, optional  
        Cross weight matrix

    Returns
    -------
    K: 2-d array 
        State feedback gains
    S: 2-d array
        Solution to Riccati equation
    E: 1-d array 
        Eigenvalues of the closed loop system
    
    Examples
    --------
    >>> K, S, E = lqr(sys, Q, R, [N])
    >>> K, S, E = lqr(A, B, Q, R, [N])

    """

    # Make sure that SLICOT is installed
    try:
        from slycot import sb02md
        from slycot import sb02mt
    except ImportError:
        raise ControlSlycot("can't find slycot module 'sb02md' or 'sb02nt'")

    # 
    # Process the arguments and figure out what inputs we received
    #
    
    # Get the system description
    if (len(args) < 4):
        raise ControlArgument("not enough input arguments")

    elif (ctrlutil.issys(args[0])):
        # We were passed a system as the first argument; extract A and B
        #! TODO: really just need to check for A and B attributes
        A = np.array(args[0].A, ndmin=2, dtype=float);
        B = np.array(args[0].B, ndmin=2, dtype=float);
        index = 1;
    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]));

    # 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 G matrix required by SB02MD
    A_b,B_b,Q_b,R_b,L_b,ipiv,oufact,G = \
        sb02mt(nstates, ninputs, B, R, A, Q, N, jobl='N');

    # Call the SLICOT function
    X,rcond,w,S,U,A_inv = sb02md(nstates, A_b, G, Q_b, 'D')

    # Now compute the return value
    K = np.dot(np.linalg.inv(R), (np.dot(B.T, X) + N.T));
    S = X;
    E = w[0:nstates];

    return K, S, E
コード例 #8
0
ファイル: slycot.py プロジェクト: tobiasleibner/pymor
    def solve_ricc(A, E=None, B=None, Q=None, C=None, R=None, G=None, trans=False, options=None):
        """Find a factor of the solution of a Riccati equation

        Returns factor :math:`Z` such that :math:`Z Z^T` is
        approximately the solution :math:`X` of a Riccati equation

        .. math::
            A^T X E + E^T X A - E^T X B R^{-1} B^T X E + Q = 0.

        If E in `None`, it is taken to be the identity matrix.
        Q can instead be given as C^T * C. In this case, Q needs to be
        `None`, and C not `None`.
        B * R^{-1} B^T can instead be given by G. In this case, B and R
        need to be `None`, and G not `None`.
        If R and G are `None`, then R is taken to be the identity
        matrix.
        If trans is `True`, then the dual Riccati equation is solved

        .. math::
            A X E^T + E X A^T - E X C^T R^{-1} C X E^T + Q = 0,

        where Q can be replaced by B * B^T and C^T * R^{-1} * C by G.

        This uses the `slycot` package, in particular its interfaces to
        SLICOT functions `SB02MD` (for the standard Riccati equations)
        and `SG02AD` (for the generalized Riccati equations).
        These methods are only applicable to medium-sized dense
        problems and need access to the matrix data of all operators.

        Parameters
        ----------
        A
            The |Operator| A.
        B
            The |Operator| B or `None`.
        E
            The |Operator| E or `None`.
        Q
            The |Operator| Q or `None`.
        C
            The |Operator| C or `None`.
        R
            The |Operator| R or `None`.
        G
            The |Operator| G or `None`.
        trans
            If the dual equation needs to be solved.
        options
            The |solver_options| to use (see
            :func:`ricc_solver_options`).

        Returns
        -------
        Z
            Low-rank factor of the Riccati equation solution,
            |VectorArray| from `A.source`.
        """
        _solve_ricc_check_args(A, E, B, Q, C, R, G, trans)
        options = _parse_options(options, ricc_solver_options(), 'slycot', None, False)
        assert options['type'] == 'slycot'

        import slycot
        A_mat = to_matrix(A, format='dense')
        B_mat = to_matrix(B, format='dense') if B else None
        C_mat = to_matrix(C, format='dense') if C else None
        R_mat = to_matrix(R, format='dense') if R else None
        G_mat = to_matrix(G, format='dense') if G else None
        Q_mat = to_matrix(Q, format='dense') if Q else None

        n = A_mat.shape[0]
        dico = 'C'

        if E is None:
            if not trans:
                if G is None:
                    if R is None:
                        G_mat = B_mat.dot(B_mat.T)
                    else:
                        G_mat = slycot.sb02mt(n, B_mat.shape[1], B_mat, R_mat)[-1]
                if C is not None:
                    Q_mat = C_mat.T.dot(C_mat)
                X = slycot.sb02md(n, A_mat, G_mat, Q_mat, dico)[0]
            else:
                if G is None:
                    if R is None:
                        G_mat = C_mat.T.dot(C_mat)
                    else:
                        G_mat = slycot.sb02mt(n, C_mat.shape[0], C_mat.T, R_mat)[-1]
                if B is not None:
                    Q_mat = B_mat.dot(B_mat.T)
                X = slycot.sb02md(n, A_mat.T, G_mat, Q_mat, dico)[0]
        else:
            E_mat = to_matrix(E, format='dense') if E else None
            jobb = 'B' if G is None else 'B'
            fact = 'C' if Q is None else 'N'
            uplo = 'U'
            jobl = 'Z'
            scal = 'N'
            sort = 'S'
            acc = 'R'
            if not trans:
                m = 0 if B is None else B_mat.shape[1]
                p = 0 if C is None else C_mat.shape[0]
                if G is not None:
                    B_mat = G_mat
                    R_mat = np.empty((1, 1))
                elif R is None:
                    R_mat = np.eye(m)
                if Q is None:
                    Q_mat = C_mat
                L_mat = np.empty((n, m))
                ret = slycot.sg02ad(dico, jobb, fact, uplo, jobl, scal, sort, acc, n, m, p,
                                    A_mat, E_mat, B_mat, Q_mat, R_mat, L_mat)
            else:
                m = 0 if C is None else C_mat.shape[0]
                p = 0 if B is None else B_mat.shape[1]
                if G is not None:
                    C_mat = G_mat
                    R_mat = np.empty((1, 1))
                elif R is None:
                    C_mat = C_mat.T
                    R_mat = np.eye(m)
                else:
                    C_mat = C_mat.T
                if Q is None:
                    Q_mat = B_mat.T
                L_mat = np.empty((n, m))
                ret = slycot.sg02ad(dico, jobb, fact, uplo, jobl, scal, sort, acc, n, m, p,
                                    A_mat.T, E_mat.T, C_mat, Q_mat, R_mat, L_mat)
            X = ret[1]
            iwarn = ret[-1]
            if iwarn == 1:
                print('slycot.sg02ad warning: solution may be inaccurate.')

        from pymor.bindings.scipy import chol
        Z = chol(X, copy=False)
        Z = A.source.from_numpy(np.array(Z).T)

        return Z