Menu
▾
▴
[python-control-discuss] SF.net SVN: python-control:[157] trunk
|
From: <mur...@us...> - 2011-06-17 23:51:53
|
Revision: 157
http://python-control.svn.sourceforge.net/python-control/?rev=157&view=rev
Author: murrayrm
Date: 2011-06-17 23:51:46 +0000 (Fri, 17 Jun 2011)
Log Message:
-----------
Added in matrix equation solver routes (lyap, dlyap, care, dare) from Johnsson, Nordh, Olofsson, Romero). Converted test code to standard format.
Modified Paths:
--------------
trunk/ChangeLog
trunk/src/__init__.py
trunk/src/matlab.py
Added Paths:
-----------
trunk/src/mateqn.py
trunk/tests/mateqn_test.py
Modified: trunk/ChangeLog
===================================================================
--- trunk/ChangeLog 2011-04-17 18:45:29 UTC (rev 156)
+++ trunk/ChangeLog 2011-06-17 23:51:46 UTC (rev 157)
@@ -1,3 +1,20 @@
+2011-06-16 Richard Murray <murray@malabar.local>
+
+ * src/matlab.py: import mateqn functions
+
+ * src/__init__.py: import mateqn functions
+
+ * tests/test_all.py: added unit tests for matrix solvers, converting
+ to standard format along the way. Seems to work even if slycot
+ routines are not in place, but I'm not sure if this is for the right
+ reasons...
+
+ * src/mateqn.py: added matrix solvers from LTH (Ola Johnsson, Jerker
+ Nordh, Bjorn Olofsson, Vanessa Romero). Moved slycot function
+ checks to the portion of the code where they are used, so that
+ missing slycot doesn't mess up initialization if proper version of
+ slycot is not available.
+
2011-04-02 Richard Murray <murray@malabar.local>
* src/xferfcn.py (TransferFunction.__add__): fixed bug when adding a
Modified: trunk/src/__init__.py
===================================================================
--- trunk/src/__init__.py 2011-04-17 18:45:29 UTC (rev 156)
+++ trunk/src/__init__.py 2011-06-17 23:51:46 UTC (rev 157)
@@ -65,3 +65,4 @@
from delay import *
from modelsimp import *
from rlocus import *
+from mateqn import *
Added: trunk/src/mateqn.py
===================================================================
--- trunk/src/mateqn.py (rev 0)
+++ trunk/src/mateqn.py 2011-06-17 23:51:46 UTC (rev 157)
@@ -0,0 +1,928 @@
+# mateqn.py - matrix equation solvers (Lyapunov, Riccati)
+from __future__ import division
+
+""" Implementation of the functions lyap, dlyap, care and dare
+for solution of Lyapunov and Riccati equations. """
+
+"""Copyright (c) 2011, All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+1. Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+
+3. Neither the name of the project author nor the names of its
+ contributors may be used to endorse or promote products derived
+ from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CALTECH
+OR THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
+OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGE.
+
+Author: Bjorn Olofsson
+"""
+
+from numpy.linalg import inv
+from scipy import shape,size,asarray,copy,zeros,eye,dot
+
+from control.exception import ControlSlycot,ControlArgument
+
+#### Lyapunov equation solvers lyap and dlyap
+
+def lyap(A,Q,C=None,E=None):
+ """ X = lyap(A,Q) solves the continuous-time Lyapunov equation
+
+ A X + X A^T + Q = 0
+
+ where A and Q are square matrices of the same dimension.
+ Further, Q must be symmetric.
+
+ X = lyap(A,Q,C) solves the Sylvester equation
+
+ A X + X Q + C = 0
+
+ where A and Q are square matrices.
+
+ X = lyap(A,Q,None,E) solves the generalized continuous-time
+ Lyapunov equation
+
+ A X E^T + E X A^T + Q = 0
+
+ where Q is a symmetric matrix and A, Q and E are square matrices
+ of the same dimension. """
+
+ # Make sure we have access to the right slycot routines
+ try:
+ from slycot import sb03md
+ except ImportError:
+ raise ControlSlycot("can't find slycot module 'sb03md'")
+
+ try:
+ from slycot import sb04md
+ except ImportError:
+ raise ControlSlycot("can't find slycot module 'sb04md'")
+
+ # Reshape 1-d arrays
+ if len(shape(A)) == 1:
+ A = A.reshape(1,A.size)
+
+ if len(shape(Q)) == 1:
+ Q = Q.reshape(1,Q.size)
+
+ if C != None and len(shape(C)) == 1:
+ C = C.reshape(1,C.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(Q) == 1:
+ m = 1
+ else:
+ m = size(Q,0)
+
+ # Solve standard Lyapunov equation
+ if C==None and E==None:
+ # Check input data for consistency
+ if shape(A) != shape(Q):
+ raise ControlArgument("A and Q must be matrices of identical \
+ sizes.")
+
+ 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]:
+ raise ControlArgument("Q must be a quadratic matrix.")
+
+ if not (asarray(Q) == asarray(Q).T).all():
+ raise ControlArgument("Q must be a symmetric matrix.")
+
+ # Solve the Lyapunov equation by calling Slycot function sb03md
+ try:
+ X,scale,sep,ferr,w = sb03md(n,-Q,A,eye(n,n),'C',trana='T')
+ except ValueError, ve:
+ if ve.info < 0:
+ e = ValueError(ve.message)
+ e.info = ve.info
+ elif ve.info == n+1:
+ e = ValueError("The matrix A and -A have common or very \
+ close eigenvalues.")
+ e.info = ve.info
+ else:
+ e = ValueError("The QR algorithm failed to compute all \
+ the eigenvalues (see LAPACK Library routine DGEES).")
+ e.info = ve.info
+ raise e
+
+ # Solve the Sylvester equation
+ elif C != 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]:
+ raise ControlArgument("Q must be a quadratic matrix.")
+
+ if (size(C) > 1 and shape(C)[0] != n) or \
+ (size(C) > 1 and shape(C)[1] != m) or \
+ (size(C) == 1 and size(A) != 1) or (size(C) == 1 and size(Q) != 1):
+ raise ControlArgument("C matrix has incompatible dimensions.")
+
+ # Solve the Sylvester equation by calling the Slycot function sb04md
+ try:
+ X = sb04md(n,m,A,Q,-C)
+ except ValueError, ve:
+ if ve.info < 0:
+ e = ValueError(ve.message)
+ e.info = ve.info
+ elif ve.info > m:
+ e = ValueError("A singular matrix was encountered whilst \
+ solving for the %i-th column of matrix X." % ve.info-m)
+ e.info = ve.info
+ else:
+ e = ValueError("The QR algorithm failed to compute all the \
+ eigenvalues (see LAPACK Library routine DGEES).")
+ e.info = ve.info
+ raise e
+
+ # Solve the generalized Lyapunov equation
+ elif C == None and E != None:
+ # Check input data for consistency
+ 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 square matrix with the same \
+ dimension as A.")
+
+ 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 square matrix with the same \
+ dimension as A.")
+
+ if not (asarray(Q) == asarray(Q).T).all():
+ raise ControlArgument("Q must be a symmetric matrix.")
+
+ # Make sure we have access to the write slicot routine
+ try:
+ from slycot import sg03ad
+ except ImportError:
+ raise ControlSlycot("can't find slycot module 'sg03ad'")
+
+ # Solve the generalized Lyapunov equation by calling Slycot
+ # function sg03ad
+ try:
+ A,E,Q,Z,X,scale,sep,ferr,alphar,alphai,beta = \
+ sg03ad('C','B','N','T','L',n,A,E,eye(n,n),eye(n,n),-Q)
+ except ValueError, ve:
+ if ve.info < 0 or ve.info > 4:
+ e = ValueError(ve.message)
+ e.info = ve.info
+ elif ve.info == 1:
+ e = ValueError("The matrix contained in the upper \
+ Hessenberg part of the array A is not in \
+ upper quasitriangular form")
+ e.info = ve.info
+ elif ve.info == 2:
+ e = ValueError("The pencil A - lambda * E cannot be \
+ reduced to generalized Schur form: LAPACK \
+ routine DGEGS has failed to converge")
+ e.info = ve.info
+ elif ve.info == 4:
+ e = ValueError("The pencil A - lambda * E has a \
+ degenerate pair of eigenvalues. That is, \
+ lambda_i = lambda_j for some i and j, where \
+ lambda_i and lambda_j are eigenvalues of \
+ A - lambda * E. Hence, the equation is \
+ singular; perturbed values were \
+ used to solve the equation (but the matrices \
+ A and E are unchanged)")
+ e.info = ve.info
+ raise e
+ # Invalid set of input parameters
+ else:
+ raise ControlArgument("Invalid set of input parameters")
+
+ return X
+
+
+def dlyap(A,Q,C=None,E=None):
+ """ dlyap(A,Q) solves the discrete-time Lyapunov equation
+
+ A X A^T - X + Q = 0
+
+ where A and Q are square matrices of the same dimension. Further
+ Q must be symmetric.
+
+ dlyap(A,Q,C) solves the Sylvester equation
+
+ A X Q^T - X + C = 0
+
+ where A and Q are square matrices.
+
+ dlyap(A,Q,None,E) solves the generalized discrete-time Lyapunov
+ equation
+
+ A X A^T - E X E^T + Q = 0
+
+ where Q is a symmetric matrix and A, Q and E are square matrices
+ of the same dimension. """
+
+ # Make sure we have access to the right slycot routines
+ try:
+ from slycot import sb03md
+ except ImportError:
+ raise ControlSlycot("can't find slycot module 'sb03md'")
+
+ try:
+ from slycot import sb04qd
+ except ImportError:
+ raise ControlSlycot("can't find slycot module 'sb04qd'")
+
+ try:
+ from slycot import sg03ad
+ except ImportError:
+ raise ControlSlycot("can't find slycot module 'sg03ad'")
+
+ # Reshape 1-d arrays
+ if len(shape(A)) == 1:
+ A = A.reshape(1,A.size)
+
+ if len(shape(Q)) == 1:
+ Q = Q.reshape(1,Q.size)
+
+ if C != None and len(shape(C)) == 1:
+ C = C.reshape(1,C.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(Q) == 1:
+ m = 1
+ else:
+ m = size(Q,0)
+
+ # Solve standard Lyapunov equation
+ if C==None and E==None:
+ # Check input data for consistency
+ if shape(A) != shape(Q):
+ raise ControlArgument("A and Q must be matrices of identical \
+ sizes.")
+
+ 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]:
+ raise ControlArgument("Q must be a quadratic matrix.")
+
+ if not (asarray(Q) == asarray(Q).T).all():
+ raise ControlArgument("Q must be a symmetric matrix.")
+
+ # Solve the Lyapunov equation by calling the Slycot function sb03md
+ try:
+ X,scale,sep,ferr,w = sb03md(n,-Q,A,eye(n,n),'D',trana='T')
+ except ValueError, ve:
+ if ve.info < 0:
+ e = ValueError(ve.message)
+ e.info = ve.info
+ else:
+ e = ValueError("The QR algorithm failed to compute all the \
+ eigenvalues (see LAPACK Library routine DGEES).")
+ e.info = ve.info
+ raise e
+
+ # Solve the Sylvester equation
+ elif C != 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]:
+ raise ControlArgument("Q must be a quadratic matrix")
+
+ if (size(C) > 1 and shape(C)[0] != n) or \
+ (size(C) > 1 and shape(C)[1] != m) or \
+ (size(C) == 1 and size(A) != 1) or (size(C) == 1 and size(Q) != 1):
+ raise ControlArgument("C matrix has incompatible dimensions")
+
+ # Solve the Sylvester equation by calling Slycot function sb04qd
+ try:
+ X = sb04qd(n,m,-A,asarray(Q).T,C)
+ except ValueError, ve:
+ if ve.info < 0:
+ e = ValueError(ve.message)
+ e.info = ve.info
+ elif ve.info > m:
+ e = ValueError("A singular matrix was encountered whilst \
+ solving for the %i-th column of matrix X." % ve.info-m)
+ e.info = ve.info
+ else:
+ e = ValueError("The QR algorithm failed to compute all the \
+ eigenvalues (see LAPACK Library routine DGEES)")
+ e.info = ve.info
+ raise e
+
+ # Solve the generalized Lyapunov equation
+ elif C == None and E != None:
+ # Check input data for consistency
+ 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 square matrix with the same \
+ dimension as A.")
+
+ 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 square matrix with the same \
+ dimension as A.")
+
+ if not (asarray(Q) == asarray(Q).T).all():
+ raise ControlArgument("Q must be a symmetric matrix.")
+
+ # Solve the generalized Lyapunov equation by calling Slycot
+ # function sg03ad
+ try:
+ A,E,Q,Z,X,scale,sep,ferr,alphar,alphai,beta = \
+ sg03ad('D','B','N','T','L',n,A,E,eye(n,n),eye(n,n),-Q)
+ except ValueError, ve:
+ if ve.info < 0 or ve.info > 4:
+ e = ValueError(ve.message)
+ e.info = ve.info
+ elif ve.info == 1:
+ e = ValueError("The matrix contained in the upper \
+ Hessenberg part of the array A is not in \
+ upper quasitriangular form")
+ e.info = ve.info
+ elif ve.info == 2:
+ e = ValueError("The pencil A - lambda * E cannot be \
+ reduced to generalized Schur form: LAPACK \
+ routine DGEGS has failed to converge")
+ e.info = ve.info
+ elif ve.info == 3:
+ e = ValueError("The pencil A - lambda * E has a \
+ pair of reciprocal eigenvalues. That is, \
+ lambda_i = 1/lambda_j for some i and j, \
+ where lambda_i and lambda_j are eigenvalues \
+ of A - lambda * E. Hence, the equation is \
+ singular; perturbed values were \
+ used to solve the equation (but the \
+ matrices A and E are unchanged)")
+ e.info = ve.info
+ raise e
+ # Invalid set of input parameters
+ else:
+ raise ControlArgument("Invalid set of input parameters")
+
+ return X
+
+
+
+#### Riccati equation solvers care and dare
+
+def care(A,B,Q,R=None,S=None,E=None):
+ """ (X,L,G) = care(A,B,Q) solves the continuous-time algebraic Riccati
+ equation
+
+ A^T X + X A - X B B^T X + 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 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
+
+ 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. 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. """
+
+ # 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)
+ if R==None:
+ R = eye(m,m)
+
+ # 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
+ 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_o,U,A_inv = sb02md(n,A,G,Q,'C')
+ 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(dot(1/(R_ba),asarray(B_ba).T) , X)
+ else:
+ G = dot(dot(inv(R_ba),asarray(B_ba).T) , X)
+
+ # 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
+ 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('C','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
+
+ # 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 = dot(inv(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 (X , L , G)
+
+ # Invalid set of input parameters
+ else:
+ raise ControlArgument("Invalid set of input parameters.")
+
+
+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.")
Modified: trunk/src/matlab.py
===================================================================
--- trunk/src/matlab.py 2011-04-17 18:45:29 UTC (rev 156)
+++ trunk/src/matlab.py 2011-06-17 23:51:46 UTC (rev 157)
@@ -81,6 +81,7 @@
from statefbk import ctrb, obsv, gram, place, lqr
from delay import pade
from modelsimp import hsvd, balred, modred
+from mateqn import lyap, dlyap, dare, care
__doc__ += """
The control.matlab module defines functions that are roughly the
@@ -255,9 +256,9 @@
lti/conj - complex conjugation of model coefficients
Matrix equation solvers and linear algebra
- lyap, dlyap - solve Lyapunov equations
+\* lyap, dlyap - solve Lyapunov equations
lyapchol, dlyapchol - square-root Lyapunov solvers
- care, dare - solve algebraic Riccati equations
+\* care, dare - solve algebraic Riccati equations
gcare, gdare - generalized Riccati solvers
bdschur - block diagonalization of a square matrix
Added: trunk/tests/mateqn_test.py
===================================================================
--- trunk/tests/mateqn_test.py (rev 0)
+++ trunk/tests/mateqn_test.py 2011-06-17 23:51:46 UTC (rev 157)
@@ -0,0 +1,236 @@
+#!/usr/bin/env python
+#
+# mateqn_test.py - test wuit for matrix equation solvers
+#
+#! Currently uses numpy.testing framework; will dump you out of unittest
+#! if an error occurs. Should figure out the right way to fix this.
+
+""" Test cases for lyap, dlyap, care and dare functions in the file
+pyctrl_lin_alg.py. """
+
+"""Copyright (c) 2011, All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+1. Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+
+3. Neither the name of the project author nor the names of its
+ contributors may be used to endorse or promote products derived
+ from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CALTECH
+OR THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
+OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGE.
+
+Author: Bjorn Olofsson
+"""
+
+import unittest
+from numpy import array
+from numpy.testing import assert_array_almost_equal
+from numpy.linalg import inv
+from scipy import zeros,dot
+from control.mateqn import lyap,dlyap,care,dare
+
+# Test cases (lyap and dlyap)
+
+class TestMatrixEquations(unittest.TestCase):
+ """These are tests for the matrix equation solvers in mateqn.py"""
+
+ def setUp(self):
+ # Make sure that the slycot functions that we require exist
+ from slycot import sb02md, sb02mt, sb03md, sb04md, sg02ad, \
+ sg03ad, sb04qd
+
+ def test_lyap(self):
+ A = array([[-1, 1],[-1, 0]])
+ Q = array([[1,0],[0,1]])
+ X = lyap(A,Q)
+ # print "The solution obtained is ", X
+ assert_array_almost_equal(dot(A,X)+dot(X,A.T)+Q,zeros((2,2)))
+
+ A = array([[1, 2],[-3, -4]])
+ Q = array([[3, 1],[1, 1]])
+ X = lyap(A,Q)
+ # print "The solution obtained is ", X
+ assert_array_almost_equal(dot(A,X)+dot(X,A.T)+Q,zeros((2,2)))
+
+ def test_lyap_sylvester(self):
+ A = 5
+ B = array([[4, 3], [4, 3]])
+ C = array([2, 1])
+ X = lyap(A,B,C)
+ # print "The solution obtained is ", X
+ assert_array_almost_equal(dot(A,X)+dot(X,B)+C,zeros((1,2)))
+
+ A = array([[2,1],[1,2]])
+ B = array([[1,2],[0.5,0.1]])
+ C = array([[1,0],[0,1]])
+ X = lyap(A,B,C)
+ # print "The solution obtained is ", X
+ assert_array_almost_equal(dot(A,X)+dot(X,B)+C,zeros((2,2)))
+
+ def test_lyap_g(self):
+ A = array([[-1, 2],[-3, -4]])
+ Q = array([[3, 1],[1, 1]])
+ E = array([[1,2],[2,1]])
+ X = lyap(A,Q,None,E)
+ # print "The solution obtained is ", X
+ assert_array_almost_equal(dot(A,dot(X,E.T)) + dot(E,dot(X,A.T)) + Q, \
+ zeros((2,2)))
+
+ def test_dlyap(self):
+ A = array([[-0.6, 0],[-0.1, -0.4]])
+ Q = array([[1,0],[0,1]])
+ X = dlyap(A,Q)
+ # print "The solution obtained is ", X
+ assert_array_almost_equal(dot(A,dot(X,A.T))-X+Q,zeros((2,2)))
+
+ A = array([[-0.6, 0],[-0.1, -0.4]])
+ Q = array([[3, 1],[1, 1]])
+ X = dlyap(A,Q)
+ # print "The solution obtained is ", X
+ assert_array_almost_equal(dot(A,dot(X,A.T))-X+Q,zeros((2,2)))
+
+ def test_dlyap_g(self):
+ A = array([[-0.6, 0],[-0.1, -0.4]])
+ Q = array([[3, 1],[1, 1]])
+ E = array([[1, 1],[2, 1]])
+ X = dlyap(A,Q,None,E)
+ # print "The solution obtained is ", X
+ assert_array_almost_equal(dot(A,dot(X,A.T))-dot(E,dot(X,E.T))+Q, \
+ zeros((2,2)))
+
+ def test_dlyap_sylvester(self):
+ A = 5
+ B = array([[4, 3], [4, 3]])
+ C = array([2, 1])
+ X = dlyap(A,B,C)
+ # print "The solution obtained is ", X
+ assert_array_almost_equal(dot(A,dot(X,B.T))-X+C,zeros((1,2)))
+
+ A = array([[2,1],[1,2]])
+ B = array([[1,2],[0.5,0.1]])
+ C = array([[1,0],[0,1]])
+ X = dlyap(A,B,C)
+ # print "The solution obtained is ", X
+ assert_array_almost_equal(dot(A,dot(X,B.T))-X+C,zeros((2,2)))
+
+ def test_care(self):
+ A = array([[-2, -1],[-1, -1]])
+ Q = array([[0, 0],[0, 1]])
+ B = array([[1, 0],[0, 4]])
+
+ X,L,G = care(A,B,Q)
+ # print "The solution obtained is", X
+ assert_array_almost_equal(dot(A.T,X) + dot(X,A) - \
+ dot(X,dot(B,dot(B.T,X))) + Q , zeros((2,2)))
+ assert_array_almost_equal(dot(B.T,X) , G)
+
+ def test_care_g(self):
+ A = array([[-2, -1],[-1, -1]])
+ Q = array([[0, 0],[0, 1]])
+ B = array([[1, 0],[0, 4]])
+ R = array([[2, 0],[0, 1]])
+ S = array([[0, 0],[0, 0]])
+ E = array([[2, 1],[1, 2]])
+
+ X,L,G = care(A,B,Q,R,S,E)
+ # print "The solution obtained is", X
+ assert_array_almost_equal(dot(A.T,dot(X,E)) + dot(E.T,dot(X,A)) - \
+ dot(dot(dot(E.T,dot(X,B))+S,inv(R) ) ,
+ dot(B.T,dot(X,E))+S.T ) + Q , \
+ zeros((2,2)))
+ assert_array_almost_equal(dot( inv(R) , dot(B.T,dot(X,E)) + S.T) , G)
+
+ A = array([[-2, -1],[-1, -1]])
+ Q = array([[0, 0],[0, 1]])
+ B = array([[1],[0]])
+ R = 1
+ S = array([[1],[0]])
+ E = array([[2, 1],[1, 2]])
+
+ X,L,G = care(A,B,Q,R,S,E)
+ # print "The solution obtained is", X
+ assert_array_almost_equal(dot(A.T,dot(X,E)) + dot(E.T,dot(X,A)) - \
+ dot( dot( dot(E.T,dot(X,B))+S,1/R ) , dot(B.T,dot(X,E))+S.T ) \
+ + Q , zeros((2,2)))
+ assert_array_almost_equal(dot( 1/R , dot(B.T,dot(X,E)) + S.T) , G)
+
+ def test_dare(self):
+ A = array([[-0.6, 0],[-0.1, -0.4]])
+ Q = array([[2, 1],[1, 0]])
+ B = array([[2, 1],[0, 1]])
+ R = array([[1, 0],[0, 1]])
+
+ X,L,G = dare(A,B,Q,R)
+ # print "The solution obtained is", X
+ assert_array_almost_equal(dot(A.T,dot(X,A))-X-dot(dot(dot(A.T,dot(X,B)) , \
+ inv(dot(B.T,dot(X,B))+R)) , dot(B.T,dot(X,A))) + Q , zeros((2,2)) )
+ assert_array_almost_equal( dot( inv( dot(B.T,dot(X,B)) + R) , \
+ dot(B.T,dot(X,A)) ) , G)
+
+ A = array([[1, 0],[-1, 1]])
+ Q = array([[0, 1],[1, 1]])
+ B = array([[1],[0]])
+ R = 2
+
+ X,L,G = dare(A,B,Q,R)
+ # print "The solution obtained is", X
+ assert_array_almost_equal(dot(A.T,dot(X,A))-X-dot(dot(dot(A.T,dot(X,B)) , \
+ inv(dot(B.T,dot(X,B))+R)) , dot(B.T,dot(X,A))) + Q , zeros((2,2)) )
+ assert_array_almost_equal( dot( 1 / ( dot(B.T,dot(X,B)) + R) , \
+ dot(B.T,dot(X,A)) ) , G)
+
+ def test_dare_g(self):
+ A = array([[-0.6, 0],[-0.1, -0.4]])
+ Q = array([[2, 1],[1, 3]])
+ B = array([[1, 5],[2, 4]])
+ R = array([[1, 0],[0, 1]])
+ S = array([[1, 0],[2, 0]])
+ E = array([[2, 1],[1, 2]])
+
+ X,L,G = dare(A,B,Q,R,S,E)
+ # print "The solution obtained is", X
+ assert_array_almost_equal(dot(A.T,dot(X,A))-dot(E.T,dot(X,E)) - \
+ dot( dot(A.T,dot(X,B))+S , dot( inv(dot(B.T,dot(X,B)) + R) ,
+ dot(B.T,dot(X,A))+S.T)) + Q , zeros((2,2)) )
+ assert_array_almost_equal( dot( inv( dot(B.T,dot(X,B)) + R) , \
+ dot(B.T,dot(X,A)) + S.T ) , G)
+
+ A = array([[-0.6, 0],[-0.1, -0.4]])
+ Q = array([[2, 1],[1, 3]])
+ B = array([[1],[2]])
+ R = 1
+ S = array([[1],[2]])
+ E = array([[2, 1],[1, 2]])
+
+ X,L,G = dare(A,B,Q,R,S,E)
+ # print "The solution obtained is", X
+ assert_array_almost_equal(dot(A.T,dot(X,A))-dot(E.T,dot(X,E)) - \
+ dot( dot(A.T,dot(X,B))+S , dot( inv(dot(B.T,dot(X,B)) + R) ,
+ dot(B.T,dot(X,A))+S.T)) + Q , zeros((2,2)) )
+ assert_array_almost_equal( dot( 1 / ( dot(B.T,dot(X,B)) + R) , \
+ dot(B.T,dot(X,A)) + S.T ) , G)
+
+def suite():
+ return unittest.TestLoader().loadTestsFromTestCase(TestMatrixEquations)
+
+if __name__ == "__main__":
+ unittest.main()
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
