Mercurial > forge
changeset 8240:d61c0169b4fc octave-forge
control: add discrete-time support to ncfsyn (case D==0)
| author | paramaniac |
|---|---|
| date | Sun, 31 Jul 2011 21:57:09 +0000 |
| parents | 4aa6b72d861b |
| children | c8d721d2a848 |
| files | main/control/devel/ncfsyn/SB02OD.f main/control/devel/ncfsyn/SB02OU.f main/control/devel/ncfsyn/SB02OV.f main/control/devel/ncfsyn/SB02OW.f main/control/devel/ncfsyn/SB02OY.f main/control/devel/ncfsyn/SB10ID.dat main/control/devel/ncfsyn/SB10ID.res main/control/devel/ncfsyn/SB10KD.dat main/control/devel/ncfsyn/SB10KD.f main/control/devel/ncfsyn/SB10KD.res main/control/devel/ncfsyn/SB10ZD.dat main/control/devel/ncfsyn/SB10ZD.f main/control/devel/ncfsyn/SB10ZD.res main/control/devel/ncfsyn/makefile_ncfsyn.m main/control/devel/ncfsyn/ncfsyn.m main/control/devel/ncfsyn/slsb10kd.cc |
| diffstat | 16 files changed, 3889 insertions(+), 1 deletions(-) [+] |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ncfsyn/SB02OD.f Sun Jul 31 21:57:09 2011 +0000 @@ -0,0 +1,856 @@ + SUBROUTINE SB02OD( DICO, JOBB, FACT, UPLO, JOBL, SORT, N, M, P, A, + $ LDA, B, LDB, Q, LDQ, R, LDR, L, LDL, RCOND, X, + $ LDX, ALFAR, ALFAI, BETA, S, LDS, T, LDT, U, + $ LDU, TOL, IWORK, DWORK, LDWORK, BWORK, INFO ) +C +C SLICOT RELEASE 5.0. +C +C Copyright (c) 2002-2010 NICONET e.V. +C +C This program is free software: you can redistribute it and/or +C modify it under the terms of the GNU General Public License as +C published by the Free Software Foundation, either version 2 of +C the License, or (at your option) any later version. +C +C This program is distributed in the hope that it will be useful, +C but WITHOUT ANY WARRANTY; without even the implied warranty of +C MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +C GNU General Public License for more details. +C +C You should have received a copy of the GNU General Public License +C along with this program. If not, see +C <http://www.gnu.org/licenses/>. +C +C PURPOSE +C +C To solve for X either the continuous-time algebraic Riccati +C equation +C -1 +C Q + A'X + XA - (L+XB)R (L+XB)' = 0 (1) +C +C or the discrete-time algebraic Riccati equation +C -1 +C X = A'XA - (L+A'XB)(R + B'XB) (L+A'XB)' + Q (2) +C +C where A, B, Q, R, and L are N-by-N, N-by-M, N-by-N, M-by-M and +C N-by-M matrices, respectively, such that Q = C'C, R = D'D and +C L = C'D; X is an N-by-N symmetric matrix. +C The routine also returns the computed values of the closed-loop +C spectrum of the system, i.e., the stable eigenvalues lambda(1), +C ..., lambda(N) of the corresponding Hamiltonian or symplectic +C pencil, in the continuous-time case or discrete-time case, +C respectively. +C -1 +C Optionally, matrix G = BR B' may be given instead of B and R. +C Other options include the case with Q and/or R given in a +C factored form, Q = C'C, R = D'D, and with L a zero matrix. +C +C The routine uses the method of deflating subspaces, based on +C reordering the eigenvalues in a generalized Schur matrix pair. +C A standard eigenproblem is solved in the continuous-time case +C if G is given. +C +C ARGUMENTS +C +C Mode Parameters +C +C DICO CHARACTER*1 +C Specifies the type of Riccati equation to be solved as +C follows: +C = 'C': Equation (1), continuous-time case; +C = 'D': Equation (2), discrete-time case. +C +C JOBB CHARACTER*1 +C Specifies whether or not the matrix G is given, instead +C of the matrices B and R, as follows: +C = 'B': B and R are given; +C = 'G': G is given. +C +C FACT CHARACTER*1 +C Specifies whether or not the matrices Q and/or R (if +C JOBB = 'B') are factored, as follows: +C = 'N': Not factored, Q and R are given; +C = 'C': C is given, and Q = C'C; +C = 'D': D is given, and R = D'D; +C = 'B': Both factors C and D are given, Q = C'C, R = D'D. +C +C UPLO CHARACTER*1 +C If JOBB = 'G', or FACT = 'N', specifies which triangle of +C the matrices G and Q (if FACT = 'N'), or Q and R (if +C JOBB = 'B'), is stored, as follows: +C = 'U': Upper triangle is stored; +C = 'L': Lower triangle is stored. +C +C JOBL CHARACTER*1 +C Specifies whether or not the matrix L is zero, as follows: +C = 'Z': L is zero; +C = 'N': L is nonzero. +C JOBL is not used if JOBB = 'G' and JOBL = 'Z' is assumed. +C SLICOT Library routine SB02MT should be called just before +C SB02OD, for obtaining the results when JOBB = 'G' and +C JOBL = 'N'. +C +C SORT CHARACTER*1 +C Specifies which eigenvalues should be obtained in the top +C of the generalized Schur form, as follows: +C = 'S': Stable eigenvalues come first; +C = 'U': Unstable eigenvalues come first. +C +C Input/Output Parameters +C +C N (input) INTEGER +C The actual state dimension, i.e. the order of the matrices +C A, Q, and X, and the number of rows of the matrices B +C and L. N >= 0. +C +C M (input) INTEGER +C The number of system inputs. If JOBB = 'B', M is the +C order of the matrix R, and the number of columns of the +C matrix B. M >= 0. +C M is not used if JOBB = 'G'. +C +C P (input) INTEGER +C The number of system outputs. If FACT = 'C' or 'D' or 'B', +C P is the number of rows of the matrices C and/or D. +C P >= 0. +C Otherwise, P is not used. +C +C A (input) DOUBLE PRECISION array, dimension (LDA,N) +C The leading N-by-N part of this array must contain the +C state matrix A of the system. +C +C LDA INTEGER +C The leading dimension of array A. LDA >= MAX(1,N). +C +C B (input) DOUBLE PRECISION array, dimension (LDB,*) +C If JOBB = 'B', the leading N-by-M part of this array must +C contain the input matrix B of the system. +C If JOBB = 'G', the leading N-by-N upper triangular part +C (if UPLO = 'U') or lower triangular part (if UPLO = 'L') +C of this array must contain the upper triangular part or +C lower triangular part, respectively, of the matrix +C -1 +C G = BR B'. The stricly lower triangular part (if +C UPLO = 'U') or stricly upper triangular part (if +C UPLO = 'L') is not referenced. +C +C LDB INTEGER +C The leading dimension of array B. LDB >= MAX(1,N). +C +C Q (input) DOUBLE PRECISION array, dimension (LDQ,N) +C If FACT = 'N' or 'D', the leading N-by-N upper triangular +C part (if UPLO = 'U') or lower triangular part (if UPLO = +C 'L') of this array must contain the upper triangular part +C or lower triangular part, respectively, of the symmetric +C state weighting matrix Q. The stricly lower triangular +C part (if UPLO = 'U') or stricly upper triangular part (if +C UPLO = 'L') is not referenced. +C If JOBB = 'B', the triangular part of this array defined +C by UPLO is modified internally, but is restored on exit. +C If FACT = 'C' or 'B', the leading P-by-N part of this +C array must contain the output matrix C of the system. +C If JOBB = 'B', this part is modified internally, but is +C restored on exit. +C +C LDQ INTEGER +C The leading dimension of array Q. +C LDQ >= MAX(1,N) if FACT = 'N' or 'D', +C LDQ >= MAX(1,P) if FACT = 'C' or 'B'. +C +C R (input) DOUBLE PRECISION array, dimension (LDR,M) +C If FACT = 'N' or 'C', the leading M-by-M upper triangular +C part (if UPLO = 'U') or lower triangular part (if UPLO = +C 'L') of this array must contain the upper triangular part +C or lower triangular part, respectively, of the symmetric +C input weighting matrix R. The stricly lower triangular +C part (if UPLO = 'U') or stricly upper triangular part (if +C UPLO = 'L') is not referenced. +C The triangular part of this array defined by UPLO is +C modified internally, but is restored on exit. +C If FACT = 'D' or 'B', the leading P-by-M part of this +C array must contain the direct transmission matrix D of the +C system. This part is modified internally, but is restored +C on exit. +C If JOBB = 'G', this array is not referenced. +C +C LDR INTEGER +C The leading dimension of array R. +C LDR >= MAX(1,M) if JOBB = 'B' and FACT = 'N' or 'C'; +C LDR >= MAX(1,P) if JOBB = 'B' and FACT = 'D' or 'B'; +C LDR >= 1 if JOBB = 'G'. +C +C L (input) DOUBLE PRECISION array, dimension (LDL,M) +C If JOBL = 'N' (and JOBB = 'B'), the leading N-by-M part of +C this array must contain the cross weighting matrix L. +C This part is modified internally, but is restored on exit. +C If JOBL = 'Z' or JOBB = 'G', this array is not referenced. +C +C LDL INTEGER +C The leading dimension of array L. +C LDL >= MAX(1,N) if JOBL = 'N' and JOBB = 'B'; +C LDL >= 1 if JOBL = 'Z' or JOBB = 'G'. +C +C RCOND (output) DOUBLE PRECISION +C An estimate of the reciprocal of the condition number (in +C the 1-norm) of the N-th order system of algebraic +C equations from which the solution matrix X is obtained. +C +C X (output) DOUBLE PRECISION array, dimension (LDX,N) +C The leading N-by-N part of this array contains the +C solution matrix X of the problem. +C +C LDX INTEGER +C The leading dimension of array X. LDX >= MAX(1,N). +C +C ALFAR (output) DOUBLE PRECISION array, dimension (2*N) +C ALFAI (output) DOUBLE PRECISION array, dimension (2*N) +C BETA (output) DOUBLE PRECISION array, dimension (2*N) +C The generalized eigenvalues of the 2N-by-2N matrix pair, +C ordered as specified by SORT (if INFO = 0). For instance, +C if SORT = 'S', the leading N elements of these arrays +C contain the closed-loop spectrum of the system matrix +C A - BF, where F is the optimal feedback matrix computed +C based on the solution matrix X. Specifically, +C lambda(k) = [ALFAR(k)+j*ALFAI(k)]/BETA(k) for +C k = 1,2,...,N. +C If DICO = 'C' and JOBB = 'G', the elements of BETA are +C set to 1. +C +C S (output) DOUBLE PRECISION array, dimension (LDS,*) +C The leading 2N-by-2N part of this array contains the +C ordered real Schur form S of the first matrix in the +C reduced matrix pencil associated to the optimal problem, +C or of the corresponding Hamiltonian matrix, if DICO = 'C' +C and JOBB = 'G'. That is, +C +C (S S ) +C ( 11 12) +C S = ( ), +C (0 S ) +C ( 22) +C +C where S , S and S are N-by-N matrices. +C 11 12 22 +C Array S must have 2*N+M columns if JOBB = 'B', and 2*N +C columns, otherwise. +C +C LDS INTEGER +C The leading dimension of array S. +C LDS >= MAX(1,2*N+M) if JOBB = 'B', +C LDS >= MAX(1,2*N) if JOBB = 'G'. +C +C T (output) DOUBLE PRECISION array, dimension (LDT,2*N) +C If DICO = 'D' or JOBB = 'B', the leading 2N-by-2N part of +C this array contains the ordered upper triangular form T of +C the second matrix in the reduced matrix pencil associated +C to the optimal problem. That is, +C +C (T T ) +C ( 11 12) +C T = ( ), +C (0 T ) +C ( 22) +C +C where T , T and T are N-by-N matrices. +C 11 12 22 +C If DICO = 'C' and JOBB = 'G' this array is not referenced. +C +C LDT INTEGER +C The leading dimension of array T. +C LDT >= MAX(1,2*N+M) if JOBB = 'B', +C LDT >= MAX(1,2*N) if JOBB = 'G' and DICO = 'D', +C LDT >= 1 if JOBB = 'G' and DICO = 'C'. +C +C U (output) DOUBLE PRECISION array, dimension (LDU,2*N) +C The leading 2N-by-2N part of this array contains the right +C transformation matrix U which reduces the 2N-by-2N matrix +C pencil to the ordered generalized real Schur form (S,T), +C or the Hamiltonian matrix to the ordered real Schur +C form S, if DICO = 'C' and JOBB = 'G'. That is, +C +C (U U ) +C ( 11 12) +C U = ( ), +C (U U ) +C ( 21 22) +C +C where U , U , U and U are N-by-N matrices. +C 11 12 21 22 +C +C LDU INTEGER +C The leading dimension of array U. LDU >= MAX(1,2*N). +C +C Tolerances +C +C TOL DOUBLE PRECISION +C The tolerance to be used to test for near singularity of +C the original matrix pencil, specifically of the triangular +C factor obtained during the reduction process. If the user +C sets TOL > 0, then the given value of TOL is used as a +C lower bound for the reciprocal condition number of that +C matrix; a matrix whose estimated condition number is less +C than 1/TOL is considered to be nonsingular. If the user +C sets TOL <= 0, then a default tolerance, defined by +C TOLDEF = EPS, is used instead, where EPS is the machine +C precision (see LAPACK Library routine DLAMCH). +C This parameter is not referenced if JOBB = 'G'. +C +C Workspace +C +C IWORK INTEGER array, dimension (LIWORK) +C LIWORK >= MAX(1,M,2*N) if JOBB = 'B', +C LIWORK >= MAX(1,2*N) if JOBB = 'G'. +C +C DWORK DOUBLE PRECISION array, dimension (LDWORK) +C On exit, if INFO = 0, DWORK(1) returns the optimal value +C of LDWORK. If JOBB = 'B' and N > 0, DWORK(2) returns the +C reciprocal of the condition number of the M-by-M lower +C triangular matrix obtained after compressing the matrix +C pencil of order 2N+M to obtain a pencil of order 2N. +C If INFO = 0 or INFO = 6, DWORK(3) returns the scaling +C factor used internally, which should multiply the +C submatrix Y2 to recover X from the first N columns of U +C (see METHOD). +C +C LDWORK INTEGER +C The length of the array DWORK. +C LDWORK >= MAX(3,6*N), if JOBB = 'G', +C DICO = 'C'; +C LDWORK >= MAX(7*(2*N+1)+16,16*N), if JOBB = 'G', +C DICO = 'D'; +C LDWORK >= MAX(7*(2*N+1)+16,16*N,2*N+M,3*M), if JOBB = 'B'. +C For optimum performance LDWORK should be larger. +C +C BWORK LOGICAL array, dimension (2*N) +C +C Error Indicator +C +C INFO INTEGER +C = 0: successful exit; +C < 0: if INFO = -i, the i-th argument had an illegal +C value; +C = 1: if the computed extended matrix pencil is singular, +C possibly due to rounding errors; +C = 2: if the QZ (or QR) algorithm failed; +C = 3: if reordering of the (generalized) eigenvalues +C failed; +C = 4: if after reordering, roundoff changed values of +C some complex eigenvalues so that leading eigenvalues +C in the (generalized) Schur form no longer satisfy +C the stability condition; this could also be caused +C due to scaling; +C = 5: if the computed dimension of the solution does not +C equal N; +C = 6: if a singular matrix was encountered during the +C computation of the solution matrix X. +C +C METHOD +C +C The routine uses a variant of the method of deflating subspaces +C proposed by van Dooren [1]. See also [2], [3]. +C It is assumed that (A,B) is stabilizable and (C,A) is detectable. +C Under these assumptions the algebraic Riccati equation is known to +C have a unique non-negative definite solution. +C The first step in the method of deflating subspaces is to form the +C extended Hamiltonian matrices, dimension 2N + M given by +C +C discrete-time continuous-time +C +C |A 0 B| |I 0 0| |A 0 B| |I 0 0| +C |Q -I L| - z |0 -A' 0|, |Q A' L| - s |0 -I 0|. +C |L' 0 R| |0 -B' 0| |L' B' R| |0 0 0| +C +C Next, these pencils are compressed to a form (see [1]) +C +C lambda x A - B . +C f f +C +C This generalized eigenvalue problem is then solved using the QZ +C algorithm and the stable deflating subspace Ys is determined. +C If [Y1'|Y2']' is a basis for Ys, then the required solution is +C -1 +C X = Y2 x Y1 . +C A standard eigenvalue problem is solved using the QR algorithm in +C the continuous-time case when G is given (DICO = 'C', JOBB = 'G'). +C +C REFERENCES +C +C [1] Van Dooren, P. +C A Generalized Eigenvalue Approach for Solving Riccati +C Equations. +C SIAM J. Sci. Stat. Comp., 2, pp. 121-135, 1981. +C +C [2] Mehrmann, V. +C The Autonomous Linear Quadratic Control Problem. Theory and +C Numerical Solution. +C Lect. Notes in Control and Information Sciences, vol. 163, +C Springer-Verlag, Berlin, 1991. +C +C [3] Sima, V. +C Algorithms for Linear-Quadratic Optimization. +C Pure and Applied Mathematics: A Series of Monographs and +C Textbooks, vol. 200, Marcel Dekker, Inc., New York, 1996. +C +C NUMERICAL ASPECTS +C +C This routine is particularly suited for systems where the matrix R +C is ill-conditioned. Internal scaling is used. +C +C FURTHER COMMENTS +C +C To obtain a stabilizing solution of the algebraic Riccati +C equations set SORT = 'S'. +C +C The routine can also compute the anti-stabilizing solutions of +C the algebraic Riccati equations, by specifying SORT = 'U'. +C +C CONTRIBUTOR +C +C Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Sep. 1997. +C Supersedes Release 2.0 routine SB02CD by T.G.J. Beelen, Philips, +C Eindhoven, Holland. +C +C REVISIONS +C +C V. Sima, Katholieke Univ. Leuven, Belgium, May 1999, June 2002, +C December 2002, January 2005. +C +C KEYWORDS +C +C Algebraic Riccati equation, closed loop system, continuous-time +C system, discrete-time system, optimal regulator, Schur form. +C +C ****************************************************************** +C +C .. Parameters .. + DOUBLE PRECISION ZERO, HALF, ONE, THREE + PARAMETER ( ZERO = 0.0D0, HALF = 0.5D0, ONE = 1.0D0, + $ THREE = 3.0D0 ) +C .. Scalar Arguments .. + CHARACTER DICO, FACT, JOBB, JOBL, SORT, UPLO + INTEGER INFO, LDA, LDB, LDL, LDQ, LDR, LDS, LDT, LDU, + $ LDWORK, LDX, M, N, P + DOUBLE PRECISION RCOND, TOL +C .. Array Arguments .. + LOGICAL BWORK(*) + INTEGER IWORK(*) + DOUBLE PRECISION A(LDA,*), ALFAI(*), ALFAR(*), B(LDB,*), BETA(*), + $ DWORK(*), L(LDL,*), Q(LDQ,*), R(LDR,*), + $ S(LDS,*), T(LDT,*), U(LDU,*), X(LDX,*) +C .. Local Scalars .. + CHARACTER QTYPE, RTYPE + LOGICAL DISCR, LFACB, LFACN, LFACQ, LFACR, LJOBB, LJOBL, + $ LJOBLN, LSCAL, LSCL, LSORT, LUPLO + INTEGER I, INFO1, J, LDW, MP, NDIM, NN, NNM, NP, NP1, + $ WRKOPT + DOUBLE PRECISION QSCAL, RCONDL, RNORM, RSCAL, SCALE, UNORM +C .. Local Arrays .. + DOUBLE PRECISION DUM(1) +C .. External Functions .. + LOGICAL LSAME, SB02MR, SB02MV, SB02OU, SB02OV, SB02OW + DOUBLE PRECISION DLAMCH, DLANGE, DLANSY + EXTERNAL DLAMCH, DLANGE, DLANSY, LSAME, SB02MR, SB02MV, + $ SB02OU, SB02OV, SB02OW +C .. External Subroutines .. + EXTERNAL DAXPY, DCOPY, DGECON, DGEES, DGETRF, DGETRS, + $ DGGES, DLACPY, DLASCL, DLASET, DSCAL, DSWAP, + $ SB02OY, XERBLA +C .. Intrinsic Functions .. + INTRINSIC INT, MAX, MIN, SQRT +C .. Executable Statements .. +C + INFO = 0 + DISCR = LSAME( DICO, 'D' ) + LJOBB = LSAME( JOBB, 'B' ) + LFACN = LSAME( FACT, 'N' ) + LFACQ = LSAME( FACT, 'C' ) + LFACR = LSAME( FACT, 'D' ) + LFACB = LSAME( FACT, 'B' ) + LUPLO = LSAME( UPLO, 'U' ) + LSORT = LSAME( SORT, 'S' ) +C + NN = 2*N + IF ( LJOBB ) THEN + LJOBL = LSAME( JOBL, 'Z' ) + LJOBLN = LSAME( JOBL, 'N' ) + NNM = NN + M + LDW = MAX( NNM, 3*M ) + ELSE + NNM = NN + LDW = 1 + END IF + NP1 = N + 1 +C +C Test the input scalar arguments. +C + IF( .NOT.DISCR .AND. .NOT.LSAME( DICO, 'C' ) ) THEN + INFO = -1 + ELSE IF( .NOT.LJOBB .AND. .NOT.LSAME( JOBB, 'G' ) ) THEN + INFO = -2 + ELSE IF( .NOT.LFACQ .AND. .NOT.LFACR .AND. .NOT.LFACB + $ .AND. .NOT.LFACN ) THEN + INFO = -3 + ELSE IF( .NOT.LJOBB .OR. LFACN ) THEN + IF( .NOT.LUPLO .AND. .NOT.LSAME( UPLO, 'L' ) ) + $ INFO = -4 + END IF + IF( INFO.EQ.0 .AND. LJOBB ) THEN + IF( .NOT.LJOBL .AND. .NOT.LJOBLN ) + $ INFO = -5 + END IF + IF( INFO.EQ.0 ) THEN + IF( .NOT.LSORT .AND. .NOT.LSAME( SORT, 'U' ) ) THEN + INFO = -6 + ELSE IF( N.LT.0 ) THEN + INFO = -7 + ELSE IF( LJOBB ) THEN + IF( M.LT.0 ) + $ INFO = -8 + END IF + END IF + IF( INFO.EQ.0 .AND. .NOT.LFACN ) THEN + IF( P.LT.0 ) + $ INFO = -9 + END IF + IF( INFO.EQ.0 ) THEN + IF( LDA.LT.MAX( 1, N ) ) THEN + INFO = -11 + ELSE IF( LDB.LT.MAX( 1, N ) ) THEN + INFO = -13 + ELSE IF( ( ( LFACN.OR.LFACR ) .AND. LDQ.LT.MAX( 1, N ) ) .OR. + $ ( ( LFACQ.OR.LFACB ) .AND. LDQ.LT.MAX( 1, P ) ) ) THEN + INFO = -15 + ELSE IF( LDR.LT.1 ) THEN + INFO = -17 + ELSE IF( LDL.LT.1 ) THEN + INFO = -19 + ELSE IF( LJOBB ) THEN + IF ( ( LFACN.OR.LFACQ ) .AND. LDR.LT.M .OR. + $ ( LFACR.OR.LFACB ) .AND. LDR.LT.P ) THEN + INFO = -17 + ELSE IF( LJOBLN .AND. LDL.LT.N ) THEN + INFO = -19 + END IF + END IF + END IF + IF( INFO.EQ.0 ) THEN + IF( LDX.LT.MAX( 1, N ) ) THEN + INFO = -22 + ELSE IF( LDS.LT.MAX( 1, NNM ) ) THEN + INFO = -27 + ELSE IF( LDT.LT.1 ) THEN + INFO = -29 + ELSE IF( LDU.LT.MAX( 1, NN ) ) THEN + INFO = -31 + ELSE IF( LDWORK.LT.MAX( 3, 6*N ) ) THEN + INFO = -35 + ELSE IF( DISCR .OR. LJOBB ) THEN + IF( LDT.LT.NNM ) THEN + INFO = -29 + ELSE IF( LDWORK.LT.MAX( 14*N + 23, 16*N, LDW ) ) THEN + INFO = -35 + END IF + END IF + END IF +C + IF ( INFO.NE.0 ) THEN +C +C Error return. +C + CALL XERBLA( 'SB02OD', -INFO ) + RETURN + END IF +C +C Quick return if possible. +C + IF ( N.EQ.0 ) THEN + RCOND = ONE + DWORK(1) = THREE + DWORK(3) = ONE + RETURN + END IF +C +C Always scale the matrix pencil. +C + LSCAL = .TRUE. +C +C Start computations. +C +C (Note: Comments in the code beginning "Workspace:" describe the +C minimal amount of real workspace needed at that point in the +C code, as well as the preferred amount for good performance. +C NB refers to the optimal block size for the immediately +C following subroutine, as returned by ILAENV.) +C + IF ( LSCAL .AND. LJOBB ) THEN +C +C Scale the matrices Q, R, and L so that +C norm(Q) + norm(R) + norm(L) = 1, +C using the 1-norm. If Q and/or R are factored, the norms of +C the factors are used. +C Workspace: need max(N,M), if FACT = 'N'; +C N, if FACT = 'D'; +C M, if FACT = 'C'. +C + IF ( LFACN .OR. LFACR ) THEN + SCALE = DLANSY( '1-norm', UPLO, N, Q, LDQ, DWORK ) + QTYPE = UPLO + NP = N + ELSE + SCALE = DLANGE( '1-norm', P, N, Q, LDQ, DWORK ) + QTYPE = 'G' + NP = P + END IF +C + IF ( LFACN .OR. LFACQ ) THEN + RNORM = DLANSY( '1-norm', UPLO, M, R, LDR, DWORK ) + RTYPE = UPLO + MP = M + ELSE + RNORM = DLANGE( '1-norm', P, M, R, LDR, DWORK ) + RTYPE = 'G' + MP = P + END IF + SCALE = SCALE + RNORM +C + IF ( LJOBLN ) + $ SCALE = SCALE + DLANGE( '1-norm', N, M, L, LDL, DWORK ) + IF ( SCALE.EQ.ZERO ) + $ SCALE = ONE +C + IF ( LFACN .OR. LFACR ) THEN + QSCAL = SCALE + ELSE + QSCAL = SQRT( SCALE ) + END IF +C + IF ( LFACN .OR. LFACQ ) THEN + RSCAL = SCALE + ELSE + RSCAL = SQRT( SCALE ) + END IF +C + CALL DLASCL( QTYPE, 0, 0, QSCAL, ONE, NP, N, Q, LDQ, INFO1 ) + CALL DLASCL( RTYPE, 0, 0, RSCAL, ONE, MP, M, R, LDR, INFO1 ) + IF ( LJOBLN ) + $ CALL DLASCL( 'G', 0, 0, SCALE, ONE, N, M, L, LDL, INFO1 ) + END IF +C +C Construct the extended matrix pair. +C +C Workspace: need 1, if JOBB = 'G', +C max(1,2*N+M,3*M), if JOBB = 'B'; +C prefer larger. +C + CALL SB02OY( 'Optimal control', DICO, JOBB, FACT, UPLO, JOBL, + $ 'Identity E', N, M, P, A, LDA, B, LDB, Q, LDQ, R, + $ LDR, L, LDL, U, 1, S, LDS, T, LDT, TOL, IWORK, DWORK, + $ LDWORK, INFO ) +C + IF ( LSCAL .AND. LJOBB ) THEN +C +C Undo scaling of the data arrays. +C + CALL DLASCL( QTYPE, 0, 0, ONE, QSCAL, NP, N, Q, LDQ, INFO1 ) + CALL DLASCL( RTYPE, 0, 0, ONE, RSCAL, MP, M, R, LDR, INFO1 ) + IF ( LJOBLN ) + $ CALL DLASCL( 'G', 0, 0, ONE, SCALE, N, M, L, LDL, INFO1 ) + END IF +C + IF ( INFO.NE.0 ) + $ RETURN + WRKOPT = DWORK(1) + IF ( LJOBB ) RCONDL = DWORK(2) +C + IF ( LSCAL .AND. .NOT.LJOBB ) THEN +C +C This part of the code is used when G is given (JOBB = 'G'). +C A standard eigenproblem is solved in the continuous-time case. +C Scale the Hamiltonian matrix S, if DICO = 'C', or the +C symplectic pencil (S,T), if DICO = 'D', using the square roots +C of the norms of the matrices Q and G. +C Workspace: need N. +C + IF ( LFACN .OR. LFACR ) THEN + SCALE = SQRT( DLANSY( '1-norm', UPLO, N, Q, LDQ, DWORK ) ) + ELSE + SCALE = DLANGE( '1-norm', P, N, Q, LDQ, DWORK ) + END IF + RNORM = SQRT( DLANSY( '1-norm', UPLO, N, B, LDB, DWORK ) ) +C + LSCL = MIN( SCALE, RNORM ).GT.ZERO .AND. SCALE.NE.RNORM +C + IF( LSCL ) THEN + IF( DISCR ) THEN + CALL DLASCL( 'G', 0, 0, SCALE, RNORM, N, N, S(NP1,1), + $ LDS, INFO1 ) + CALL DLASCL( 'G', 0, 0, RNORM, SCALE, N, N, T(1,NP1), + $ LDT, INFO1 ) + ELSE + CALL DLASCL( 'G', 0, 0, SCALE, -RNORM, N, N, S(NP1,1), + $ LDS, INFO1 ) + CALL DLASCL( 'G', 0, 0, RNORM, SCALE, N, N, S(1,NP1), + $ LDS, INFO1 ) + CALL DLASCL( 'G', 0, 0, ONE, -ONE, N, N, S(NP1,NP1), + $ LDS, INFO1 ) + END IF + ELSE + IF( .NOT.DISCR ) THEN + CALL DLASCL( 'G', 0, 0, ONE, -ONE, N, NN, S(NP1,1), LDS, + $ INFO1 ) + END IF + END IF + ELSE + LSCL = .FALSE. + END IF +C +C Workspace: need max(7*(2*N+1)+16,16*N), +C if JOBB = 'B' or DICO = 'D'; +C 6*N, if JOBB = 'G' and DICO = 'C'; +C prefer larger. +C + IF ( DISCR ) THEN + IF ( LSORT ) THEN +C +C The natural tendency of the QZ algorithm to get the largest +C eigenvalues in the leading part of the matrix pair is +C exploited, by computing the unstable eigenvalues of the +C permuted matrix pair. +C + CALL DGGES( 'No vectors', 'Vectors', 'Sort', SB02OV, NN, T, + $ LDT, S, LDS, NDIM, ALFAR, ALFAI, BETA, U, LDU, + $ U, LDU, DWORK, LDWORK, BWORK, INFO1 ) + CALL DSWAP( N, ALFAR(NP1), 1, ALFAR, 1 ) + CALL DSWAP( N, ALFAI(NP1), 1, ALFAI, 1 ) + CALL DSWAP( N, BETA (NP1), 1, BETA, 1 ) + ELSE + CALL DGGES( 'No vectors', 'Vectors', 'Sort', SB02OV, NN, S, + $ LDS, T, LDT, NDIM, ALFAR, ALFAI, BETA, U, LDU, + $ U, LDU, DWORK, LDWORK, BWORK, INFO1 ) + END IF + ELSE + IF ( LJOBB ) THEN + IF ( LSORT ) THEN + CALL DGGES( 'No vectors', 'Vectors', 'Sort', SB02OW, NN, + $ S, LDS, T, LDT, NDIM, ALFAR, ALFAI, BETA, U, + $ LDU, U, LDU, DWORK, LDWORK, BWORK, INFO1 ) + ELSE + CALL DGGES( 'No vectors', 'Vectors', 'Sort', SB02OU, NN, + $ S, LDS, T, LDT, NDIM, ALFAR, ALFAI, BETA, U, + $ LDU, U, LDU, DWORK, LDWORK, BWORK, INFO1 ) + END IF + ELSE + IF ( LSORT ) THEN + CALL DGEES( 'Vectors', 'Sort', SB02MV, NN, S, LDS, NDIM, + $ ALFAR, ALFAI, U, LDU, DWORK, LDWORK, BWORK, + $ INFO1 ) + ELSE + CALL DGEES( 'Vectors', 'Sort', SB02MR, NN, S, LDS, NDIM, + $ ALFAR, ALFAI, U, LDU, DWORK, LDWORK, BWORK, + $ INFO1 ) + END IF + DUM(1) = ONE + CALL DCOPY( NN, DUM, 0, BETA, 1 ) + END IF + END IF + IF ( INFO1.GT.0 .AND. INFO1.LE.NN+1 ) THEN + INFO = 2 + ELSE IF ( INFO1.EQ.NN+2 ) THEN + INFO = 4 + ELSE IF ( INFO1.EQ.NN+3 ) THEN + INFO = 3 + ELSE IF ( NDIM.NE.N ) THEN + INFO = 5 + END IF + IF ( INFO.NE.0 ) + $ RETURN + WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) ) +C +C Select submatrices U1 and U2 out of the array U which define the +C solution X = U2 x inv(U1). +C Since X = X' we may obtain X as the solution of the system of +C linear equations U1' x X = U2', where +C U1 = U(1:n, 1:n), +C U2 = U(n+1:2n, 1:n). +C Use the (2,1) block of S as a workspace for factoring U1. +C + DO 20 J = 1, N + CALL DCOPY( N, U(NP1,J), 1, X(J,1), LDX ) + 20 CONTINUE +C + CALL DLACPY( 'Full', N, N, U, LDU, S(NP1,1), LDS ) +C +C Check if U1 is singular. +C + UNORM = DLANGE( '1-norm', N, N, S(NP1,1), LDS, DWORK ) +C +C Solve the system U1' x X = U2'. +C + CALL DGETRF( N, N, S(NP1,1), LDS, IWORK, INFO1 ) + IF ( INFO1.NE.0 ) THEN + INFO = 6 + DWORK(3) = ONE + IF ( LSCAL ) THEN + IF ( LJOBB ) THEN + DWORK(3) = SCALE + ELSE IF ( LSCL ) THEN + DWORK(3) = SCALE / RNORM + END IF + END IF + RETURN + ELSE +C +C Estimate the reciprocal condition of U1. +C Workspace: need 3*N. +C + CALL DGECON( '1-norm', N, S(NP1,1), LDS, UNORM, RCOND, DWORK, + $ IWORK(NP1), INFO ) +C + IF ( RCOND.LT.DLAMCH( 'Epsilon' ) ) THEN +C +C Nearly singular matrix. Set INFO for error return. +C + INFO = 6 + RETURN + END IF + WRKOPT = MAX( WRKOPT, 3*N ) + CALL DGETRS( 'Transpose', N, N, S(NP1,1), LDS, IWORK, X, LDX, + $ INFO1 ) +C +C Set S(2,1) to zero. +C + CALL DLASET( 'Full', N, N, ZERO, ZERO, S(NP1,1), LDS ) +C + IF ( LSCAL ) THEN +C +C Prepare to undo scaling for the solution X. +C + IF ( .NOT.LJOBB ) THEN + IF ( LSCL ) THEN + SCALE = SCALE / RNORM + ELSE + SCALE = ONE + END IF + END IF + DWORK(3) = SCALE + SCALE = HALF*SCALE + ELSE + DWORK(3) = ONE + SCALE = HALF + END IF +C +C Make sure the solution matrix X is symmetric. +C + DO 40 I = 1, N + CALL DAXPY( N-I+1, ONE, X(I,I), LDX, X(I,I), 1 ) + CALL DSCAL( N-I+1, SCALE, X(I,I), 1 ) + CALL DCOPY( N-I+1, X(I,I), 1, X(I,I), LDX ) + 40 CONTINUE + END IF +C + DWORK(1) = WRKOPT + IF ( LJOBB ) DWORK(2) = RCONDL +C + RETURN +C *** Last line of SB02OD *** + END
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ncfsyn/SB02OU.f Sun Jul 31 21:57:09 2011 +0000 @@ -0,0 +1,83 @@ + LOGICAL FUNCTION SB02OU( ALPHAR, ALPHAI, BETA ) +C +C SLICOT RELEASE 5.0. +C +C Copyright (c) 2002-2010 NICONET e.V. +C +C This program is free software: you can redistribute it and/or +C modify it under the terms of the GNU General Public License as +C published by the Free Software Foundation, either version 2 of +C the License, or (at your option) any later version. +C +C This program is distributed in the hope that it will be useful, +C but WITHOUT ANY WARRANTY; without even the implied warranty of +C MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +C GNU General Public License for more details. +C +C You should have received a copy of the GNU General Public License +C along with this program. If not, see +C <http://www.gnu.org/licenses/>. +C +C PURPOSE +C +C To select the unstable generalized eigenvalues for solving the +C continuous-time algebraic Riccati equation. +C +C ARGUMENTS +C +C Input/Output Parameters +C +C ALPHAR (input) DOUBLE PRECISION +C The real part of the numerator of the current eigenvalue +C considered. +C +C ALPHAI (input) DOUBLE PRECISION +C The imaginary part of the numerator of the current +C eigenvalue considered. +C +C BETA (input) DOUBLE PRECISION +C The (real) denominator of the current eigenvalue +C considered. It is assumed that BETA <> 0 (regular case). +C +C METHOD +C +C The function value SB02OU is set to .TRUE. for an unstable +C eigenvalue and to .FALSE., otherwise. +C +C REFERENCES +C +C None. +C +C NUMERICAL ASPECTS +C +C None. +C +C CONTRIBUTOR +C +C Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Sep. 1997. +C Supersedes Release 2.0 routine SB02CW by P. Van Dooren, Philips +C Research Laboratory, Brussels, Belgium. +C +C REVISIONS +C +C - +C +C KEYWORDS +C +C Algebraic Riccati equation, closed loop system, continuous-time +C system, optimal regulator, Schur form. +C +C ****************************************************************** +C + DOUBLE PRECISION ZERO + PARAMETER ( ZERO = 0.0D0 ) +C .. Scalar Arguments .. + DOUBLE PRECISION ALPHAR, ALPHAI, BETA +C .. Executable Statements .. +C + SB02OU = ( ALPHAR.LT.ZERO .AND. BETA.LT.ZERO ) .OR. + $ ( ALPHAR.GT.ZERO .AND. BETA.GT.ZERO ) +C + RETURN +C *** Last line of SB02OU *** + END
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ncfsyn/SB02OV.f Sun Jul 31 21:57:09 2011 +0000 @@ -0,0 +1,88 @@ + LOGICAL FUNCTION SB02OV( ALPHAR, ALPHAI, BETA ) +C +C SLICOT RELEASE 5.0. +C +C Copyright (c) 2002-2010 NICONET e.V. +C +C This program is free software: you can redistribute it and/or +C modify it under the terms of the GNU General Public License as +C published by the Free Software Foundation, either version 2 of +C the License, or (at your option) any later version. +C +C This program is distributed in the hope that it will be useful, +C but WITHOUT ANY WARRANTY; without even the implied warranty of +C MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +C GNU General Public License for more details. +C +C You should have received a copy of the GNU General Public License +C along with this program. If not, see +C <http://www.gnu.org/licenses/>. +C +C PURPOSE +C +C To select the unstable generalized eigenvalues for solving the +C discrete-time algebraic Riccati equation. +C +C ARGUMENTS +C +C Input/Output Parameters +C +C ALPHAR (input) DOUBLE PRECISION +C The real part of the numerator of the current eigenvalue +C considered. +C +C ALPHAI (input) DOUBLE PRECISION +C The imaginary part of the numerator of the current +C eigenvalue considered. +C +C BETA (input) DOUBLE PRECISION +C The (real) denominator of the current eigenvalue +C considered. +C +C METHOD +C +C The function value SB02OV is set to .TRUE. for an unstable +C eigenvalue (i.e., with modulus greater than or equal to one) and +C to .FALSE., otherwise. +C +C REFERENCES +C +C None. +C +C NUMERICAL ASPECTS +C +C None. +C +C CONTRIBUTOR +C +C Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Sep. 1997. +C Supersedes Release 2.0 routine SB02CX by P. Van Dooren, Philips +C Research Laboratory, Brussels, Belgium. +C +C REVISIONS +C +C - +C +C KEYWORDS +C +C Algebraic Riccati equation, closed loop system, continuous-time +C system, optimal regulator, Schur form. +C +C ****************************************************************** +C + DOUBLE PRECISION ONE + PARAMETER ( ONE = 1.0D0 ) +C .. Scalar Arguments .. + DOUBLE PRECISION ALPHAR, ALPHAI, BETA +C .. External Functions .. + DOUBLE PRECISION DLAPY2 + EXTERNAL DLAPY2 +C .. Intrinsic Functions .. + INTRINSIC ABS +C .. Executable Statements .. +C + SB02OV = DLAPY2( ALPHAR, ALPHAI ).GE.ABS( BETA ) +C + RETURN +C *** Last line of SB02OV *** + END
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ncfsyn/SB02OW.f Sun Jul 31 21:57:09 2011 +0000 @@ -0,0 +1,83 @@ + LOGICAL FUNCTION SB02OW( ALPHAR, ALPHAI, BETA ) +C +C SLICOT RELEASE 5.0. +C +C Copyright (c) 2002-2010 NICONET e.V. +C +C This program is free software: you can redistribute it and/or +C modify it under the terms of the GNU General Public License as +C published by the Free Software Foundation, either version 2 of +C the License, or (at your option) any later version. +C +C This program is distributed in the hope that it will be useful, +C but WITHOUT ANY WARRANTY; without even the implied warranty of +C MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +C GNU General Public License for more details. +C +C You should have received a copy of the GNU General Public License +C along with this program. If not, see +C <http://www.gnu.org/licenses/>. +C +C PURPOSE +C +C To select the stable generalized eigenvalues for solving the +C continuous-time algebraic Riccati equation. +C +C ARGUMENTS +C +C Input/Output Parameters +C +C ALPHAR (input) DOUBLE PRECISION +C The real part of the numerator of the current eigenvalue +C considered. +C +C ALPHAI (input) DOUBLE PRECISION +C The imaginary part of the numerator of the current +C eigenvalue considered. +C +C BETA (input) DOUBLE PRECISION +C The (real) denominator of the current eigenvalue +C considered. It is assumed that BETA <> 0 (regular case). +C +C METHOD +C +C The function value SB02OW is set to .TRUE. for a stable eigenvalue +C and to .FALSE., otherwise. +C +C REFERENCES +C +C None. +C +C NUMERICAL ASPECTS +C +C None. +C +C CONTRIBUTOR +C +C Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Sep. 1997. +C Supersedes Release 2.0 routine SB02CW by P. Van Dooren, Philips +C Research Laboratory, Brussels, Belgium. +C +C REVISIONS +C +C - +C +C KEYWORDS +C +C Algebraic Riccati equation, closed loop system, continuous-time +C system, optimal regulator, Schur form. +C +C ****************************************************************** +C + DOUBLE PRECISION ZERO + PARAMETER ( ZERO = 0.0D0 ) +C .. Scalar Arguments .. + DOUBLE PRECISION ALPHAR, ALPHAI, BETA +C .. Executable Statements .. +C + SB02OW = ( ALPHAR.LT.ZERO .AND. BETA.GT.ZERO ) .OR. + $ ( ALPHAR.GT.ZERO .AND. BETA.LT.ZERO ) +C + RETURN +C *** Last line of SB02OW *** + END
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ncfsyn/SB02OY.f Sun Jul 31 21:57:09 2011 +0000 @@ -0,0 +1,791 @@ + SUBROUTINE SB02OY( TYPE, DICO, JOBB, FACT, UPLO, JOBL, JOBE, N, M, + $ P, A, LDA, B, LDB, Q, LDQ, R, LDR, L, LDL, E, + $ LDE, AF, LDAF, BF, LDBF, TOL, IWORK, DWORK, + $ LDWORK, INFO ) +C +C SLICOT RELEASE 5.0. +C +C Copyright (c) 2002-2010 NICONET e.V. +C +C This program is free software: you can redistribute it and/or +C modify it under the terms of the GNU General Public License as +C published by the Free Software Foundation, either version 2 of +C the License, or (at your option) any later version. +C +C This program is distributed in the hope that it will be useful, +C but WITHOUT ANY WARRANTY; without even the implied warranty of +C MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +C GNU General Public License for more details. +C +C You should have received a copy of the GNU General Public License +C along with this program. If not, see +C <http://www.gnu.org/licenses/>. +C +C PURPOSE +C +C To construct the extended matrix pairs for the computation of the +C solution of the algebraic matrix Riccati equations arising in the +C problems of optimal control, both discrete and continuous-time, +C and of spectral factorization, both discrete and continuous-time. +C These matrix pairs, of dimension 2N + M, are given by +C +C discrete-time continuous-time +C +C |A 0 B| |E 0 0| |A 0 B| |E 0 0| +C |Q -E' L| - z |0 -A' 0|, |Q A' L| - s |0 -E' 0|. (1) +C |L' 0 R| |0 -B' 0| |L' B' R| |0 0 0| +C +C After construction, these pencils are compressed to a form +C (see [1]) +C +C lambda x A - B , +C f f +C +C where A and B are 2N-by-2N matrices. +C f f +C -1 +C Optionally, matrix G = BR B' may be given instead of B and R; +C then, for L = 0, 2N-by-2N matrix pairs are directly constructed as +C +C discrete-time continuous-time +C +C |A 0 | |E G | |A -G | |E 0 | +C | | - z | |, | | - s | |. (2) +C |Q -E'| |0 -A'| |Q A'| |0 -E'| +C +C Similar pairs are obtained for non-zero L, if SLICOT Library +C routine SB02MT is called before SB02OY. +C Other options include the case with E identity matrix, L a zero +C matrix, or Q and/or R given in a factored form, Q = C'C, R = D'D. +C For spectral factorization problems, there are minor differences +C (e.g., B is replaced by C'). +C The second matrix in (2) is not constructed in the continuous-time +C case if E is specified as being an identity matrix. +C +C ARGUMENTS +C +C Mode Parameters +C +C TYPE CHARACTER*1 +C Specifies the type of problem to be addressed as follows: +C = 'O': Optimal control problem; +C = 'S': Spectral factorization problem. +C +C DICO CHARACTER*1 +C Specifies the type of linear system considered as follows: +C = 'C': Continuous-time system; +C = 'D': Discrete-time system. +C +C JOBB CHARACTER*1 +C Specifies whether or not the matrix G is given, instead +C of the matrices B and R, as follows: +C = 'B': B and R are given; +C = 'G': G is given. +C For JOBB = 'G', a 2N-by-2N matrix pair is directly +C obtained assuming L = 0 (see the description of JOBL). +C +C FACT CHARACTER*1 +C Specifies whether or not the matrices Q and/or R (if +C JOBB = 'B') are factored, as follows: +C = 'N': Not factored, Q and R are given; +C = 'C': C is given, and Q = C'C; +C = 'D': D is given, and R = D'D (if TYPE = 'O'), or +C R = D + D' (if TYPE = 'S'); +C = 'B': Both factors C and D are given, Q = C'C, R = D'D +C (or R = D + D'). +C +C UPLO CHARACTER*1 +C If JOBB = 'G', or FACT = 'N', specifies which triangle of +C the matrices G and Q (if FACT = 'N'), or Q and R (if +C JOBB = 'B'), is stored, as follows: +C = 'U': Upper triangle is stored; +C = 'L': Lower triangle is stored. +C +C JOBL CHARACTER*1 +C Specifies whether or not the matrix L is zero, as follows: +C = 'Z': L is zero; +C = 'N': L is nonzero. +C JOBL is not used if JOBB = 'G' and JOBL = 'Z' is assumed. +C Using SLICOT Library routine SB02MT to compute the +C corresponding A and Q in this case, before calling SB02OY, +C enables to obtain 2N-by-2N matrix pairs directly. +C +C JOBE CHARACTER*1 +C Specifies whether or not the matrix E is identity, as +C follows: +C = 'I': E is the identity matrix; +C = 'N': E is a general matrix. +C +C Input/Output Parameters +C +C N (input) INTEGER +C The order of the matrices A, Q, and E, and the number +C of rows of the matrices B and L. N >= 0. +C +C M (input) INTEGER +C If JOBB = 'B', M is the order of the matrix R, and the +C number of columns of the matrix B. M >= 0. +C M is not used if JOBB = 'G'. +C +C P (input) INTEGER +C If FACT = 'C' or 'D' or 'B', or if TYPE = 'S', P is the +C number of rows of the matrix C and/or D, respectively. +C P >= 0, and if JOBB = 'B' and TYPE = 'S', then P = M. +C Otherwise, P is not used. +C +C A (input) DOUBLE PRECISION array, dimension (LDA,N) +C The leading N-by-N part of this array must contain the +C state matrix A of the system. +C +C LDA INTEGER +C The leading dimension of array A. LDA >= MAX(1,N). +C +C B (input) DOUBLE PRECISION array, dimension (LDB,*) +C If JOBB = 'B', the leading N-by-M part of this array must +C contain the input matrix B of the system. +C If JOBB = 'G', the leading N-by-N upper triangular part +C (if UPLO = 'U') or lower triangular part (if UPLO = 'L') +C of this array must contain the upper triangular part or +C lower triangular part, respectively, of the matrix +C -1 +C G = BR B'. The stricly lower triangular part (if +C UPLO = 'U') or stricly upper triangular part (if +C UPLO = 'L') is not referenced. +C +C LDB INTEGER +C The leading dimension of array B. LDB >= MAX(1,N). +C +C Q (input) DOUBLE PRECISION array, dimension (LDQ,N) +C If FACT = 'N' or 'D', the leading N-by-N upper triangular +C part (if UPLO = 'U') or lower triangular part (if UPLO = +C 'L') of this array must contain the upper triangular part +C or lower triangular part, respectively, of the symmetric +C output weighting matrix Q. The stricly lower triangular +C part (if UPLO = 'U') or stricly upper triangular part (if +C UPLO = 'L') is not referenced. +C If FACT = 'C' or 'B', the leading P-by-N part of this +C array must contain the output matrix C of the system. +C +C LDQ INTEGER +C The leading dimension of array Q. +C LDQ >= MAX(1,N) if FACT = 'N' or 'D', +C LDQ >= MAX(1,P) if FACT = 'C' or 'B'. +C +C R (input) DOUBLE PRECISION array, dimension (LDR,M) +C If FACT = 'N' or 'C', the leading M-by-M upper triangular +C part (if UPLO = 'U') or lower triangular part (if UPLO = +C 'L') of this array must contain the upper triangular part +C or lower triangular part, respectively, of the symmetric +C input weighting matrix R. The stricly lower triangular +C part (if UPLO = 'U') or stricly upper triangular part (if +C UPLO = 'L') is not referenced. +C If FACT = 'D' or 'B', the leading P-by-M part of this +C array must contain the direct transmission matrix D of the +C system. +C If JOBB = 'G', this array is not referenced. +C +C LDR INTEGER +C The leading dimension of array R. +C LDR >= MAX(1,M) if JOBB = 'B' and FACT = 'N' or 'C'; +C LDR >= MAX(1,P) if JOBB = 'B' and FACT = 'D' or 'B'; +C LDR >= 1 if JOBB = 'G'. +C +C L (input) DOUBLE PRECISION array, dimension (LDL,M) +C If JOBL = 'N' (and JOBB = 'B'), the leading N-by-M part of +C this array must contain the cross weighting matrix L. +C If JOBL = 'Z' or JOBB = 'G', this array is not referenced. +C +C LDL INTEGER +C The leading dimension of array L. +C LDL >= MAX(1,N) if JOBL = 'N'; +C LDL >= 1 if JOBL = 'Z' or JOBB = 'G'. +C +C E (input) DOUBLE PRECISION array, dimension (LDE,N) +C If JOBE = 'N', the leading N-by-N part of this array must +C contain the matrix E of the descriptor system. +C If JOBE = 'I', E is taken as identity and this array is +C not referenced. +C +C LDE INTEGER +C The leading dimension of array E. +C LDE >= MAX(1,N) if JOBE = 'N'; +C LDE >= 1 if JOBE = 'I'. +C +C AF (output) DOUBLE PRECISION array, dimension (LDAF,*) +C The leading 2N-by-2N part of this array contains the +C matrix A in the matrix pencil. +C f +C Array AF must have 2*N+M columns if JOBB = 'B', and 2*N +C columns, otherwise. +C +C LDAF INTEGER +C The leading dimension of array AF. +C LDAF >= MAX(1,2*N+M) if JOBB = 'B', +C LDAF >= MAX(1,2*N) if JOBB = 'G'. +C +C BF (output) DOUBLE PRECISION array, dimension (LDBF,2*N) +C If DICO = 'D' or JOBB = 'B' or JOBE = 'N', the leading +C 2N-by-2N part of this array contains the matrix B in the +C f +C matrix pencil. +C The last M zero columns are never constructed. +C If DICO = 'C' and JOBB = 'G' and JOBE = 'I', this array +C is not referenced. +C +C LDBF INTEGER +C The leading dimension of array BF. +C LDBF >= MAX(1,2*N+M) if JOBB = 'B', +C LDBF >= MAX(1,2*N) if JOBB = 'G' and ( DICO = 'D' or +C JOBE = 'N' ), +C LDBF >= 1 if JOBB = 'G' and ( DICO = 'C' and +C JOBE = 'I' ). +C +C Tolerances +C +C TOL DOUBLE PRECISION +C The tolerance to be used to test for near singularity of +C the original matrix pencil, specifically of the triangular +C factor obtained during the reduction process. If the user +C sets TOL > 0, then the given value of TOL is used as a +C lower bound for the reciprocal condition number of that +C matrix; a matrix whose estimated condition number is less +C than 1/TOL is considered to be nonsingular. If the user +C sets TOL <= 0, then a default tolerance, defined by +C TOLDEF = EPS, is used instead, where EPS is the machine +C precision (see LAPACK Library routine DLAMCH). +C This parameter is not referenced if JOBB = 'G'. +C +C Workspace +C +C IWORK INTEGER array, dimension (LIWORK) +C LIWORK >= M if JOBB = 'B', +C LIWORK >= 1 if JOBB = 'G'. +C +C DWORK DOUBLE PRECISION array, dimension (LDWORK) +C On exit, if INFO = 0, DWORK(1) returns the optimal value +C of LDWORK. If JOBB = 'B', DWORK(2) returns the reciprocal +C of the condition number of the M-by-M lower triangular +C matrix obtained after compression. +C +C LDWORK INTEGER +C The length of the array DWORK. +C LDWORK >= 1 if JOBB = 'G', +C LDWORK >= MAX(1,2*N + M,3*M) if JOBB = 'B'. +C For optimum performance LDWORK should be larger. +C +C Error Indicator +C +C INFO INTEGER +C = 0: successful exit; +C < 0: if INFO = -i, the i-th argument had an illegal +C value; +C = 1: if the computed extended matrix pencil is singular, +C possibly due to rounding errors. +C +C METHOD +C +C The extended matrix pairs are constructed, taking various options +C into account. If JOBB = 'B', the problem order is reduced from +C 2N+M to 2N (see [1]). +C +C REFERENCES +C +C [1] Van Dooren, P. +C A Generalized Eigenvalue Approach for Solving Riccati +C Equations. +C SIAM J. Sci. Stat. Comp., 2, pp. 121-135, 1981. +C +C [2] Mehrmann, V. +C The Autonomous Linear Quadratic Control Problem. Theory and +C Numerical Solution. +C Lect. Notes in Control and Information Sciences, vol. 163, +C Springer-Verlag, Berlin, 1991. +C +C [3] Sima, V. +C Algorithms for Linear-Quadratic Optimization. +C Pure and Applied Mathematics: A Series of Monographs and +C Textbooks, vol. 200, Marcel Dekker, Inc., New York, 1996. +C +C NUMERICAL ASPECTS +C +C The algorithm is backward stable. +C +C CONTRIBUTORS +C +C Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Sep. 1997. +C Supersedes Release 2.0 routine SB02CY by T.G.J. Beelen, Philips, +C Eindhoven, Holland, M. Vanbegin, and P. Van Dooren, Philips +C Research Laboratory, Brussels, Belgium. +C +C REVISIONS +C +C V. Sima, Research Institute for Informatics, Bucharest, Dec. 2002. +C +C KEYWORDS +C +C Algebraic Riccati equation, closed loop system, continuous-time +C system, discrete-time system, optimal regulator, Schur form. +C +C ****************************************************************** +C + DOUBLE PRECISION ZERO, ONE + PARAMETER ( ZERO = 0.0D0, ONE = 1.0D0 ) +C .. Scalar Arguments .. + CHARACTER DICO, FACT, JOBB, JOBE, JOBL, TYPE, UPLO + INTEGER INFO, LDA, LDAF, LDB, LDBF, LDE, LDL, LDQ, LDR, + $ LDWORK, M, N, P + DOUBLE PRECISION TOL +C .. Array Arguments .. + INTEGER IWORK(*) + DOUBLE PRECISION A(LDA,*), AF(LDAF,*), B(LDB,*), BF(LDBF,*), + $ DWORK(*), E(LDE,*), L(LDL,*), Q(LDQ,*), R(LDR,*) +C .. Local Scalars .. + LOGICAL DISCR, LFACB, LFACN, LFACQ, LFACR, LJOBB, LJOBE, + $ LJOBL, LUPLO, OPTC + INTEGER I, ITAU, J, JWORK, N2, N2P1, NM, NNM, NP1, + $ WRKOPT + DOUBLE PRECISION RCOND, TOLDEF +C .. External Functions .. + LOGICAL LSAME + DOUBLE PRECISION DLAMCH + EXTERNAL DLAMCH, LSAME +C .. External Subroutines .. + EXTERNAL DCOPY, DGEQLF, DLACPY, DLASET, DORMQL, DSYRK, + $ DTRCON, XERBLA +C .. Intrinsic Functions .. + INTRINSIC INT, MAX +C .. Executable Statements .. +C + INFO = 0 + OPTC = LSAME( TYPE, 'O' ) + DISCR = LSAME( DICO, 'D' ) + LJOBB = LSAME( JOBB, 'B' ) + LFACN = LSAME( FACT, 'N' ) + LFACQ = LSAME( FACT, 'C' ) + LFACR = LSAME( FACT, 'D' ) + LFACB = LSAME( FACT, 'B' ) + LUPLO = LSAME( UPLO, 'U' ) + LJOBE = LSAME( JOBE, 'I' ) + N2 = N + N + IF ( LJOBB ) THEN + LJOBL = LSAME( JOBL, 'Z' ) + NM = N + M + NNM = N2 + M + ELSE + NM = N + NNM = N2 + END IF + NP1 = N + 1 + N2P1 = N2 + 1 +C +C Test the input scalar arguments. +C + IF( .NOT.OPTC .AND. .NOT.LSAME( TYPE, 'S' ) ) THEN + INFO = -1 + ELSE IF( .NOT.DISCR .AND. .NOT.LSAME( DICO, 'C' ) ) THEN + INFO = -2 + ELSE IF( .NOT.LJOBB .AND. .NOT.LSAME( JOBB, 'G' ) ) THEN + INFO = -3 + ELSE IF( .NOT.LFACQ .AND. .NOT.LFACR .AND. .NOT.LFACB + $ .AND. .NOT.LFACN ) THEN + INFO = -4 + ELSE IF( .NOT.LJOBB .OR. LFACN ) THEN + IF( .NOT.LUPLO .AND. .NOT.LSAME( UPLO, 'L' ) ) + $ INFO = -5 + ELSE IF( LJOBB ) THEN + IF( .NOT.LJOBL .AND. .NOT.LSAME( JOBL, 'N' ) ) + $ INFO = -6 + ELSE IF( .NOT.LJOBE .AND. .NOT.LSAME( JOBE, 'N' ) ) THEN + INFO = -7 + ELSE IF( N.LT.0 ) THEN + INFO = -8 + ELSE IF( LJOBB ) THEN + IF( M.LT.0 ) + $ INFO = -9 + ELSE IF( .NOT.LFACN .OR. .NOT.OPTC ) THEN + IF( P.LT.0 ) THEN + INFO = -10 + ELSE IF( LJOBB ) THEN + IF( .NOT.OPTC .AND. P.NE.M ) + $ INFO = -10 + END IF + ELSE IF( LDA.LT.MAX( 1, N ) ) THEN + INFO = -12 + ELSE IF( LDB.LT.MAX( 1, N ) ) THEN + INFO = -14 + ELSE IF( ( ( LFACN.OR.LFACR ) .AND. LDQ.LT.MAX( 1, N ) ) .OR. + $ ( ( LFACQ.OR.LFACB ) .AND. LDQ.LT.MAX( 1, P ) ) ) THEN + INFO = -16 + ELSE IF( LDR.LT.1 ) THEN + INFO = -18 + ELSE IF( LJOBB ) THEN + IF ( ( LFACN.OR.LFACQ ) .AND. LDR.LT.M .OR. + $ ( LFACR.OR.LFACB ) .AND. LDR.LT.P ) THEN + INFO = -18 + ELSE IF( ( .NOT.LJOBL .AND. LDL.LT.MAX( 1, N ) ) .OR. + $ ( LJOBL .AND. LDL.LT.1 ) ) THEN + INFO = -20 + END IF + END IF + IF( ( .NOT.LJOBE .AND. LDE.LT.MAX( 1, N ) ) .OR. + $ ( LJOBE .AND. LDE.LT.1 ) ) THEN + INFO = -22 + ELSE IF( LDAF.LT.MAX( 1, NNM ) ) THEN + INFO = -24 + ELSE IF( ( ( LJOBB .OR. DISCR .OR. .NOT.LJOBE ) .AND. + $ LDBF.LT.NNM ) .OR. ( LDBF.LT.1 ) ) THEN + INFO = -26 + ELSE IF( ( LJOBB .AND. LDWORK.LT.MAX( NNM, 3*M ) ) .OR. + $ LDWORK.LT.1 ) THEN + INFO = -30 + END IF +C + IF ( INFO.NE.0 ) THEN +C +C Error return. +C + CALL XERBLA( 'SB02OY', -INFO ) + RETURN + END IF +C +C Quick return if possible. +C + DWORK(1) = ONE + IF ( N.EQ.0 ) + $ RETURN +C +C Construct the extended matrices in AF and BF, by block-columns. +C + CALL DLACPY( 'Full', N, N, A, LDA, AF, LDAF ) +C + IF ( .NOT.LFACQ .AND. .NOT.LFACB ) THEN + CALL DLACPY( UPLO, N, N, Q, LDQ, AF(NP1,1), LDAF ) + IF ( LUPLO ) THEN +C +C Construct the lower triangle of Q. +C + DO 20 J = 1, N - 1 + CALL DCOPY( N-J, Q(J,J+1), LDQ, AF(NP1+J,J), 1 ) + 20 CONTINUE +C + ELSE +C +C Construct the upper triangle of Q. +C + DO 40 J = 2, N + CALL DCOPY( J-1, Q(J,1), LDQ, AF(NP1,J), 1 ) + 40 CONTINUE +C + END IF + ELSE + CALL DSYRK( 'Upper', 'Transpose', N, P, ONE, Q, LDQ, ZERO, + $ AF(NP1,1), LDAF ) +C + DO 60 J = 2, N + CALL DCOPY( J-1, AF(NP1,J), 1, AF(N+J,1), LDAF ) + 60 CONTINUE +C + END IF +C + IF ( LJOBB ) THEN + IF ( LJOBL ) THEN + CALL DLASET( 'Full', M, N, ZERO, ZERO, AF(N2P1,1), LDAF ) + ELSE +C + DO 80 I = 1, N + CALL DCOPY( M, L(I,1), LDL, AF(N2P1,I), 1 ) + 80 CONTINUE +C + END IF + END IF +C + IF ( DISCR.OR.LJOBB ) THEN + CALL DLASET( 'Full', N, N, ZERO, ZERO, AF(1,NP1), LDAF ) + ELSE + IF ( LUPLO ) THEN +C +C Construct (1,2) block of AF using the upper triangle of G. +C + DO 140 J = 1, N +C + DO 100 I = 1, J + AF(I,N+J)= -B(I,J) + 100 CONTINUE +C + DO 120 I = J + 1, N + AF(I,N+J)= -B(J,I) + 120 CONTINUE +C + 140 CONTINUE +C + ELSE +C +C Construct (1,2) block of AF using the lower triangle of G. +C + DO 200 J = 1, N +C + DO 160 I = 1, J - 1 + AF(I,N+J)= -B(J,I) + 160 CONTINUE +C + DO 180 I = J, N + AF(I,N+J)= -B(I,J) + 180 CONTINUE +C + 200 CONTINUE +C + END IF + END IF +C + IF ( DISCR ) THEN + IF ( LJOBE ) THEN + CALL DLASET( 'Full', NM, N, ZERO, -ONE, AF(NP1,NP1), LDAF ) + ELSE +C + DO 240 J = 1, N +C + DO 220 I = 1, N + AF(N+I,N+J)= -E(J,I) + 220 CONTINUE +C + 240 CONTINUE +C + IF ( LJOBB ) + $ CALL DLASET( 'Full', M, N, ZERO, ZERO, AF(N2P1,NP1), + $ LDAF ) + END IF + ELSE +C + DO 280 J = 1, N +C + DO 260 I = 1, N + AF(N+I,N+J)= A(J,I) + 260 CONTINUE +C + 280 CONTINUE +C + IF ( LJOBB ) THEN + IF ( OPTC ) THEN +C + DO 300 J = 1, N + CALL DCOPY ( M, B(J,1), LDB, AF(N2P1,N+J), 1 ) + 300 CONTINUE +C + ELSE + CALL DLACPY( 'Full', P, N, Q, LDQ, AF(N2P1,NP1), LDAF ) + END IF + END IF + END IF +C + IF ( LJOBB ) THEN +C + IF ( OPTC ) THEN + CALL DLACPY( 'Full', N, M, B, LDB, AF(1,N2P1), LDAF ) + ELSE +C + DO 320 I = 1, P + CALL DCOPY( N, Q(I,1), LDQ, AF(1,N2+I), 1 ) + 320 CONTINUE +C + END IF +C + IF ( LJOBL ) THEN + CALL DLASET( 'Full', N, M, ZERO, ZERO, AF(NP1,N2P1), LDAF ) + ELSE + CALL DLACPY( 'Full', N, M, L, LDL, AF(NP1,N2P1), LDAF ) + END IF +C + IF ( .NOT.LFACR .AND. .NOT.LFACB ) THEN + CALL DLACPY( UPLO, M, M, R, LDR, AF(N2P1,N2P1), LDAF ) + IF ( LUPLO ) THEN +C +C Construct the lower triangle of R. +C + DO 340 J = 1, M - 1 + CALL DCOPY( M-J, R(J,J+1), LDR, AF(N2P1+J,N2+J), 1 ) + 340 CONTINUE +C + ELSE +C +C Construct the upper triangle of R. +C + DO 360 J = 2, M + CALL DCOPY( J-1, R(J,1), LDR, AF(N2P1,N2+J), 1 ) + 360 CONTINUE +C + END IF + ELSE IF ( OPTC ) THEN + CALL DSYRK( 'Upper', 'Transpose', M, P, ONE, R, LDR, ZERO, + $ AF(N2P1,N2P1), LDAF ) +C + DO 380 J = 2, M + CALL DCOPY( J-1, AF(N2P1,N2+J), 1, AF(N2+J,N2P1), LDAF ) + 380 CONTINUE +C + ELSE +C + DO 420 J = 1, M +C + DO 400 I = 1, P + AF(N2+I,N2+J) = R(I,J) + R(J,I) + 400 CONTINUE +C + 420 CONTINUE +C + END IF + END IF +C + IF ( .NOT.LJOBB .AND. .NOT.DISCR .AND. LJOBE ) + $ RETURN +C +C Construct the first two block columns of BF. +C + IF ( LJOBE ) THEN + CALL DLASET( 'Full', N+NM, N, ZERO, ONE, BF, LDBF ) + ELSE + CALL DLACPY( 'Full', N, N, E, LDE, BF, LDBF ) + CALL DLASET( 'Full', NM, N, ZERO, ZERO, BF(NP1,1), LDBF ) + END IF +C + IF ( .NOT.DISCR.OR.LJOBB ) THEN + CALL DLASET( 'Full', N, N, ZERO, ZERO, BF(1,NP1), LDBF ) + ELSE + IF ( LUPLO ) THEN +C +C Construct (1,2) block of BF using the upper triangle of G. +C + DO 480 J = 1, N +C + DO 440 I = 1, J + BF(I,N+J)= B(I,J) + 440 CONTINUE +C + DO 460 I = J + 1, N + BF(I,N+J)= B(J,I) + 460 CONTINUE +C + 480 CONTINUE +C + ELSE +C +C Construct (1,2) block of BF using the lower triangle of G. +C + DO 540 J = 1, N +C + DO 500 I = 1, J - 1 + BF(I,N+J)= B(J,I) + 500 CONTINUE +C + DO 520 I = J, N + BF(I,N+J)= B(I,J) + 520 CONTINUE +C + 540 CONTINUE +C + END IF + END IF +C + IF ( DISCR ) THEN +C + DO 580 J = 1, N +C + DO 560 I = 1, N + BF(N+I,N+J)= -A(J,I) + 560 CONTINUE +C + 580 CONTINUE +C + IF ( LJOBB ) THEN +C + IF ( OPTC ) THEN +C + DO 620 J = 1, N +C + DO 600 I = 1, M + BF(N2+I,N+J)= -B(J,I) + 600 CONTINUE +C + 620 CONTINUE +C + ELSE +C + DO 660 J = 1, N +C + DO 640 I = 1, P + BF(N2+I,N+J) = -Q(I,J) + 640 CONTINUE +C + 660 CONTINUE +C + END IF + END IF +C + ELSE + IF ( LJOBE ) THEN + CALL DLASET( 'Full', NM, N, ZERO, -ONE, BF(NP1,NP1), LDBF ) + ELSE +C + DO 700 J = 1, N +C + DO 680 I = 1, N + BF(N+I,N+J)= -E(J,I) + 680 CONTINUE +C + 700 CONTINUE +C + IF ( LJOBB ) + $ CALL DLASET( 'Full', M, N, ZERO, ZERO, BF(N2P1,NP1), + $ LDBF ) + END IF + END IF +C + IF ( .NOT.LJOBB ) + $ RETURN +C +C Compress the pencil lambda x BF - AF, using QL factorization. +C (Note: Comments in the code beginning "Workspace:" describe the +C minimal amount of real workspace needed at that point in the +C code, as well as the preferred amount for good performance. +C NB refers to the optimal block size for the immediately +C following subroutine, as returned by ILAENV.) +C +C Workspace: need 2*M; prefer M + M*NB. +C + ITAU = 1 + JWORK = ITAU + M + CALL DGEQLF( NNM, M, AF(1,N2P1), LDAF, DWORK(ITAU), DWORK(JWORK), + $ LDWORK-JWORK+1, INFO ) + WRKOPT = DWORK(JWORK) +C +C Workspace: need 2*N+M; prefer M + 2*N*NB. +C + CALL DORMQL( 'Left', 'Transpose', NNM, N2, M, AF(1,N2P1), LDAF, + $ DWORK(ITAU), AF, LDAF, DWORK(JWORK), LDWORK-JWORK+1, + $ INFO ) + WRKOPT = MAX( WRKOPT, INT( DWORK(JWORK) )+JWORK-1 ) +C + CALL DORMQL( 'Left', 'Transpose', NNM, N2, M, AF(1,N2P1), LDAF, + $ DWORK(ITAU), BF, LDBF, DWORK(JWORK), LDWORK-JWORK+1, + $ INFO ) +C +C Check the singularity of the L factor in the QL factorization: +C if singular, then the extended matrix pencil is also singular. +C Workspace 3*M. +C + TOLDEF = TOL + IF ( TOLDEF.LE.ZERO ) + $ TOLDEF = DLAMCH( 'Epsilon' ) +C + CALL DTRCON( '1-norm', 'Lower', 'Non unit', M, AF(N2P1,N2P1), + $ LDAF, RCOND, DWORK, IWORK, INFO ) + WRKOPT = MAX( WRKOPT, 3*M ) +C + IF ( RCOND.LE.TOLDEF ) + $ INFO = 1 +C + DWORK(1) = WRKOPT + DWORK(2) = RCOND +C + RETURN +C *** Last line of SB02OY *** + END
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ncfsyn/SB10ID.dat Sun Jul 31 21:57:09 2011 +0000 @@ -0,0 +1,21 @@ + SB10ID EXAMPLE PROGRAM DATA + 6 2 3 + -1.0 0.0 4.0 5.0 -3.0 -2.0 + -2.0 4.0 -7.0 -2.0 0.0 3.0 + -6.0 9.0 -5.0 0.0 2.0 -1.0 + -8.0 4.0 7.0 -1.0 -3.0 0.0 + 2.0 5.0 8.0 -9.0 1.0 -4.0 + 3.0 -5.0 8.0 0.0 2.0 -6.0 + -3.0 -4.0 + 2.0 0.0 + -5.0 -7.0 + 4.0 -6.0 + -3.0 9.0 + 1.0 -2.0 + 1.0 -1.0 2.0 -4.0 0.0 -3.0 + -3.0 0.0 5.0 -1.0 1.0 1.0 + -7.0 5.0 0.0 -8.0 2.0 -2.0 + 1.0 -2.0 + 0.0 4.0 + 5.0 -3.0 + 1.0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ncfsyn/SB10ID.res Sun Jul 31 21:57:09 2011 +0000 @@ -0,0 +1,32 @@ + SB10ID EXAMPLE PROGRAM RESULTS + + + The controller state matrix AK is + + -39.0671 9.9293 22.2322 -27.4113 43.8655 + -6.6117 3.0006 11.0878 -11.4130 15.4269 + 33.6805 -6.6934 -23.9953 14.1438 -33.4358 + -32.3191 9.7316 25.4033 -24.0473 42.0517 + -44.1655 18.7767 34.8873 -42.4369 50.8437 + + The controller input matrix BK is + + -10.2905 -16.5382 -10.9782 + -4.3598 -8.7525 -5.1447 + 6.5962 1.8975 6.2316 + -9.8770 -14.7041 -11.8778 + -9.6726 -22.7309 -18.2692 + + The controller output matrix CK is + + -0.6647 -0.0599 -1.0376 0.5619 1.7297 + -8.4202 3.9573 7.3094 -7.6283 10.6768 + + The controller matrix DK is + + 0.8466 0.4979 -0.6993 + -1.2226 -4.8689 -4.5056 + + The estimated condition numbers are + + 0.13861D-01 0.90541D-02
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ncfsyn/SB10KD.dat Sun Jul 31 21:57:09 2011 +0000 @@ -0,0 +1,17 @@ + SB10KD EXAMPLE PROGRAM DATA + 6 2 2 + 0.2 0.0 0.3 0.0 -0.3 -0.1 + -0.3 0.2 -0.4 -0.3 0.0 0.0 + -0.1 0.1 -0.1 0.0 0.0 -0.3 + 0.1 0.0 0.0 -0.1 -0.1 0.0 + 0.0 0.3 0.6 0.2 0.1 -0.4 + 0.2 -0.4 0.0 0.0 0.2 -0.2 + -1.0 -2.0 + 1.0 3.0 + -3.0 -4.0 + 1.0 -2.0 + 0.0 1.0 + 1.0 5.0 + 1.0 -1.0 2.0 -2.0 0.0 -3.0 + -3.0 0.0 1.0 -1.0 1.0 -1.0 + 1.1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ncfsyn/SB10KD.f Sun Jul 31 21:57:09 2011 +0000 @@ -0,0 +1,650 @@ + SUBROUTINE SB10KD( N, M, NP, A, LDA, B, LDB, C, LDC, FACTOR, + $ AK, LDAK, BK, LDBK, CK, LDCK, DK, LDDK, RCOND, + $ IWORK, DWORK, LDWORK, BWORK, INFO ) +C +C SLICOT RELEASE 5.0. +C +C Copyright (c) 2002-2010 NICONET e.V. +C +C This program is free software: you can redistribute it and/or +C modify it under the terms of the GNU General Public License as +C published by the Free Software Foundation, either version 2 of +C the License, or (at your option) any later version. +C +C This program is distributed in the hope that it will be useful, +C but WITHOUT ANY WARRANTY; without even the implied warranty of +C MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +C GNU General Public License for more details. +C +C You should have received a copy of the GNU General Public License +C along with this program. If not, see +C <http://www.gnu.org/licenses/>. +C +C PURPOSE +C +C To compute the matrices of the positive feedback controller +C +C | Ak | Bk | +C K = |----|----| +C | Ck | Dk | +C +C for the shaped plant +C +C | A | B | +C G = |---|---| +C | C | 0 | +C +C in the Discrete-Time Loop Shaping Design Procedure. +C +C ARGUMENTS +C +C Input/Output Parameters +C +C N (input) INTEGER +C The order of the plant. N >= 0. +C +C M (input) INTEGER +C The column size of the matrix B. M >= 0. +C +C NP (input) INTEGER +C The row size of the matrix C. NP >= 0. +C +C A (input) DOUBLE PRECISION array, dimension (LDA,N) +C The leading N-by-N part of this array must contain the +C system state matrix A of the shaped plant. +C +C LDA INTEGER +C The leading dimension of the array A. LDA >= max(1,N). +C +C B (input) DOUBLE PRECISION array, dimension (LDB,M) +C The leading N-by-M part of this array must contain the +C system input matrix B of the shaped plant. +C +C LDB INTEGER +C The leading dimension of the array B. LDB >= max(1,N). +C +C C (input) DOUBLE PRECISION array, dimension (LDC,N) +C The leading NP-by-N part of this array must contain the +C system output matrix C of the shaped plant. +C +C LDC INTEGER +C The leading dimension of the array C. LDC >= max(1,NP). +C +C FACTOR (input) DOUBLE PRECISION +C = 1 implies that an optimal controller is required; +C > 1 implies that a suboptimal controller is required +C achieving a performance FACTOR less than optimal. +C FACTOR >= 1. +C +C AK (output) DOUBLE PRECISION array, dimension (LDAK,N) +C The leading N-by-N part of this array contains the +C controller state matrix Ak. +C +C LDAK INTEGER +C The leading dimension of the array AK. LDAK >= max(1,N). +C +C BK (output) DOUBLE PRECISION array, dimension (LDBK,NP) +C The leading N-by-NP part of this array contains the +C controller input matrix Bk. +C +C LDBK INTEGER +C The leading dimension of the array BK. LDBK >= max(1,N). +C +C CK (output) DOUBLE PRECISION array, dimension (LDCK,N) +C The leading M-by-N part of this array contains the +C controller output matrix Ck. +C +C LDCK INTEGER +C The leading dimension of the array CK. LDCK >= max(1,M). +C +C DK (output) DOUBLE PRECISION array, dimension (LDDK,NP) +C The leading M-by-NP part of this array contains the +C controller matrix Dk. +C +C LDDK INTEGER +C The leading dimension of the array DK. LDDK >= max(1,M). +C +C RCOND (output) DOUBLE PRECISION array, dimension (4) +C RCOND(1) contains an estimate of the reciprocal condition +C number of the linear system of equations from +C which the solution of the P-Riccati equation is +C obtained; +C RCOND(2) contains an estimate of the reciprocal condition +C number of the linear system of equations from +C which the solution of the Q-Riccati equation is +C obtained; +C RCOND(3) contains an estimate of the reciprocal condition +C number of the linear system of equations from +C which the solution of the X-Riccati equation is +C obtained; +C RCOND(4) contains an estimate of the reciprocal condition +C number of the matrix Rx + Bx'*X*Bx (see the +C comments in the code). +C +C Workspace +C +C IWORK INTEGER array, dimension 2*max(N,NP+M) +C +C DWORK DOUBLE PRECISION array, dimension (LDWORK) +C On exit, if INFO = 0, DWORK(1) contains the optimal value +C of LDWORK. +C +C LDWORK INTEGER +C The dimension of the array DWORK. +C LDWORK >= 15*N*N + 6*N + +C max( 14*N+23, 16*N, 2*N+NP+M, 3*(NP+M) ) + +C max( N*N, 11*N*NP + 2*M*M + 8*NP*NP + 8*M*N + +C 4*M*NP + NP ). +C For good performance, LDWORK must generally be larger. +C +C BWORK LOGICAL array, dimension (2*N) +C +C Error Indicator +C +C INFO INTEGER +C = 0: successful exit; +C < 0: if INFO = -i, the i-th argument had an illegal +C value; +C = 1: the P-Riccati equation is not solved successfully; +C = 2: the Q-Riccati equation is not solved successfully; +C = 3: the X-Riccati equation is not solved successfully; +C = 4: the iteration to compute eigenvalues failed to +C converge; +C = 5: the matrix Rx + Bx'*X*Bx is singular; +C = 6: the closed-loop system is unstable. +C +C METHOD +C +C The routine implements the method presented in [1]. +C +C REFERENCES +C +C [1] McFarlane, D. and Glover, K. +C A loop shaping design procedure using H_infinity synthesis. +C IEEE Trans. Automat. Control, vol. AC-37, no. 6, pp. 759-769, +C 1992. +C +C NUMERICAL ASPECTS +C +C The accuracy of the results depends on the conditioning of the +C two Riccati equations solved in the controller design. For +C better conditioning it is advised to take FACTOR > 1. +C +C CONTRIBUTORS +C +C P.Hr. Petkov, D.W. Gu and M.M. Konstantinov, October 2000. +C +C REVISIONS +C +C V. Sima, Katholieke University Leuven, January 2001, +C February 2001. +C +C KEYWORDS +C +C H_infinity control, Loop-shaping design, Robust control. +C +C ****************************************************************** +C +C .. Parameters .. + DOUBLE PRECISION ZERO, ONE + PARAMETER ( ZERO = 0.0D+0, ONE = 1.0D+0 ) +C .. +C .. Scalar Arguments .. + INTEGER INFO, LDA, LDAK, LDB, LDBK, LDC, LDCK, LDDK, + $ LDWORK, M, N, NP + DOUBLE PRECISION FACTOR +C .. +C .. Array Arguments .. + INTEGER IWORK( * ) + LOGICAL BWORK( * ) + DOUBLE PRECISION A( LDA, * ), AK( LDAK, * ), B( LDB, * ), + $ BK( LDBK, * ), C( LDC, * ), CK( LDCK, * ), + $ DK( LDDK, * ), DWORK( * ), RCOND( 4 ) +C .. +C .. Local Scalars .. + INTEGER I, I1, I2, I3, I4, I5, I6, I7, I8, I9, I10, + $ I11, I12, I13, I14, I15, I16, I17, I18, I19, + $ I20, I21, I22, I23, I24, I25, I26, INFO2, + $ IWRK, J, LWA, LWAMAX, MINWRK, N2, NS, SDIM + DOUBLE PRECISION GAMMA, RNORM +C .. +C .. External Functions .. + LOGICAL SELECT + DOUBLE PRECISION DLANSY, DLAPY2 + EXTERNAL DLANSY, DLAPY2, SELECT +C .. +C .. External Subroutines .. + EXTERNAL DGEMM, DGEES, DLACPY, DLASET, DPOTRF, DPOTRS, + $ DSYCON, DSYEV, DSYRK, DSYTRF, DSYTRS, SB02OD, + $ XERBLA +C .. +C .. Intrinsic Functions .. + INTRINSIC DBLE, INT, MAX, SQRT +C .. +C .. Executable Statements .. +C +C Decode and Test input parameters. +C + INFO = 0 + IF( N.LT.0 ) THEN + INFO = -1 + ELSE IF( M.LT.0 ) THEN + INFO = -2 + ELSE IF( NP.LT.0 ) THEN + INFO = -3 + ELSE IF( LDA.LT.MAX( 1, N ) ) THEN + INFO = -5 + ELSE IF( LDB.LT.MAX( 1, N ) ) THEN + INFO = -7 + ELSE IF( LDC.LT.MAX( 1, NP ) ) THEN + INFO = -9 + ELSE IF( FACTOR.LT.ONE ) THEN + INFO = -10 + ELSE IF( LDAK.LT.MAX( 1, N ) ) THEN + INFO = -12 + ELSE IF( LDBK.LT.MAX( 1, N ) ) THEN + INFO = -14 + ELSE IF( LDCK.LT.MAX( 1, M ) ) THEN + INFO = -16 + ELSE IF( LDDK.LT.MAX( 1, M ) ) THEN + INFO = -18 + END IF +C +C Compute workspace. +C + MINWRK = 15*N*N + 6*N + MAX( 14*N+23, 16*N, 2*N+NP+M, 3*(NP+M) ) + + $ MAX( N*N, 11*N*NP + 2*M*M + 8*NP*NP + 8*M*N + + $ 4*M*NP + NP ) + IF( LDWORK.LT.MINWRK ) THEN + INFO = -22 + END IF + IF( INFO.NE.0 ) THEN + CALL XERBLA( 'SB10KD', -INFO ) + RETURN + END IF +C +C Quick return if possible. +C + IF( N.EQ.0 .OR. M.EQ.0 .OR. NP.EQ.0 ) THEN + RCOND( 1 ) = ONE + RCOND( 2 ) = ONE + RCOND( 3 ) = ONE + RCOND( 4 ) = ONE + DWORK( 1 ) = ONE + RETURN + END IF +C +C Workspace usage. +C + N2 = 2*N + I1 = N*N + I2 = I1 + N*N + I3 = I2 + N*N + I4 = I3 + N*N + I5 = I4 + N2 + I6 = I5 + N2 + I7 = I6 + N2 + I8 = I7 + N2*N2 + I9 = I8 + N2*N2 +C + IWRK = I9 + N2*N2 + LWAMAX = 0 +C +C Compute Cr = C'*C . +C + CALL DSYRK( 'U', 'T', N, NP, ONE, C, LDC, ZERO, DWORK( I2+1 ), N ) +C +C Compute Dr = B*B' . +C + CALL DSYRK( 'U', 'N', N, M, ONE, B, LDB, ZERO, DWORK( I3+1 ), N ) +C -1 +C Solution of the Riccati equation A'*P*(In + Dr*P) *A - P + Cr = 0. +C + CALL SB02OD( 'D', 'G', 'N', 'U', 'Z', 'S', N, M, NP, A, LDA, + $ DWORK( I3+1 ), N, DWORK( I2+1 ), N, DWORK, M, DWORK, + $ N, RCOND( 1 ), DWORK, N, DWORK( I4+1 ), + $ DWORK( I5+1 ), DWORK( I6+1 ), DWORK( I7+1 ), N2, + $ DWORK( I8+1 ), N2, DWORK( I9+1 ), N2, -ONE, IWORK, + $ DWORK( IWRK+1 ), LDWORK-IWRK, BWORK, INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 1 + RETURN + END IF + LWA = INT( DWORK( IWRK+1 ) ) + IWRK + LWAMAX = MAX( LWA, LWAMAX ) +C +C Transpose A in AK (used as workspace). +C + DO 40 J = 1, N + DO 30 I = 1, N + AK( I,J ) = A( J,I ) + 30 CONTINUE + 40 CONTINUE +C -1 +C Solution of the Riccati equation A*Q*(In + Cr*Q) *A' - Q + Dr = 0. +C + CALL SB02OD( 'D', 'G', 'N', 'U', 'Z', 'S', N, M, NP, AK, LDAK, + $ DWORK( I2+1 ), N, DWORK( I3+1 ), N, DWORK, M, DWORK, + $ N, RCOND( 2 ), DWORK( I1+1 ), N, DWORK( I4+1 ), + $ DWORK( I5+1 ), DWORK( I6+1 ), DWORK( I7+1 ), N2, + $ DWORK( I8+1 ), N2, DWORK( I9+1 ), N2, -ONE, IWORK, + $ DWORK( IWRK+1 ), LDWORK-IWRK, BWORK, INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 2 + RETURN + END IF + LWA = INT( DWORK( IWRK+1 ) ) + IWRK + LWAMAX = MAX( LWA, LWAMAX ) +C +C Compute gamma. +C + CALL DGEMM( 'N', 'N', N, N, N, ONE, DWORK( I1+1 ), N, DWORK, N, + $ ZERO, AK, LDAK ) + CALL DGEES( 'N', 'N', SELECT, N, AK, LDAK, SDIM, DWORK( I6+1 ), + $ DWORK( I7+1 ), DWORK( IWRK+1 ), N, DWORK( IWRK+1 ), + $ LDWORK-IWRK, BWORK, INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 4 + RETURN + END IF + LWA = INT( DWORK( IWRK+1 ) ) + IWRK + LWAMAX = MAX( LWA, LWAMAX ) + GAMMA = ZERO + DO 50 I = 1, N + GAMMA = MAX( GAMMA, DWORK( I6+I ) ) + 50 CONTINUE + GAMMA = FACTOR*SQRT( ONE + GAMMA ) +C +C Workspace usage. +C + I3 = I2 + N*NP + I4 = I3 + NP*NP + I5 = I4 + NP*NP + I6 = I5 + NP*NP + I7 = I6 + NP + I8 = I7 + NP*NP + I9 = I8 + NP*NP + I10 = I9 + NP*NP + I11 = I10 + N*NP + I12 = I11 + N*NP + I13 = I12 + ( NP+M )*( NP+M ) + I14 = I13 + N*( NP+M ) + I15 = I14 + N*( NP+M ) + I16 = I15 + N*N + I17 = I16 + N2 + I18 = I17 + N2 + I19 = I18 + N2 + I20 = I19 + ( N2+NP+M )*( N2+NP+M ) + I21 = I20 + ( N2+NP+M )*N2 +C + IWRK = I21 + N2*N2 +C +C Compute Q*C' . +C + CALL DGEMM( 'N', 'T', N, NP, N, ONE, DWORK( I1+1 ), N, C, LDC, + $ ZERO, DWORK( I2+1 ), N ) +C +C Compute Ip + C*Q*C' . +C + CALL DLASET( 'Full', NP, NP, ZERO, ONE, DWORK( I3+1 ), NP ) + CALL DGEMM( 'N', 'N', NP, NP, N, ONE, C, LDC, DWORK( I2+1 ), N, + $ ONE, DWORK( I3+1 ), NP ) +C +C Compute the eigenvalues and eigenvectors of Ip + C'*Q*C +C + CALL DLACPY( 'U', NP, NP, DWORK( I3+1 ), NP, DWORK( I5+1 ), NP ) + CALL DSYEV( 'V', 'U', NP, DWORK( I5+1 ), NP, DWORK( I6+1 ), + $ DWORK( IWRK+1 ), LDWORK-IWRK, INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 4 + RETURN + END IF + LWA = INT( DWORK( IWRK+1 ) ) + IWRK + LWAMAX = MAX( LWA, LWAMAX ) +C -1 +C Compute ( Ip + C'*Q*C ) . +C + DO 70 J = 1, NP + DO 60 I = 1, NP + DWORK( I9+I+(J-1)*NP ) = DWORK( I5+J+(I-1)*NP ) / + $ DWORK( I6+I ) + 60 CONTINUE + 70 CONTINUE + CALL DGEMM( 'N', 'N', NP, NP, NP, ONE, DWORK( I5+1 ), NP, + $ DWORK( I9+1 ), NP, ZERO, DWORK( I4+1 ), NP ) +C +C Compute Z2 . +C + DO 90 J = 1, NP + DO 80 I = 1, NP + DWORK( I9+I+(J-1)*NP ) = DWORK( I5+J+(I-1)*NP ) / + $ SQRT( DWORK( I6+I ) ) + 80 CONTINUE + 90 CONTINUE + CALL DGEMM( 'N', 'N', NP, NP, NP, ONE, DWORK( I5+1 ), NP, + $ DWORK( I9+1 ), NP, ZERO, DWORK( I7+1 ), NP ) +C -1 +C Compute Z2 . +C + DO 110 J = 1, NP + DO 100 I = 1, NP + DWORK( I9+I+(J-1)*NP ) = DWORK( I5+J+(I-1)*NP )* + $ SQRT( DWORK( I6+I ) ) + 100 CONTINUE + 110 CONTINUE + CALL DGEMM( 'N', 'N', NP, NP, NP, ONE, DWORK( I5+1 ), NP, + $ DWORK( I9+1 ), NP, ZERO, DWORK( I8+1 ), NP ) +C +C Compute A*Q*C' . +C + CALL DGEMM( 'N', 'N', N, NP, N, ONE, A, LDA, DWORK( I2+1 ), N, + $ ZERO, DWORK( I10+1 ), N ) +C -1 +C Compute H = -A*Q*C'*( Ip + C*Q*C' ) . +C + CALL DGEMM( 'N', 'N', N, NP, NP, -ONE, DWORK( I10+1 ), N, + $ DWORK( I4+1 ), NP, ZERO, DWORK( I11+1 ), N ) +C +C Compute Rx . +C + CALL DLASET( 'F', NP+M, NP+M, ZERO, ONE, DWORK( I12+1 ), NP+M ) + DO 130 J = 1, NP + DO 120 I = 1, NP + DWORK( I12+I+(J-1)*(NP+M) ) = DWORK( I3+I+(J-1)*NP ) + 120 CONTINUE + DWORK( I12+J+(J-1)*(NP+M) ) = DWORK( I3+J+(J-1)*NP ) - + $ GAMMA*GAMMA + 130 CONTINUE +C +C Compute Bx . +C + CALL DGEMM( 'N', 'N', N, NP, NP, -ONE, DWORK( I11+1 ), N, + $ DWORK( I8+1 ), NP, ZERO, DWORK( I13+1 ), N ) + DO 150 J = 1, M + DO 140 I = 1, N + DWORK( I13+N*NP+I+(J-1)*N ) = B( I, J ) + 140 CONTINUE + 150 CONTINUE +C +C Compute Sx . +C + CALL DGEMM( 'T', 'N', N, NP, NP, ONE, C, LDC, DWORK( I8+1 ), NP, + $ ZERO, DWORK( I14+1 ), N ) + CALL DLASET( 'F', N, M, ZERO, ZERO, DWORK( I14+N*NP+1 ), N ) +C +C Solve the Riccati equation +C -1 +C X = A'*X*A + Cx - (Sx + A'*X*Bx)*(Rx + Bx'*X*B ) *(Sx'+Bx'*X*A). +C + CALL SB02OD( 'D', 'B', 'C', 'U', 'N', 'S', N, NP+M, NP, A, LDA, + $ DWORK( I13+1 ), N, C, LDC, DWORK( I12+1 ), NP+M, + $ DWORK( I14+1 ), N, RCOND( 3 ), DWORK( I15+1 ), N, + $ DWORK( I16+1 ), DWORK( I17+1 ), DWORK( I18+1 ), + $ DWORK( I19+1 ), N2+NP+M, DWORK( I20+1 ), N2+NP+M, + $ DWORK( I21+1 ), N2, -ONE, IWORK, DWORK( IWRK+1 ), + $ LDWORK-IWRK, BWORK, INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 3 + RETURN + END IF + LWA = INT( DWORK( IWRK+1 ) ) + IWRK + LWAMAX = MAX( LWA, LWAMAX ) +C + I22 = I16 + I23 = I22 + ( NP+M )*N + I24 = I23 + ( NP+M )*( NP+M ) + I25 = I24 + ( NP+M )*N + I26 = I25 + M*N +C + IWRK = I25 +C +C Compute Bx'*X . +C + CALL DGEMM( 'T', 'N', NP+M, N, N, ONE, DWORK( I13+1 ), N, + $ DWORK( I15+1 ), N, ZERO, DWORK( I22+1 ), NP+M ) +C +C Compute Rx + Bx'*X*Bx . +C + CALL DLACPY( 'F', NP+M, NP+M, DWORK( I12+1 ), NP+M, + $ DWORK( I23+1 ), NP+M ) + CALL DGEMM( 'N', 'N', NP+M, NP+M, N, ONE, DWORK( I22+1 ), NP+M, + $ DWORK( I13+1 ), N, ONE, DWORK( I23+1 ), NP+M ) +C +C Compute -( Sx' + Bx'*X*A ) . +C + DO 170 J = 1, N + DO 160 I = 1, NP+M + DWORK( I24+I+(J-1)*(NP+M) ) = DWORK( I14+J+(I-1)*N ) + 160 CONTINUE + 170 CONTINUE + CALL DGEMM( 'N', 'N', NP+M, N, N, -ONE, DWORK( I22+1 ), NP+M, + $ A, LDA, -ONE, DWORK( I24+1 ), NP+M ) +C +C Factorize Rx + Bx'*X*Bx . +C + RNORM = DLANSY( '1', 'U', NP+M, DWORK( I23+1 ), NP+M, + $ DWORK( IWRK+1 ) ) + CALL DSYTRF( 'U', NP+M, DWORK( I23+1 ), NP+M, IWORK, + $ DWORK( IWRK+1 ), LDWORK-IWRK, INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 5 + RETURN + END IF + LWA = INT( DWORK( IWRK+1 ) ) + IWRK + LWAMAX = MAX( LWA, LWAMAX ) + CALL DSYCON( 'U', NP+M, DWORK( I23+1 ), NP+M, IWORK, RNORM, + $ RCOND( 4 ), DWORK( IWRK+1 ), IWORK( NP+M+1), INFO2 ) +C -1 +C Compute F = -( Rx + Bx'*X*Bx ) ( Sx' + Bx'*X*A ) . +C + CALL DSYTRS( 'U', NP+M, N, DWORK( I23+1 ), NP+M, IWORK, + $ DWORK( I24+1 ), NP+M, INFO2 ) +C +C Compute B'*X . +C + CALL DGEMM( 'T', 'N', M, N, N, ONE, B, LDB, DWORK( I15+1 ), N, + $ ZERO, DWORK( I25+1 ), M ) +C +C Compute Im + B'*X*B . +C + CALL DLASET( 'F', M, M, ZERO, ONE, DWORK( I23+1 ), M ) + CALL DGEMM( 'N', 'N', M, M, N, ONE, DWORK( I25+1 ), M, B, LDB, + $ ONE, DWORK( I23+1 ), M ) +C +C Factorize Im + B'*X*B . +C + CALL DPOTRF( 'U', M, DWORK( I23+1 ), M, INFO2 ) +C -1 +C Compute ( Im + B'*X*B ) B'*X . +C + CALL DPOTRS( 'U', M, N, DWORK( I23+1 ), M, DWORK( I25+1 ), M, + $ INFO2 ) +C -1 +C Compute Dk = ( Im + B'*X*B ) B'*X*H . +C + CALL DGEMM( 'N', 'N', M, NP, N, ONE, DWORK( I25+1 ), M, + $ DWORK( I11+1 ), N, ZERO, DK, LDDK ) +C +C Compute Bk = -H + B*Dk . +C + CALL DLACPY( 'F', N, NP, DWORK( I11+1 ), N, BK, LDBK ) + CALL DGEMM( 'N', 'N', N, NP, M, ONE, B, LDB, DK, LDDK, -ONE, + $ BK, LDBK ) +C -1 +C Compute Dk*Z2 . +C + CALL DGEMM( 'N', 'N', M, NP, NP, ONE, DK, LDDK, DWORK( I8+1 ), + $ NP, ZERO, DWORK( I26+1 ), M ) +C +C Compute F1 + Z2*C . +C + CALL DLACPY( 'F', NP, N, DWORK( I24+1 ), NP+M, DWORK( I12+1 ), + $ NP ) + CALL DGEMM( 'N', 'N', NP, N, NP, ONE, DWORK( I7+1 ), NP, C, LDC, + $ ONE, DWORK( I12+1 ), NP ) +C -1 +C Compute Ck = F2 - Dk*Z2 *( F1 + Z2*C ) . +C + CALL DLACPY( 'F', M, N, DWORK( I24+NP+1 ), NP+M, CK, LDCK ) + CALL DGEMM( 'N', 'N', M, N, NP, -ONE, DWORK( I26+1 ), M, + $ DWORK( I12+1 ), NP, ONE, CK, LDCK ) +C +C Compute Ak = A + H*C + B*Ck . +C + CALL DLACPY( 'F', N, N, A, LDA, AK, LDAK ) + CALL DGEMM( 'N', 'N', N, N, NP, ONE, DWORK( I11+1 ), N, C, LDC, + $ ONE, AK, LDAK ) + CALL DGEMM( 'N', 'N', N, N, M, ONE, B, LDB, CK, LDCK, ONE, AK, + $ LDAK ) +C +C Workspace usage. +C + I1 = M*N + I2 = I1 + N2*N2 + I3 = I2 + N2 +C + IWRK = I3 + N2 +C +C Compute Dk*C . +C + CALL DGEMM( 'N', 'N', M, N, NP, ONE, DK, LDDK, C, LDC, ZERO, + $ DWORK, M ) +C +C Compute the closed-loop state matrix. +C + CALL DLACPY( 'F', N, N, A, LDA, DWORK( I1+1 ), N2 ) + CALL DGEMM( 'N', 'N', N, N, M, -ONE, B, LDB, DWORK, M, ONE, + $ DWORK( I1+1 ), N2 ) + CALL DGEMM( 'N', 'N', N, N, NP, -ONE, BK, LDBK, C, LDC, ZERO, + $ DWORK( I1+N+1 ), N2 ) + CALL DGEMM( 'N', 'N', N, N, M, ONE, B, LDB, CK, LDCK, ZERO, + $ DWORK( I1+N2*N+1 ), N2 ) + CALL DLACPY( 'F', N, N, AK, LDAK, DWORK( I1+N2*N+N+1 ), N2 ) +C +C Compute the closed-loop poles. +C + CALL DGEES( 'N', 'N', SELECT, N2, DWORK( I1+1 ), N2, SDIM, + $ DWORK( I2+1 ), DWORK( I3+1 ), DWORK( IWRK+1 ), N, + $ DWORK( IWRK+1 ), LDWORK-IWRK, BWORK, INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 4 + RETURN + END IF + LWA = INT( DWORK( IWRK+1 ) ) + IWRK + LWAMAX = MAX( LWA, LWAMAX ) +C +C Check the stability of the closed-loop system. +C + NS = 0 + DO 180 I = 1, N2 + IF( DLAPY2( DWORK( I2+I ), DWORK( I3+I ) ).GT.ONE ) NS = NS + 1 + 180 CONTINUE + IF( NS.GT.0 ) THEN + INFO = 6 + RETURN + END IF +C + DWORK( 1 ) = DBLE( LWAMAX ) + RETURN +C *** Last line of SB10KD *** + END
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ncfsyn/SB10KD.res Sun Jul 31 21:57:09 2011 +0000 @@ -0,0 +1,34 @@ + SB10KD EXAMPLE PROGRAM RESULTS + + + The controller state matrix AK is + + 0.0337 0.0222 0.0858 0.1264 -0.1872 0.1547 + 0.4457 0.0668 -0.2255 -0.3204 -0.4548 -0.0691 + -0.2419 -0.2506 -0.0982 -0.1321 -0.0130 -0.0838 + -0.4402 0.3654 -0.0335 -0.2444 0.6366 -0.6469 + -0.3623 0.3854 0.4162 0.4502 0.0065 0.1261 + -0.0121 -0.4377 0.0604 0.2265 -0.3389 0.4542 + + The controller input matrix BK is + + 0.0931 -0.0269 + -0.0872 0.1599 + 0.0956 -0.1469 + -0.1728 0.0129 + 0.2022 -0.1154 + 0.2419 -0.1737 + + The controller output matrix CK is + + -0.3677 0.2188 0.0403 -0.0854 0.3564 -0.3535 + 0.1624 -0.0708 0.0058 0.0606 -0.2163 0.1802 + + The controller matrix DK is + + -0.0857 -0.0246 + 0.0460 0.0074 + + The estimated condition numbers are + + 0.11269D-01 0.17596D-01 0.18225D+00 0.75968D-03
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ncfsyn/SB10ZD.dat Sun Jul 31 21:57:09 2011 +0000 @@ -0,0 +1,21 @@ + SB10LD EXAMPLE PROGRAM DATA + 6 2 3 + 0.2 0.0 3.0 0.0 -0.3 -0.1 + -3.0 0.2 -0.4 -0.3 0.0 0.0 + -0.1 0.1 -1.0 0.0 0.0 -3.0 + 1.0 0.0 0.0 -1.0 -1.0 0.0 + 0.0 0.3 0.6 2.0 0.1 -0.4 + 0.2 -4.0 0.0 0.0 0.2 -2.0 + -1.0 -2.0 + 1.0 3.0 + -3.0 -4.0 + 1.0 -2.0 + 0.0 1.0 + 1.0 5.0 + 1.0 -1.0 2.0 -2.0 0.0 -3.0 + -3.0 0.0 1.0 -1.0 1.0 -1.0 + 2.0 4.0 -3.0 0.0 5.0 1.0 + 10.0 -6.0 + -7.0 8.0 + 2.0 -4.0 + 1.1 0.0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ncfsyn/SB10ZD.f Sun Jul 31 21:57:09 2011 +0000 @@ -0,0 +1,914 @@ + SUBROUTINE SB10ZD( N, M, NP, A, LDA, B, LDB, C, LDC, D, LDD, + $ FACTOR, AK, LDAK, BK, LDBK, CK, LDCK, DK, + $ LDDK, RCOND, TOL, IWORK, DWORK, LDWORK, BWORK, + $ INFO ) +C +C SLICOT RELEASE 5.0. +C +C Copyright (c) 2002-2010 NICONET e.V. +C +C This program is free software: you can redistribute it and/or +C modify it under the terms of the GNU General Public License as +C published by the Free Software Foundation, either version 2 of +C the License, or (at your option) any later version. +C +C This program is distributed in the hope that it will be useful, +C but WITHOUT ANY WARRANTY; without even the implied warranty of +C MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +C GNU General Public License for more details. +C +C You should have received a copy of the GNU General Public License +C along with this program. If not, see +C <http://www.gnu.org/licenses/>. +C +C PURPOSE +C +C To compute the matrices of the positive feedback controller +C +C | Ak | Bk | +C K = |----|----| +C | Ck | Dk | +C +C for the shaped plant +C +C | A | B | +C G = |---|---| +C | C | D | +C +C in the Discrete-Time Loop Shaping Design Procedure. +C +C ARGUMENTS +C +C Input/Output Parameters +C +C N (input) INTEGER +C The order of the plant. N >= 0. +C +C M (input) INTEGER +C The column size of the matrix B. M >= 0. +C +C NP (input) INTEGER +C The row size of the matrix C. NP >= 0. +C +C A (input) DOUBLE PRECISION array, dimension (LDA,N) +C The leading N-by-N part of this array must contain the +C system state matrix A of the shaped plant. +C +C LDA INTEGER +C The leading dimension of the array A. LDA >= max(1,N). +C +C B (input) DOUBLE PRECISION array, dimension (LDB,M) +C The leading N-by-M part of this array must contain the +C system input matrix B of the shaped plant. +C +C LDB INTEGER +C The leading dimension of the array B. LDB >= max(1,N). +C +C C (input) DOUBLE PRECISION array, dimension (LDC,N) +C The leading NP-by-N part of this array must contain the +C system output matrix C of the shaped plant. +C +C LDC INTEGER +C The leading dimension of the array C. LDC >= max(1,NP). +C +C D (input) DOUBLE PRECISION array, dimension (LDD,M) +C The leading NP-by-M part of this array must contain the +C system input/output matrix D of the shaped plant. +C +C LDD INTEGER +C The leading dimension of the array D. LDD >= max(1,NP). +C +C FACTOR (input) DOUBLE PRECISION +C = 1 implies that an optimal controller is required +C (not recommended); +C > 1 implies that a suboptimal controller is required +C achieving a performance FACTOR less than optimal. +C FACTOR >= 1. +C +C AK (output) DOUBLE PRECISION array, dimension (LDAK,N) +C The leading N-by-N part of this array contains the +C controller state matrix Ak. +C +C LDAK INTEGER +C The leading dimension of the array AK. LDAK >= max(1,N). +C +C BK (output) DOUBLE PRECISION array, dimension (LDBK,NP) +C The leading N-by-NP part of this array contains the +C controller input matrix Bk. +C +C LDBK INTEGER +C The leading dimension of the array BK. LDBK >= max(1,N). +C +C CK (output) DOUBLE PRECISION array, dimension (LDCK,N) +C The leading M-by-N part of this array contains the +C controller output matrix Ck. +C +C LDCK INTEGER +C The leading dimension of the array CK. LDCK >= max(1,M). +C +C DK (output) DOUBLE PRECISION array, dimension (LDDK,NP) +C The leading M-by-NP part of this array contains the +C controller matrix Dk. +C +C LDDK INTEGER +C The leading dimension of the array DK. LDDK >= max(1,M). +C +C RCOND (output) DOUBLE PRECISION array, dimension (6) +C RCOND(1) contains an estimate of the reciprocal condition +C number of the linear system of equations from +C which the solution of the P-Riccati equation is +C obtained; +C RCOND(2) contains an estimate of the reciprocal condition +C number of the linear system of equations from +C which the solution of the Q-Riccati equation is +C obtained; +C RCOND(3) contains an estimate of the reciprocal condition +C number of the matrix (gamma^2-1)*In - P*Q; +C RCOND(4) contains an estimate of the reciprocal condition +C number of the matrix Rx + Bx'*X*Bx; +C RCOND(5) contains an estimate of the reciprocal condition +C ^ +C number of the matrix Ip + D*Dk; +C RCOND(6) contains an estimate of the reciprocal condition +C ^ +C number of the matrix Im + Dk*D. +C +C Tolerances +C +C TOL DOUBLE PRECISION +C Tolerance used for checking the nonsingularity of the +C matrices to be inverted. If TOL <= 0, then a default value +C equal to sqrt(EPS) is used, where EPS is the relative +C machine precision. TOL < 1. +C +C Workspace +C +C IWORK INTEGER array, dimension 2*max(N,M+NP) +C +C DWORK DOUBLE PRECISION array, dimension (LDWORK) +C On exit, if INFO = 0, DWORK(1) contains the optimal value +C of LDWORK. +C +C LDWORK INTEGER +C The dimension of the array DWORK. +C LDWORK >= 16*N*N + 5*M*M + 7*NP*NP + 6*M*N + 7*M*NP + +C 7*N*NP + 6*N + 2*(M + NP) + +C max(14*N+23,16*N,2*M-1,2*NP-1). +C For good performance, LDWORK must generally be larger. +C +C BWORK LOGICAL array, dimension (2*N) +C +C Error Indicator +C +C INFO (output) INTEGER +C = 0: successful exit; +C < 0: if INFO = -i, the i-th argument had an illegal +C value; +C = 1: the P-Riccati equation is not solved successfully; +C = 2: the Q-Riccati equation is not solved successfully; +C = 3: the iteration to compute eigenvalues or singular +C values failed to converge; +C = 4: the matrix (gamma^2-1)*In - P*Q is singular; +C = 5: the matrix Rx + Bx'*X*Bx is singular; +C ^ +C = 6: the matrix Ip + D*Dk is singular; +C ^ +C = 7: the matrix Im + Dk*D is singular; +C = 8: the matrix Ip - D*Dk is singular; +C = 9: the matrix Im - Dk*D is singular; +C = 10: the closed-loop system is unstable. +C +C METHOD +C +C The routine implements the formulas given in [1]. +C +C REFERENCES +C +C [1] Gu, D.-W., Petkov, P.H., and Konstantinov, M.M. +C On discrete H-infinity loop shaping design procedure routines. +C Technical Report 00-6, Dept. of Engineering, Univ. of +C Leicester, UK, 2000. +C +C NUMERICAL ASPECTS +C +C The accuracy of the results depends on the conditioning of the +C two Riccati equations solved in the controller design. For +C better conditioning it is advised to take FACTOR > 1. +C +C CONTRIBUTORS +C +C P.Hr. Petkov, D.W. Gu and M.M. Konstantinov, May 2001. +C +C REVISIONS +C +C V. Sima, Research Institute for Informatics, Bucharest, July 2001. +C +C KEYWORDS +C +C H_infinity control, Loop-shaping design, Robust control. +C +C ****************************************************************** +C +C .. Parameters .. + DOUBLE PRECISION ZERO, ONE + PARAMETER ( ZERO = 0.0D+0, ONE = 1.0D+0 ) +C .. +C .. Scalar Arguments .. + INTEGER INFO, LDA, LDAK, LDB, LDBK, LDC, LDCK, LDD, + $ LDDK, LDWORK, M, N, NP + DOUBLE PRECISION FACTOR, TOL +C .. +C .. Array Arguments .. + INTEGER IWORK( * ) + LOGICAL BWORK( * ) + DOUBLE PRECISION A ( LDA, * ), AK( LDAK, * ), B ( LDB, * ), + $ BK( LDBK, * ), C ( LDC, * ), CK( LDCK, * ), + $ D ( LDD, * ), DK( LDDK, * ), DWORK( * ), + $ RCOND( 6 ) +C .. +C .. Local Scalars .. + INTEGER I, I1, I2, I3, I4, I5, I6, I7, I8, I9, I10, + $ I11, I12, I13, I14, I15, I16, I17, I18, I19, + $ I20, I21, I22, I23, I24, I25, I26, INFO2, IWRK, + $ J, LWAMAX, MINWRK, N2, NS, SDIM + DOUBLE PRECISION ANORM, GAMMA, TOLL +C .. +C .. External Functions .. + LOGICAL SELECT + DOUBLE PRECISION DLAMCH, DLANGE, DLANSY, DLAPY2 + EXTERNAL DLAMCH, DLANGE, DLANSY, DLAPY2, SELECT +C .. +C .. External Subroutines .. + EXTERNAL DCOPY, DGECON, DGEES, DGEMM, DGETRF, DGETRS, + $ DLACPY, DLASCL, DLASET, DPOTRF, DPOTRS, DSWAP, + $ DSYCON, DSYEV, DSYRK, DSYTRF, DSYTRS, DTRSM, + $ DTRTRS, MA02AD, MB01RX, MB02VD, SB02OD, XERBLA +C .. +C .. Intrinsic Functions .. + INTRINSIC INT, MAX, SQRT +C .. +C .. Executable Statements .. +C +C Decode and Test input parameters. +C + INFO = 0 + IF( N.LT.0 ) THEN + INFO = -1 + ELSE IF( M.LT.0 ) THEN + INFO = -2 + ELSE IF( NP.LT.0 ) THEN + INFO = -3 + ELSE IF( LDA.LT.MAX( 1, N ) ) THEN + INFO = -5 + ELSE IF( LDB.LT.MAX( 1, N ) ) THEN + INFO = -7 + ELSE IF( LDC.LT.MAX( 1, NP ) ) THEN + INFO = -9 + ELSE IF( LDD.LT.MAX( 1, NP ) ) THEN + INFO = -11 + ELSE IF( FACTOR.LT.ONE ) THEN + INFO = -12 + ELSE IF( LDAK.LT.MAX( 1, N ) ) THEN + INFO = -14 + ELSE IF( LDBK.LT.MAX( 1, N ) ) THEN + INFO = -16 + ELSE IF( LDCK.LT.MAX( 1, M ) ) THEN + INFO = -18 + ELSE IF( LDDK.LT.MAX( 1, M ) ) THEN + INFO = -20 + ELSE IF( TOL.GE.ONE ) THEN + INFO = -22 + END IF +C +C Compute workspace. +C + MINWRK = 16*N*N + 5*M*M + 7*NP*NP + 6*M*N + 7*M*NP + 7*N*NP + + $ 6*N + 2*(M + NP) + MAX( 14*N+23, 16*N, 2*M-1, 2*NP-1 ) + IF( LDWORK.LT.MINWRK ) THEN + INFO = -25 + END IF + IF( INFO.NE.0 ) THEN + CALL XERBLA( 'SB10ZD', -INFO ) + RETURN + END IF +C +C Quick return if possible. +C Note that some computation could be made if one or two of the +C dimension parameters N, M, and P are zero, but the results are +C not so meaningful. +C + IF( N.EQ.0 .OR. M.EQ.0 .OR. NP.EQ.0 ) THEN + RCOND( 1 ) = ONE + RCOND( 2 ) = ONE + RCOND( 3 ) = ONE + RCOND( 4 ) = ONE + RCOND( 5 ) = ONE + RCOND( 6 ) = ONE + DWORK( 1 ) = ONE + RETURN + END IF +C +C Set the default tolerance, if needed. +C + IF( TOL.LE.ZERO ) THEN + TOLL = SQRT( DLAMCH( 'Epsilon' ) ) + ELSE + TOLL = TOL + END IF +C +C Workspace usage. +C + N2 = 2*N + I1 = 1 + N*N + I2 = I1 + N*N + I3 = I2 + NP*NP + I4 = I3 + M*M + I5 = I4 + NP*NP + I6 = I5 + M*M + I7 = I6 + M*N + I8 = I7 + M*N + I9 = I8 + N*N + I10 = I9 + N*N + I11 = I10 + N2 + I12 = I11 + N2 + I13 = I12 + N2 + I14 = I13 + N2*N2 + I15 = I14 + N2*N2 +C + IWRK = I15 + N2*N2 + LWAMAX = 0 +C +C Compute R1 = Ip + D*D' . +C + CALL DLASET( 'U', NP, NP, ZERO, ONE, DWORK( I2 ), NP ) + CALL DSYRK( 'U', 'N', NP, M, ONE, D, LDD, ONE, DWORK( I2 ), NP ) + CALL DLACPY( 'U', NP, NP, DWORK( I2 ), NP, DWORK( I4 ), NP ) +C +C Factorize R1 = R'*R . +C + CALL DPOTRF( 'U', NP, DWORK( I4 ), NP, INFO2 ) +C -1 +C Compute C'*R in BK . +C + CALL MA02AD( 'F', NP, N, C, LDC, BK, LDBK ) + CALL DTRSM( 'R', 'U', 'N', 'N', N, NP, ONE, DWORK( I4 ), NP, BK, + $ LDBK ) +C +C Compute R2 = Im + D'*D . +C + CALL DLASET( 'U', M, M, ZERO, ONE, DWORK( I3 ), M ) + CALL DSYRK( 'U', 'T', M, NP, ONE, D, LDD, ONE, DWORK( I3 ), M ) + CALL DLACPY( 'U', M, M, DWORK( I3 ), M, DWORK( I5 ), M ) +C +C Factorize R2 = U'*U . +C + CALL DPOTRF( 'U', M, DWORK( I5 ), M, INFO2 ) +C -1 +C Compute (U )'*B' . +C + CALL MA02AD( 'F', N, M, B, LDB, DWORK( I6 ), M ) + CALL DTRTRS( 'U', 'T', 'N', M, N, DWORK( I5 ), M, DWORK( I6 ), M, + $ INFO2 ) +C +C Compute D'*C . +C + CALL DGEMM( 'T', 'N', M, N, NP, ONE, D, LDD, C, LDC, ZERO, + $ DWORK( I7 ), M ) +C -1 +C Compute (U )'*D'*C . +C + CALL DTRTRS( 'U', 'T', 'N', M, N, DWORK( I5 ), M, DWORK( I7 ), M, + $ INFO2 ) +C -1 +C Compute Ar = A - B*R2 D'*C . +C + CALL DLACPY( 'F', N, N, A, LDA, DWORK( I8 ), N ) + CALL DGEMM( 'T', 'N', N, N, M, -ONE, DWORK( I6 ), M, DWORK( I7 ), + $ M, ONE, DWORK( I8 ), N ) +C -1 +C Compute Cr = C'*R1 *C . +C + CALL DSYRK( 'U', 'N', N, NP, ONE, BK, LDBK, ZERO, DWORK( I9 ), N ) +C -1 +C Compute Dr = B*R2 B' in AK . +C + CALL DSYRK( 'U', 'T', N, M, ONE, DWORK( I6 ), M, ZERO, AK, LDAK ) +C -1 +C Solution of the Riccati equation Ar'*P*(In + Dr*P) Ar - P + +C Cr = 0 . + CALL SB02OD( 'D', 'G', 'N', 'U', 'Z', 'S', N, M, NP, DWORK( I8 ), + $ N, AK, LDAK, DWORK( I9 ), N, DWORK, M, DWORK, N, + $ RCOND( 1 ), DWORK, N, DWORK( I10 ), DWORK( I11 ), + $ DWORK( I12 ), DWORK( I13 ), N2, DWORK( I14 ), N2, + $ DWORK( I15 ), N2, -ONE, IWORK, DWORK( IWRK ), + $ LDWORK-IWRK+1, BWORK, INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 1 + RETURN + END IF + LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 ) +C +C Transpose Ar . +C + DO 10 J = 1, N - 1 + CALL DSWAP( J, DWORK( I8+J ), N, DWORK( I8+J*N ), 1 ) + 10 CONTINUE +C -1 +C Solution of the Riccati equation Ar*Q*(In + Cr*Q) *Ar' - Q + +C Dr = 0 . + CALL SB02OD( 'D', 'G', 'N', 'U', 'Z', 'S', N, M, NP, DWORK( I8 ), + $ N, DWORK( I9 ), N, AK, LDAK, DWORK, M, DWORK, N, + $ RCOND( 2 ), DWORK( I1 ), N, DWORK( I10 ), + $ DWORK( I11 ), DWORK( I12 ), DWORK( I13 ), N2, + $ DWORK( I14 ), N2, DWORK( I15 ), N2, -ONE, IWORK, + $ DWORK( IWRK ), LDWORK-IWRK+1, BWORK, INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 2 + RETURN + END IF + LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 ) +C +C Compute gamma. +C + CALL DGEMM( 'N', 'N', N, N, N, ONE, DWORK( I1 ), N, DWORK, N, + $ ZERO, DWORK( I8 ), N ) + CALL DGEES( 'N', 'N', SELECT, N, DWORK( I8 ), N, SDIM, + $ DWORK( I10 ), DWORK( I11 ), DWORK( IWRK ), N, + $ DWORK( IWRK ), LDWORK-IWRK+1, BWORK, INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 3 + RETURN + END IF + LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 ) + GAMMA = ZERO +C + DO 20 I = 0, N - 1 + GAMMA = MAX( GAMMA, DWORK( I10+I ) ) + 20 CONTINUE +C + GAMMA = FACTOR*SQRT( ONE + GAMMA ) +C +C Workspace usage. +C + I5 = I4 + NP*NP + I6 = I5 + M*M + I7 = I6 + NP*NP + I8 = I7 + NP*NP + I9 = I8 + NP*NP + I10 = I9 + NP + I11 = I10 + NP*NP + I12 = I11 + M*M + I13 = I12 + M +C + IWRK = I13 + M*M +C +C Compute the eigenvalues and eigenvectors of R1 . +C + CALL DLACPY( 'U', NP, NP, DWORK( I2 ), NP, DWORK( I8 ), NP ) + CALL DSYEV( 'V', 'U', NP, DWORK( I8 ), NP, DWORK( I9 ), + $ DWORK( IWRK ), LDWORK-IWRK+1, INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 3 + RETURN + END IF + LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 ) +C -1/2 +C Compute R1 . +C + DO 40 J = 1, NP + DO 30 I = 1, NP + DWORK( I10-1+I+(J-1)*NP ) = DWORK( I8-1+J+(I-1)*NP ) / + $ SQRT( DWORK( I9+I-1 ) ) + 30 CONTINUE + 40 CONTINUE +C + CALL DGEMM( 'N', 'N', NP, NP, NP, ONE, DWORK( I8 ), NP, + $ DWORK( I10 ), NP, ZERO, DWORK( I4 ), NP ) +C +C Compute the eigenvalues and eigenvectors of R2 . +C + CALL DLACPY( 'U', M, M, DWORK( I3 ), M, DWORK( I11 ), M ) + CALL DSYEV( 'V', 'U', M, DWORK( I11 ), M, DWORK( I12 ), + $ DWORK( IWRK ), LDWORK-IWRK+1, INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 3 + RETURN + END IF + LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 ) +C -1/2 +C Compute R2 . +C + DO 60 J = 1, M + DO 50 I = 1, M + DWORK( I13-1+I+(J-1)*M ) = DWORK( I11-1+J+(I-1)*M ) / + $ SQRT( DWORK( I12+I-1 ) ) + 50 CONTINUE + 60 CONTINUE +C + CALL DGEMM( 'N', 'N', M, M, M, ONE, DWORK( I11 ), M, DWORK( I13 ), + $ M, ZERO, DWORK( I5 ), M ) +C +C Compute R1 + C*Q*C' . +C + CALL DGEMM( 'N', 'T', N, NP, N, ONE, DWORK( I1 ), N, C, LDC, + $ ZERO, BK, LDBK ) + CALL MB01RX( 'L', 'U', 'N', NP, N, ONE, ONE, DWORK( I2 ), NP, + $ C, LDC, BK, LDBK, INFO2 ) + CALL DLACPY( 'U', NP, NP, DWORK( I2 ), NP, DWORK( I8 ), NP ) +C +C Compute the eigenvalues and eigenvectors of R1 + C*Q*C' . +C + CALL DSYEV( 'V', 'U', NP, DWORK( I8 ), NP, DWORK( I9 ), + $ DWORK( IWRK ), LDWORK-IWRK+1, INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 3 + RETURN + END IF + LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 ) +C -1 +C Compute ( R1 + C*Q*C' ) . +C + DO 80 J = 1, NP + DO 70 I = 1, NP + DWORK( I10-1+I+(J-1)*NP ) = DWORK( I8-1+J+(I-1)*NP ) / + $ DWORK( I9+I-1 ) + 70 CONTINUE + 80 CONTINUE +C + CALL DGEMM( 'N', 'N', NP, NP, NP, ONE, DWORK( I8 ), NP, + $ DWORK( I10 ), NP, ZERO, DWORK( I6 ), NP ) +C -1 +C Compute Z2 . +C + DO 100 J = 1, NP + DO 90 I = 1, NP + DWORK( I10-1+I+(J-1)*NP ) = DWORK( I8-1+J+(I-1)*NP )* + $ SQRT( DWORK( I9+I-1 ) ) + 90 CONTINUE + 100 CONTINUE +C + CALL DGEMM( 'N', 'N', NP, NP, NP, ONE, DWORK( I8 ), NP, + $ DWORK( I10 ), NP, ZERO, DWORK( I7 ), NP ) +C +C Workspace usage. +C + I9 = I8 + N*NP + I10 = I9 + N*NP + I11 = I10 + NP*M + I12 = I11 + ( NP + M )*( NP + M ) + I13 = I12 + N*( NP + M ) + I14 = I13 + N*( NP + M ) + I15 = I14 + N*N + I16 = I15 + N*N + I17 = I16 + ( NP + M )*N + I18 = I17 + ( NP + M )*( NP + M ) + I19 = I18 + ( NP + M )*N + I20 = I19 + M*N + I21 = I20 + M*NP + I22 = I21 + NP*N + I23 = I22 + N*N + I24 = I23 + N*NP + I25 = I24 + NP*NP + I26 = I25 + M*M +C + IWRK = I26 + N*M +C +C Compute A*Q*C' + B*D' . +C + CALL DGEMM( 'N', 'T', N, NP, M, ONE, B, LDB, D, LDD, ZERO, + $ DWORK( I8 ), N ) + CALL DGEMM( 'N', 'N', N, NP, N, ONE, A, LDA, BK, LDBK, + $ ONE, DWORK( I8 ), N ) +C -1 +C Compute H = -( A*Q*C'+B*D' )*( R1 + C*Q*C' ) . +C + CALL DGEMM( 'N', 'N', N, NP, NP, -ONE, DWORK( I8 ), N, + $ DWORK( I6 ), NP, ZERO, DWORK( I9 ), N ) +C -1/2 +C Compute R1 D . +C + CALL DGEMM( 'N', 'N', NP, M, NP, ONE, DWORK( I4 ), NP, D, LDD, + $ ZERO, DWORK( I10 ), NP ) +C +C Compute Rx . +C + DO 110 J = 1, NP + CALL DCOPY( J, DWORK( I2+(J-1)*NP ), 1, + $ DWORK( I11+(J-1)*(NP+M) ), 1 ) + DWORK( I11-1+J+(J-1)*(NP+M) ) = DWORK( I2-1+J+(J-1)*NP ) - + $ GAMMA*GAMMA + 110 CONTINUE +C + CALL DGEMM( 'N', 'N', NP, M, NP, ONE, DWORK( I7 ), NP, + $ DWORK( I10 ), NP, ZERO, DWORK( I11+(NP+M)*NP ), + $ NP+M ) + CALL DLASET( 'U', M, M, ZERO, ONE, DWORK( I11+(NP+M)*NP+NP ), + $ NP+M ) +C +C Compute Bx . +C + CALL DGEMM( 'N', 'N', N, NP, NP, -ONE, DWORK( I9 ), N, + $ DWORK( I7 ), NP, ZERO, DWORK( I12 ), N ) + CALL DGEMM( 'N', 'N', N, M, M, ONE, B, LDB, DWORK( I5 ), M, + $ ZERO, DWORK( I12+N*NP ), N ) +C +C Compute Sx . +C + CALL DGEMM( 'T', 'N', N, NP, NP, ONE, C, LDC, DWORK( I7 ), NP, + $ ZERO, DWORK( I13 ), N ) + CALL DGEMM( 'T', 'N', N, M, NP, ONE, C, LDC, DWORK( I10 ), NP, + $ ZERO, DWORK( I13+N*NP ), N ) +C +C Compute (gamma^2 - 1)*In - P*Q . +C + CALL DLASET( 'F', N, N, ZERO, GAMMA*GAMMA-ONE, DWORK( I14 ), N ) + CALL DGEMM( 'N', 'N', N, N, N, -ONE, DWORK, N, DWORK( I1 ), N, + $ ONE, DWORK( I14 ), N ) +C -1 +C Compute X = ((gamma^2 - 1)*In - P*Q) *gamma^2*P . +C + CALL DLACPY( 'F', N, N, DWORK, N, DWORK( I15 ), N ) + CALL DLASCL( 'G', 0, 0, ONE, GAMMA*GAMMA, N, N, DWORK( I15 ), N, + $ INFO ) + ANORM = DLANGE( '1', N, N, DWORK( I14 ), N, DWORK( IWRK ) ) + CALL DGETRF( N, N, DWORK( I14 ), N, IWORK, INFO2 ) + IF( INFO2.GT.0 ) THEN + INFO = 4 + RETURN + END IF + CALL DGECON( '1', N, DWORK( I14 ), N, ANORM, RCOND( 3 ), + $ DWORK( IWRK ), IWORK( N+1 ), INFO2 ) +C +C Return if the matrix is singular to working precision. +C + IF( RCOND( 3 ).LT.TOLL ) THEN + INFO = 4 + RETURN + END IF + CALL DGETRS( 'N', N, N, DWORK( I14 ), N, IWORK, DWORK( I15 ), + $ N, INFO2 ) +C +C Compute Bx'*X . +C + CALL DGEMM( 'T', 'N', NP+M, N, N, ONE, DWORK( I12 ), N, + $ DWORK( I15 ), N, ZERO, DWORK( I16 ), NP+M ) +C +C Compute Rx + Bx'*X*Bx . +C + CALL DLACPY( 'U', NP+M, NP+M, DWORK( I11 ), NP+M, DWORK( I17 ), + $ NP+M ) + CALL MB01RX( 'L', 'U', 'N', NP+M, N, ONE, ONE, DWORK( I17 ), NP+M, + $ DWORK( I16 ), NP+M, DWORK( I12 ), N, INFO2 ) +C +C Compute -( Sx' + Bx'*X*A ) . +C + CALL MA02AD( 'F', N, NP+M, DWORK( I13 ), N, DWORK( I18 ), NP+M ) + CALL DGEMM( 'N', 'N', NP+M, N, N, -ONE, DWORK( I16 ), NP+M, + $ A, LDA, -ONE, DWORK( I18 ), NP+M ) +C +C Factorize Rx + Bx'*X*Bx . +C + ANORM = DLANSY( '1', 'U', NP+M, DWORK( I17 ), NP+M, + $ DWORK( IWRK ) ) + CALL DSYTRF( 'U', NP+M, DWORK( I17 ), NP+M, IWORK, + $ DWORK( IWRK ), LDWORK-IWRK, INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 5 + RETURN + END IF + CALL DSYCON( 'U', NP+M, DWORK( I17 ), NP+M, IWORK, ANORM, + $ RCOND( 4 ), DWORK( IWRK ), IWORK( NP+M+1), INFO2 ) +C +C Return if the matrix is singular to working precision. +C + IF( RCOND( 4 ).LT.TOLL ) THEN + INFO = 5 + RETURN + END IF +C -1 +C Compute F = -( Rx + Bx'*X*Bx ) ( Sx' + Bx'*X*A ) . +C + CALL DSYTRS( 'U', NP+M, N, DWORK( I17 ), NP+M, IWORK, + $ DWORK( I18 ), NP+M, INFO2 ) +C +C Compute B'*X . +C + CALL DGEMM( 'T', 'N', M, N, N, ONE, B, LDB, DWORK( I15 ), N, + $ ZERO, DWORK( I19 ), M ) +C +C Compute -( D' - B'*X*H ) . +C + DO 130 J = 1, NP + DO 120 I = 1, M + DWORK( I20-1+I+(J-1)*M ) = -D( J, I ) + 120 CONTINUE + 130 CONTINUE +C + CALL DGEMM( 'N', 'N', M, NP, N, ONE, DWORK( I19 ), M, + $ DWORK( I9 ), N, ONE, DWORK( I20 ), M ) +C -1 +C Compute C + Z2 *F1 . +C + CALL DLACPY( 'F', NP, N, C, LDC, DWORK( I21 ), NP ) + CALL DGEMM( 'N', 'N', NP, N, NP, ONE, DWORK( I7 ), NP, + $ DWORK( I18 ), NP+M, ONE, DWORK( I21 ), NP ) +C +C Compute R2 + B'*X*B . +C + CALL MB01RX( 'L', 'U', 'N', M, N, ONE, ONE, DWORK( I3 ), M, + $ DWORK( I19 ), M, B, LDB, INFO2 ) +C +C Factorize R2 + B'*X*B . +C + CALL DPOTRF( 'U', M, DWORK( I3 ), M, INFO2 ) +C ^ -1 +C Compute Dk = -( R2 + B'*X*B ) (D' - B'*X*H) . +C + CALL DLACPY( 'F', M, NP, DWORK( I20 ), M, DK, LDDK ) + CALL DPOTRS( 'U', M, NP, DWORK( I3 ), M, DK, LDDK, INFO2 ) +C ^ ^ +C Compute Bk = -H + B*Dk . +C + CALL DLACPY( 'F', N, NP, DWORK( I9 ), N, DWORK( I23 ), N ) + CALL DGEMM( 'N', 'N', N, NP, M, ONE, B, LDB, DK, LDDK, + $ -ONE, DWORK( I23 ), N ) +C -1/2 +C Compute R2 *F2 . +C + CALL DGEMM( 'N', 'N', M, N, M, ONE, DWORK( I5 ), M, + $ DWORK( I18+NP ), NP+M, ZERO, CK, LDCK ) +C ^ -1/2 ^ -1 +C Compute Ck = R2 *F2 - Dk*( C + Z2 *F1 ) . +C + CALL DGEMM( 'N', 'N', M, N, NP, -ONE, DK, LDDK, + $ DWORK( I21 ), NP, ONE, CK, LDCK ) +C ^ ^ +C Compute Ak = A + H*C + B*Ck . +C + CALL DLACPY( 'F', N, N, A, LDA, AK, LDAK ) + CALL DGEMM( 'N', 'N', N, N, NP, ONE, DWORK( I9 ), N, C, LDC, + $ ONE, AK, LDAK ) + CALL DGEMM( 'N', 'N', N, N, M, ONE, B, LDB, CK, LDCK, + $ ONE, AK, LDAK ) +C ^ +C Compute Ip + D*Dk . +C + CALL DLASET( 'Full', NP, NP, ZERO, ONE, DWORK( I24 ), NP ) + CALL DGEMM( 'N', 'N', NP, NP, M, ONE, D, LDD, DK, LDDK, + $ ONE, DWORK( I24 ), NP ) +C ^ +C Compute Im + Dk*D . +C + CALL DLASET( 'Full', M, M, ZERO, ONE, DWORK( I25 ), M ) + CALL DGEMM( 'N', 'N', M, M, NP, ONE, DK, LDDK, D, LDD, + $ ONE, DWORK( I25 ), M ) +C ^ ^ ^ ^ -1 +C Compute Ck = M*Ck, M = (Im + Dk*D) . +C + ANORM = DLANGE( '1', M, M, DWORK( I25 ), M, DWORK( IWRK ) ) + CALL DGETRF( M, M, DWORK( I25 ), M, IWORK, INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 7 + RETURN + END IF + CALL DGECON( '1', M, DWORK( I25 ), M, ANORM, RCOND( 6 ), + $ DWORK( IWRK ), IWORK( M+1 ), INFO2 ) +C +C Return if the matrix is singular to working precision. +C + IF( RCOND( 6 ).LT.TOLL ) THEN + INFO = 7 + RETURN + END IF + CALL DGETRS( 'N', M, N, DWORK( I25 ), M, IWORK, CK, LDCK, INFO2 ) +C ^ ^ +C Compute Dk = M*Dk . +C + CALL DGETRS( 'N', M, NP, DWORK( I25 ), M, IWORK, DK, LDDK, INFO2 ) +C ^ +C Compute Bk*D . +C + CALL DGEMM( 'N', 'N', N, M, NP, ONE, DWORK( I23 ), N, D, LDD, + $ ZERO, DWORK( I26 ), N ) +C ^ ^ +C Compute Ak = Ak - Bk*D*Ck. +C + CALL DGEMM( 'N', 'N', N, N, M, -ONE, DWORK( I26 ), N, CK, LDCK, + $ ONE, AK, LDAK ) +C ^ ^ -1 +C Compute Bk = Bk*(Ip + D*Dk) . +C + ANORM = DLANGE( '1', NP, NP, DWORK( I24 ), NP, DWORK( IWRK ) ) + CALL DLACPY( 'Full', N, NP, DWORK( I23 ), N, BK, LDBK ) + CALL MB02VD( 'N', N, NP, DWORK( I24 ), NP, IWORK, BK, LDBK, + $ INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 6 + RETURN + END IF + CALL DGECON( '1', NP, DWORK( I24 ), NP, ANORM, RCOND( 5 ), + $ DWORK( IWRK ), IWORK( NP+1 ), INFO2 ) +C +C Return if the matrix is singular to working precision. +C + IF( RCOND( 5 ).LT.TOLL ) THEN + INFO = 6 + RETURN + END IF +C +C Workspace usage. +C + I2 = 1 + NP*NP + I3 = I2 + N*NP + I4 = I3 + M*M + I5 = I4 + N*M + I6 = I5 + NP*N + I7 = I6 + M*N + I8 = I7 + N2*N2 + I9 = I8 + N2 +C + IWRK = I9 + N2 +C +C Compute Ip - D*Dk . +C + CALL DLASET( 'Full', NP, NP, ZERO, ONE, DWORK, NP ) + CALL DGEMM( 'N', 'N', NP, NP, M, -ONE, D, LDD, DK, LDDK, ONE, + $ DWORK, NP ) +C -1 +C Compute Bk*(Ip-D*Dk) . +C + CALL DLACPY( 'Full', N, NP, BK, LDBK, DWORK( I2 ), N ) + CALL MB02VD( 'N', N, NP, DWORK, NP, IWORK, DWORK( I2 ), N, INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 8 + RETURN + END IF +C +C Compute Im - Dk*D . +C + CALL DLASET( 'Full', M, M, ZERO, ONE, DWORK( I3 ), M ) + CALL DGEMM( 'N', 'N', M, M, NP, -ONE, DK, LDDK, D, LDD, ONE, + $ DWORK( I3 ), M ) +C -1 +C Compute B*(Im-Dk*D) . +C + CALL DLACPY( 'Full', N, M, B, LDB, DWORK( I4 ), N ) + CALL MB02VD( 'N', N, M, DWORK( I3 ), M, IWORK, DWORK( I4 ), N, + $ INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 9 + RETURN + END IF +C +C Compute D*Ck . +C + CALL DGEMM( 'N', 'N', NP, N, M, ONE, D, LDD, CK, LDCK, ZERO, + $ DWORK( I5 ), NP ) +C +C Compute Dk*C . +C + CALL DGEMM( 'N', 'N', M, N, NP, ONE, DK, LDDK, C, LDC, ZERO, + $ DWORK( I6 ), M ) +C +C Compute the closed-loop state matrix. +C + CALL DLACPY( 'F', N, N, A, LDA, DWORK( I7 ), N2 ) + CALL DGEMM( 'N', 'N', N, N, M, ONE, DWORK( I4 ), N, + $ DWORK( I6 ), M, ONE, DWORK( I7 ), N2 ) + CALL DGEMM( 'N', 'N', N, N, M, ONE, DWORK( I4 ), N, CK, LDCK, + $ ZERO, DWORK( I7+N2*N ), N2 ) + CALL DGEMM( 'N', 'N', N, N, NP, ONE, DWORK( I2 ), N, C, LDC, + $ ZERO, DWORK( I7+N ), N2 ) + CALL DLACPY( 'F', N, N, AK, LDAK, DWORK( I7+N2*N+N ), N2 ) + CALL DGEMM( 'N', 'N', N, N, NP, ONE, DWORK( I2 ), N, + $ DWORK( I5 ), NP, ONE, DWORK( I7+N2*N+N ), N2 ) +C +C Compute the closed-loop poles. +C + CALL DGEES( 'N', 'N', SELECT, N2, DWORK( I7 ), N2, SDIM, + $ DWORK( I8 ), DWORK( I9 ), DWORK( IWRK ), N, + $ DWORK( IWRK ), LDWORK-IWRK+1, BWORK, INFO2 ) + IF( INFO2.NE.0 ) THEN + INFO = 3 + RETURN + END IF + LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 ) +C +C Check the stability of the closed-loop system. +C + NS = 0 +C + DO 140 I = 0, N2 - 1 + IF( DLAPY2( DWORK( I8+I ), DWORK( I9+I ) ).GT.ONE ) + $ NS = NS + 1 + 140 CONTINUE +C + IF( NS.GT.0 ) THEN + INFO = 10 + RETURN + END IF +C + DWORK( 1 ) = DBLE( LWAMAX ) + RETURN +C *** Last line of SB10ZD *** + END
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ncfsyn/SB10ZD.res Sun Jul 31 21:57:09 2011 +0000 @@ -0,0 +1,35 @@ + SB10ZD EXAMPLE PROGRAM RESULTS + + + The controller state matrix AK is + + 1.0128 0.5101 -0.1546 1.1300 3.3759 0.4911 + -2.1257 -1.4517 -0.4486 0.3493 -1.5506 -1.4296 + -1.0930 -0.6026 -0.1344 0.2253 -1.5625 -0.6762 + 0.3207 0.1698 0.2376 -1.1781 -0.8705 0.2896 + 0.5017 0.9006 0.0668 2.3613 0.2049 0.3703 + 1.0787 0.6703 0.2783 -0.7213 0.4918 0.7435 + + The controller input matrix BK is + + 0.4132 0.3112 -0.8077 + 0.2140 0.4253 0.1811 + -0.0710 0.0807 0.3558 + -0.0121 -0.2019 0.0249 + 0.1047 0.1399 -0.0457 + -0.2542 -0.3472 0.0523 + + The controller output matrix CK is + + -0.0372 -0.0456 -0.0040 0.0962 -0.2059 -0.0571 + 0.1999 0.2994 0.1335 -0.0251 -0.3108 0.2048 + + The controller matrix DK is + + 0.0629 -0.0022 0.0363 + -0.0228 0.0195 0.0600 + + The estimated condition numbers are + + 0.27949D-03 0.66679D-03 0.45677D-01 0.23433D-07 0.68495D-01 + 0.76854D-01
--- a/main/control/devel/ncfsyn/makefile_ncfsyn.m Sun Jul 31 19:51:16 2011 +0000 +++ b/main/control/devel/ncfsyn/makefile_ncfsyn.m Sun Jul 31 21:57:09 2011 +0000 @@ -6,3 +6,13 @@ MA02AD.f MB02PD.f MB01SD.f MB01UD.f SB03SY.f \ MB01RX.f SB03MX.f SB03SX.f MB01RY.f SB03QY.f \ SB03QX.f SB03MY.f SB04PX.f SB03MV.f SB03MW.f + +mkoctfile "-Wl,-framework" "-Wl,vecLib" \ + slsb10kd.cc \ + SB10KD.f SB02OD.f select.f SB02OY.f SB02OW.f \ + SB02OV.f SB02MV.f SB02OU.f SB02MR.f + +mkoctfile "-Wl,-framework" "-Wl,vecLib" \ + SB10ZD.f MA02AD.f SB02OD.f select.f MB01RX.f \ + MB02VD.f SB02OY.f SB02OW.f SB02OV.f SB02OU.f \ + SB02MR.f MA02GD.f SB02MV.f \ No newline at end of file
--- a/main/control/devel/ncfsyn/ncfsyn.m Sun Jul 31 19:51:16 2011 +0000 +++ b/main/control/devel/ncfsyn/ncfsyn.m Sun Jul 31 21:57:09 2011 +0000 @@ -54,7 +54,7 @@ elseif (any (d(:))) # discrete-time, d != 0 [ak, bk, ck, dk, rcond] = slsb10zd (a, b, c, d, factor); else # discrete-time, d == 0 - [ak, bk, ck, dk, rcond] = slsb10kd (a, b, c, d, factor); + [ak, bk, ck, dk, rcond] = slsb10kd (a, b, c, factor); endif ## controller @@ -194,3 +194,108 @@ %!assert (BK, BKe, 1e-4); %!assert (CK, CKe, 1e-4); %!assert (DK, DKe, 1e-4); + + +## discrete-time case D==0, direct access to sb10kd +%!shared AK, BK, CK, DK, RCOND, AKe, BKe, CKe, DKe, RCONDe +%! A = [ 0.2 0.0 0.3 0.0 -0.3 -0.1 +%! -0.3 0.2 -0.4 -0.3 0.0 0.0 +%! -0.1 0.1 -0.1 0.0 0.0 -0.3 +%! 0.1 0.0 0.0 -0.1 -0.1 0.0 +%! 0.0 0.3 0.6 0.2 0.1 -0.4 +%! 0.2 -0.4 0.0 0.0 0.2 -0.2 ]; +%! +%! B = [ -1.0 -2.0 +%! 1.0 3.0 +%! -3.0 -4.0 +%! 1.0 -2.0 +%! 0.0 1.0 +%! 1.0 5.0 ]; +%! +%! C = [ 1.0 -1.0 2.0 -2.0 0.0 -3.0 +%! -3.0 0.0 1.0 -1.0 1.0 -1.0 ]; +%! +%! FACTOR = 1.1; +%! +%! [AK, BK, CK, DK, RCOND] = slsb10kd (A, B, C, FACTOR); +%! +%! AKe = [ 0.0337 0.0222 0.0858 0.1264 -0.1872 0.1547 +%! 0.4457 0.0668 -0.2255 -0.3204 -0.4548 -0.0691 +%! -0.2419 -0.2506 -0.0982 -0.1321 -0.0130 -0.0838 +%! -0.4402 0.3654 -0.0335 -0.2444 0.6366 -0.6469 +%! -0.3623 0.3854 0.4162 0.4502 0.0065 0.1261 +%! -0.0121 -0.4377 0.0604 0.2265 -0.3389 0.4542 ]; +%! +%! BKe = [ 0.0931 -0.0269 +%! -0.0872 0.1599 +%! 0.0956 -0.1469 +%! -0.1728 0.0129 +%! 0.2022 -0.1154 +%! 0.2419 -0.1737 ]; +%! +%! CKe = [ -0.3677 0.2188 0.0403 -0.0854 0.3564 -0.3535 +%! 0.1624 -0.0708 0.0058 0.0606 -0.2163 0.1802 ]; +%! +%! DKe = [ -0.0857 -0.0246 +%! 0.0460 0.0074 ]; +%! +%! RCONDe = [ 0.11269D-01 0.17596D-01 0.18225D+00 0.75968D-03 ].'; +%! +%!assert (AK, AKe, 1e-4); +%!assert (BK, BKe, 1e-4); +%!assert (CK, CKe, 1e-4); +%!assert (DK, DKe, 1e-4); +%!assert (RCOND, RCONDe, 1e-4); + + +## discrete-time case D==0 +%!shared AK, BK, CK, DK, RCOND, AKe, BKe, CKe, DKe, RCONDe +%! A = [ 0.2 0.0 0.3 0.0 -0.3 -0.1 +%! -0.3 0.2 -0.4 -0.3 0.0 0.0 +%! -0.1 0.1 -0.1 0.0 0.0 -0.3 +%! 0.1 0.0 0.0 -0.1 -0.1 0.0 +%! 0.0 0.3 0.6 0.2 0.1 -0.4 +%! 0.2 -0.4 0.0 0.0 0.2 -0.2 ]; +%! +%! B = [ -1.0 -2.0 +%! 1.0 3.0 +%! -3.0 -4.0 +%! 1.0 -2.0 +%! 0.0 1.0 +%! 1.0 5.0 ]; +%! +%! C = [ 1.0 -1.0 2.0 -2.0 0.0 -3.0 +%! -3.0 0.0 1.0 -1.0 1.0 -1.0 ]; +%! +%! FACTOR = 1.1; +%! +%! G = ss (A, B, C, [], 1); +%! K = ncfsyn (G, [], [], FACTOR); +%! [AK, BK, CK, DK] = ssdata (K); +%! +%! AKe = [ 0.0337 0.0222 0.0858 0.1264 -0.1872 0.1547 +%! 0.4457 0.0668 -0.2255 -0.3204 -0.4548 -0.0691 +%! -0.2419 -0.2506 -0.0982 -0.1321 -0.0130 -0.0838 +%! -0.4402 0.3654 -0.0335 -0.2444 0.6366 -0.6469 +%! -0.3623 0.3854 0.4162 0.4502 0.0065 0.1261 +%! -0.0121 -0.4377 0.0604 0.2265 -0.3389 0.4542 ]; +%! +%! BKe = [ 0.0931 -0.0269 +%! -0.0872 0.1599 +%! 0.0956 -0.1469 +%! -0.1728 0.0129 +%! 0.2022 -0.1154 +%! 0.2419 -0.1737 ]; +%! +%! CKe = [ -0.3677 0.2188 0.0403 -0.0854 0.3564 -0.3535 +%! 0.1624 -0.0708 0.0058 0.0606 -0.2163 0.1802 ]; +%! +%! DKe = [ -0.0857 -0.0246 +%! 0.0460 0.0074 ]; +%! +%! RCONDe = [ 0.11269D-01 0.17596D-01 0.18225D+00 0.75968D-03 ].'; +%! +%!assert (AK, AKe, 1e-4); +%!assert (BK, BKe, 1e-4); +%!assert (CK, CKe, 1e-4); +%!assert (DK, DKe, 1e-4);
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ncfsyn/slsb10kd.cc Sun Jul 31 21:57:09 2011 +0000 @@ -0,0 +1,148 @@ +/* + +Copyright (C) 2011 Lukas F. Reichlin + +This file is part of LTI Syncope. + +LTI Syncope is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +LTI Syncope is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with LTI Syncope. If not, see <http://www.gnu.org/licenses/>. + +Positive feedback controller for a discrete-time system (D == 0). +Uses SLICOT SB10KD by courtesy of NICONET e.V. +<http://www.slicot.org> + +Author: Lukas Reichlin <lukas.reichlin@gmail.com> +Created: July 2011 +Version: 0.1 + +*/ + +#include <octave/oct.h> +#include <f77-fcn.h> +#include "common.cc" + +extern "C" +{ + int F77_FUNC (sb10kd, SB10KD) + (int& N, int& M, int& NP, + double* A, int& LDA, + double* B, int& LDB, + double* C, int& LDC, + double& FACTOR, + double* AK, int& LDAK, + double* BK, int& LDBK, + double* CK, int& LDCK, + double* DK, int& LDDK, + double* RCOND, + int* IWORK, + double* DWORK, int& LDWORK, + bool* BWORK, + int& INFO); +} + +DEFUN_DLD (slsb10kd, args, nargout, + "-*- texinfo -*-\n\ +Slicot SB10KD Release 5.0\n\ +No argument checking.\n\ +For internal use only.") +{ + int nargin = args.length (); + octave_value_list retval; + + if (nargin != 4) + { + print_usage (); + } + else + { + // arguments in + Matrix a = args(0).matrix_value (); + Matrix b = args(1).matrix_value (); + Matrix c = args(2).matrix_value (); + + double factor = args(3).double_value (); + + int n = a.rows (); // n: number of states + int m = b.columns (); // m: number of inputs + int np = c.rows (); // np: number of outputs + + int lda = max (1, n); + int ldb = max (1, n); + int ldc = max (1, np); + + int ldak = max (1, n); + int ldbk = max (1, n); + int ldck = max (1, m); + int lddk = max (1, m); + + // arguments out + int nk; + Matrix ak (ldak, n); + Matrix bk (ldbk, np); + Matrix ck (ldck, n); + Matrix dk (lddk, np); + ColumnVector rcond (4); + + // workspace + int liwork = 2 * max (n, np+m); + int ldwork = 15*n*n + 6*n + + max (14*n+23, 16*n, 2*n+np+m, 3*(np+m)) + + max (n*n, 11*n*np + 2*m*m + 8*np*np + 8*m*n + 4*m*np + np); + + OCTAVE_LOCAL_BUFFER (int, iwork, liwork); + OCTAVE_LOCAL_BUFFER (double, dwork, ldwork); + OCTAVE_LOCAL_BUFFER (bool, bwork, 2*n); + + // error indicator + int info; + + + // SLICOT routine SB10KD + F77_XFCN (sb10kd, SB10KD, + (n, m, np, + a.fortran_vec (), lda, + b.fortran_vec (), ldb, + c.fortran_vec (), ldc, + factor, + ak.fortran_vec (), ldak, + bk.fortran_vec (), ldbk, + ck.fortran_vec (), ldck, + dk.fortran_vec (), lddk, + rcond.fortran_vec (), + iwork, + dwork, ldwork, + bwork, + info)); + + if (f77_exception_encountered) + error ("hinfsyn: slsb10kd: exception in SLICOT subroutine SB10KD"); + + if (info != 0) + error ("hinfsyn: slsb10kd: SB10KD returned info = %d", info); + + // resizing + ak.resize (n, n); + bk.resize (n, np); + ck.resize (m, n); + dk.resize (m, np); + + // return values + retval(0) = ak; + retval(1) = bk; + retval(2) = ck; + retval(3) = dk; + retval(4) = rcond; + } + + return retval; +}