Mercurial > forge
changeset 8628:465605bbfc4d octave-forge
control: add draft code
| author | paramaniac |
|---|---|
| date | Fri, 21 Oct 2011 17:05:35 +0000 |
| parents | f4b4df7e8881 |
| children | 5f12f86814ce |
| files | main/control/devel/ctrbf.m main/control/devel/ctrbf/MB01PD.f main/control/devel/ctrbf/MB01QD.f main/control/devel/ctrbf/MB03OY.f main/control/devel/ctrbf/TB01UD.dat main/control/devel/ctrbf/TB01UD.f main/control/devel/ctrbf/TB01UD.res main/control/devel/ctrbf/common.cc main/control/devel/ctrbf/makefile_ctrbf.m main/control/devel/ctrbf/sltb01ud.cc main/control/devel/ctrbf/test_ctrbf.m |
| diffstat | 11 files changed, 1796 insertions(+), 1 deletions(-) [+] |
line wrap: on
line diff
--- a/main/control/devel/ctrbf.m Fri Oct 21 17:00:59 2011 +0000 +++ b/main/control/devel/ctrbf.m Fri Oct 21 17:05:35 2011 +0000 @@ -19,7 +19,7 @@ ## @deftypefnx{Function File} {[@var{Abar}, @var{Bbar}, @var{Cbar}, @var{T}, @var{K}] =} ctrbf (@var{A}, @var{B}, @var{C}, @var{TOL}) ## If Co=ctrb(A,B) has rank r <= n = SIZE(A,1), then there is a ## similarity transformation Tc such that Tc = [t1 t2] where t1 -## is the controlable subspace and t2 is orthogonal to t1 +## is the controllable subspace and t2 is orthogonal to t1 ## ## @example ## @group
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ctrbf/MB01PD.f Fri Oct 21 17:05:35 2011 +0000 @@ -0,0 +1,271 @@ + SUBROUTINE MB01PD( SCUN, TYPE, M, N, KL, KU, ANRM, NBL, NROWS, A, + $ LDA, 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 scale a matrix or undo scaling. Scaling is performed, if +C necessary, so that the matrix norm will be in a safe range of +C representable numbers. +C +C ARGUMENTS +C +C Mode Parameters +C +C SCUN CHARACTER*1 +C SCUN indicates the operation to be performed. +C = 'S': scale the matrix. +C = 'U': undo scaling of the matrix. +C +C TYPE CHARACTER*1 +C TYPE indicates the storage type of the input matrix. +C = 'G': A is a full matrix. +C = 'L': A is a (block) lower triangular matrix. +C = 'U': A is an (block) upper triangular matrix. +C = 'H': A is an (block) upper Hessenberg matrix. +C = 'B': A is a symmetric band matrix with lower bandwidth +C KL and upper bandwidth KU and with the only the +C lower half stored. +C = 'Q': A is a symmetric band matrix with lower bandwidth +C KL and upper bandwidth KU and with the only the +C upper half stored. +C = 'Z': A is a band matrix with lower bandwidth KL and +C upper bandwidth KU. +C +C Input/Output Parameters +C +C M (input) INTEGER +C The number of rows of the matrix A. M >= 0. +C +C N (input) INTEGER +C The number of columns of the matrix A. N >= 0. +C +C KL (input) INTEGER +C The lower bandwidth of A. Referenced only if TYPE = 'B', +C 'Q' or 'Z'. +C +C KU (input) INTEGER +C The upper bandwidth of A. Referenced only if TYPE = 'B', +C 'Q' or 'Z'. +C +C ANRM (input) DOUBLE PRECISION +C The norm of the initial matrix A. ANRM >= 0. +C When ANRM = 0 then an immediate return is effected. +C ANRM should be preserved between the call of the routine +C with SCUN = 'S' and the corresponding one with SCUN = 'U'. +C +C NBL (input) INTEGER +C The number of diagonal blocks of the matrix A, if it has a +C block structure. To specify that matrix A has no block +C structure, set NBL = 0. NBL >= 0. +C +C NROWS (input) INTEGER array, dimension max(1,NBL) +C NROWS(i) contains the number of rows and columns of the +C i-th diagonal block of matrix A. The sum of the values +C NROWS(i), for i = 1: NBL, should be equal to min(M,N). +C The elements of the array NROWS are not referenced if +C NBL = 0. +C +C A (input/output) DOUBLE PRECISION array, dimension (LDA,N) +C On entry, the leading M by N part of this array must +C contain the matrix to be scaled/unscaled. +C On exit, the leading M by N part of A will contain +C the modified matrix. +C The storage mode of A is specified by TYPE. +C +C LDA (input) INTEGER +C The leading dimension of the array A. LDA >= max(1,M). +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 +C METHOD +C +C Denote by ANRM the norm of the matrix, and by SMLNUM and BIGNUM, +C two positive numbers near the smallest and largest safely +C representable numbers, respectively. The matrix is scaled, if +C needed, such that the norm of the result is in the range +C [SMLNUM, BIGNUM]. The scaling factor is represented as a ratio +C of two numbers, one of them being ANRM, and the other one either +C SMLNUM or BIGNUM, depending on ANRM being less than SMLNUM or +C larger than BIGNUM, respectively. For undoing the scaling, the +C norm is again compared with SMLNUM or BIGNUM, and the reciprocal +C of the previous scaling factor is used. +C +C CONTRIBUTOR +C +C V. Sima, Katholieke Univ. Leuven, Belgium, Nov. 1996. +C +C REVISIONS +C +C Oct. 2001, V. Sima, Research Institute for Informatics, Bucharest. +C +C ****************************************************************** +C +C .. Parameters .. + DOUBLE PRECISION ZERO, ONE + PARAMETER ( ZERO = 0.0D0, ONE = 1.0D0 ) +C .. Scalar Arguments .. + CHARACTER SCUN, TYPE + INTEGER INFO, KL, KU, LDA, M, MN, N, NBL + DOUBLE PRECISION ANRM +C .. Array Arguments .. + INTEGER NROWS ( * ) + DOUBLE PRECISION A( LDA, * ) +C .. Local Scalars .. + LOGICAL FIRST, LSCALE + INTEGER I, ISUM, ITYPE + DOUBLE PRECISION BIGNUM, SMLNUM +C .. External Functions .. + LOGICAL LSAME + DOUBLE PRECISION DLAMCH + EXTERNAL DLAMCH, LSAME +C .. +C .. External Subroutines .. + EXTERNAL DLABAD, MB01QD, XERBLA +C .. Intrinsic Functions .. + INTRINSIC MAX, MIN +C .. Save statement .. + SAVE BIGNUM, FIRST, SMLNUM +C .. Data statements .. + DATA FIRST/.TRUE./ +C .. +C .. Executable Statements .. +C +C Test the input scalar arguments. +C + INFO = 0 + LSCALE = LSAME( SCUN, 'S' ) + IF( LSAME( TYPE, 'G' ) ) THEN + ITYPE = 0 + ELSE IF( LSAME( TYPE, 'L' ) ) THEN + ITYPE = 1 + ELSE IF( LSAME( TYPE, 'U' ) ) THEN + ITYPE = 2 + ELSE IF( LSAME( TYPE, 'H' ) ) THEN + ITYPE = 3 + ELSE IF( LSAME( TYPE, 'B' ) ) THEN + ITYPE = 4 + ELSE IF( LSAME( TYPE, 'Q' ) ) THEN + ITYPE = 5 + ELSE IF( LSAME( TYPE, 'Z' ) ) THEN + ITYPE = 6 + ELSE + ITYPE = -1 + END IF +C + MN = MIN( M, N ) +C + ISUM = 0 + IF( NBL.GT.0 ) THEN + DO 10 I = 1, NBL + ISUM = ISUM + NROWS(I) + 10 CONTINUE + END IF +C + IF( .NOT.LSCALE .AND. .NOT.LSAME( SCUN, 'U' ) ) THEN + INFO = -1 + ELSE IF( ITYPE.EQ.-1 ) THEN + INFO = -2 + ELSE IF( M.LT.0 ) THEN + INFO = -3 + ELSE IF( N.LT.0 .OR. + $ ( ( ITYPE.EQ.4 .OR. ITYPE.EQ.5 ) .AND. N.NE.M ) ) THEN + INFO = -4 + ELSE IF( ANRM.LT.ZERO ) THEN + INFO = -7 + ELSE IF( NBL.LT.0 ) THEN + INFO = -8 + ELSE IF( NBL.GT.0 .AND. ISUM.NE.MN ) THEN + INFO = -9 + ELSE IF( ITYPE.LE.3 .AND. LDA.LT.MAX( 1, M ) ) THEN + INFO = -11 + ELSE IF( ITYPE.GE.4 ) THEN + IF( KL.LT.0 .OR. KL.GT.MAX( M-1, 0 ) ) THEN + INFO = -5 + ELSE IF( KU.LT.0 .OR. KU.GT.MAX( N-1, 0 ) .OR. + $ ( ( ITYPE.EQ.4 .OR. ITYPE.EQ.5 ) .AND. KL.NE.KU ) ) + $ THEN + INFO = -6 + ELSE IF( ( ITYPE.EQ.4 .AND. LDA.LT.KL+1 ) .OR. + $ ( ITYPE.EQ.5 .AND. LDA.LT.KU+1 ) .OR. + $ ( ITYPE.EQ.6 .AND. LDA.LT.2*KL+KU+1 ) ) THEN + INFO = -11 + END IF + END IF +C + IF( INFO.NE.0 ) THEN + CALL XERBLA( 'MB01PD', -INFO ) + RETURN + END IF +C +C Quick return if possible. +C + IF( MN.EQ.0 .OR. ANRM.EQ.ZERO ) + $ RETURN +C + IF ( FIRST ) THEN +C +C Get machine parameters. +C + SMLNUM = DLAMCH( 'S' ) / DLAMCH( 'P' ) + BIGNUM = ONE / SMLNUM + CALL DLABAD( SMLNUM, BIGNUM ) + FIRST = .FALSE. + END IF +C + IF ( LSCALE ) THEN +C +C Scale A, if its norm is outside range [SMLNUM,BIGNUM]. +C + IF( ANRM.LT.SMLNUM ) THEN +C +C Scale matrix norm up to SMLNUM. +C + CALL MB01QD( TYPE, M, N, KL, KU, ANRM, SMLNUM, NBL, NROWS, + $ A, LDA, INFO ) + ELSE IF( ANRM.GT.BIGNUM ) THEN +C +C Scale matrix norm down to BIGNUM. +C + CALL MB01QD( TYPE, M, N, KL, KU, ANRM, BIGNUM, NBL, NROWS, + $ A, LDA, INFO ) + END IF +C + ELSE +C +C Undo scaling. +C + IF( ANRM.LT.SMLNUM ) THEN + CALL MB01QD( TYPE, M, N, KL, KU, SMLNUM, ANRM, NBL, NROWS, + $ A, LDA, INFO ) + ELSE IF( ANRM.GT.BIGNUM ) THEN + CALL MB01QD( TYPE, M, N, KL, KU, BIGNUM, ANRM, NBL, NROWS, + $ A, LDA, INFO ) + END IF + END IF +C + RETURN +C *** Last line of MB01PD *** + END
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ctrbf/MB01QD.f Fri Oct 21 17:05:35 2011 +0000 @@ -0,0 +1,334 @@ + SUBROUTINE MB01QD( TYPE, M, N, KL, KU, CFROM, CTO, NBL, NROWS, A, + $ LDA, 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 multiply the M by N real matrix A by the real scalar CTO/CFROM. +C This is done without over/underflow as long as the final result +C CTO*A(I,J)/CFROM does not over/underflow. TYPE specifies that +C A may be full, (block) upper triangular, (block) lower triangular, +C (block) upper Hessenberg, or banded. +C +C ARGUMENTS +C +C Mode Parameters +C +C TYPE CHARACTER*1 +C TYPE indices the storage type of the input matrix. +C = 'G': A is a full matrix. +C = 'L': A is a (block) lower triangular matrix. +C = 'U': A is a (block) upper triangular matrix. +C = 'H': A is a (block) upper Hessenberg matrix. +C = 'B': A is a symmetric band matrix with lower bandwidth +C KL and upper bandwidth KU and with the only the +C lower half stored. +C = 'Q': A is a symmetric band matrix with lower bandwidth +C KL and upper bandwidth KU and with the only the +C upper half stored. +C = 'Z': A is a band matrix with lower bandwidth KL and +C upper bandwidth KU. +C +C Input/Output Parameters +C +C M (input) INTEGER +C The number of rows of the matrix A. M >= 0. +C +C N (input) INTEGER +C The number of columns of the matrix A. N >= 0. +C +C KL (input) INTEGER +C The lower bandwidth of A. Referenced only if TYPE = 'B', +C 'Q' or 'Z'. +C +C KU (input) INTEGER +C The upper bandwidth of A. Referenced only if TYPE = 'B', +C 'Q' or 'Z'. +C +C CFROM (input) DOUBLE PRECISION +C CTO (input) DOUBLE PRECISION +C The matrix A is multiplied by CTO/CFROM. A(I,J) is +C computed without over/underflow if the final result +C CTO*A(I,J)/CFROM can be represented without over/ +C underflow. CFROM must be nonzero. +C +C NBL (input) INTEGER +C The number of diagonal blocks of the matrix A, if it has a +C block structure. To specify that matrix A has no block +C structure, set NBL = 0. NBL >= 0. +C +C NROWS (input) INTEGER array, dimension max(1,NBL) +C NROWS(i) contains the number of rows and columns of the +C i-th diagonal block of matrix A. The sum of the values +C NROWS(i), for i = 1: NBL, should be equal to min(M,N). +C The array NROWS is not referenced if NBL = 0. +C +C A (input/output) DOUBLE PRECISION array, dimension (LDA,N) +C The matrix to be multiplied by CTO/CFROM. See TYPE for +C the storage type. +C +C LDA (input) INTEGER +C The leading dimension of the array A. LDA >= max(1,M). +C +C Error Indicator +C +C INFO INTEGER +C Not used in this implementation. +C +C METHOD +C +C Matrix A is multiplied by the real scalar CTO/CFROM, taking into +C account the specified storage mode of the matrix. +C MB01QD is a version of the LAPACK routine DLASCL, modified for +C dealing with block triangular, or block Hessenberg matrices. +C For efficiency, no tests of the input scalar parameters are +C performed. +C +C CONTRIBUTOR +C +C V. Sima, Katholieke Univ. Leuven, Belgium, Nov. 1996. +C +C ****************************************************************** +C +C .. Parameters .. + DOUBLE PRECISION ZERO, ONE + PARAMETER ( ZERO = 0.0D0, ONE = 1.0D0 ) +C .. +C .. Scalar Arguments .. + CHARACTER TYPE + INTEGER INFO, KL, KU, LDA, M, N, NBL + DOUBLE PRECISION CFROM, CTO +C .. +C .. Array Arguments .. + INTEGER NROWS ( * ) + DOUBLE PRECISION A( LDA, * ) +C .. +C .. Local Scalars .. + LOGICAL DONE, NOBLC + INTEGER I, IFIN, ITYPE, J, JFIN, JINI, K, K1, K2, K3, + $ K4 + DOUBLE PRECISION BIGNUM, CFROM1, CFROMC, CTO1, CTOC, MUL, SMLNUM +C .. +C .. External Functions .. + LOGICAL LSAME + DOUBLE PRECISION DLAMCH + EXTERNAL LSAME, DLAMCH +C .. +C .. Intrinsic Functions .. + INTRINSIC ABS, MAX, MIN +C .. +C .. Executable Statements .. +C + IF( LSAME( TYPE, 'G' ) ) THEN + ITYPE = 0 + ELSE IF( LSAME( TYPE, 'L' ) ) THEN + ITYPE = 1 + ELSE IF( LSAME( TYPE, 'U' ) ) THEN + ITYPE = 2 + ELSE IF( LSAME( TYPE, 'H' ) ) THEN + ITYPE = 3 + ELSE IF( LSAME( TYPE, 'B' ) ) THEN + ITYPE = 4 + ELSE IF( LSAME( TYPE, 'Q' ) ) THEN + ITYPE = 5 + ELSE + ITYPE = 6 + END IF +C +C Quick return if possible. +C + IF( MIN( M, N ).EQ.0 ) + $ RETURN +C +C Get machine parameters. +C + SMLNUM = DLAMCH( 'S' ) + BIGNUM = ONE / SMLNUM +C + CFROMC = CFROM + CTOC = CTO +C + 10 CONTINUE + CFROM1 = CFROMC*SMLNUM + CTO1 = CTOC / BIGNUM + IF( ABS( CFROM1 ).GT.ABS( CTOC ) .AND. CTOC.NE.ZERO ) THEN + MUL = SMLNUM + DONE = .FALSE. + CFROMC = CFROM1 + ELSE IF( ABS( CTO1 ).GT.ABS( CFROMC ) ) THEN + MUL = BIGNUM + DONE = .FALSE. + CTOC = CTO1 + ELSE + MUL = CTOC / CFROMC + DONE = .TRUE. + END IF +C + NOBLC = NBL.EQ.0 +C + IF( ITYPE.EQ.0 ) THEN +C +C Full matrix +C + DO 30 J = 1, N + DO 20 I = 1, M + A( I, J ) = A( I, J )*MUL + 20 CONTINUE + 30 CONTINUE +C + ELSE IF( ITYPE.EQ.1 ) THEN +C + IF ( NOBLC ) THEN +C +C Lower triangular matrix +C + DO 50 J = 1, N + DO 40 I = J, M + A( I, J ) = A( I, J )*MUL + 40 CONTINUE + 50 CONTINUE +C + ELSE +C +C Block lower triangular matrix +C + JFIN = 0 + DO 80 K = 1, NBL + JINI = JFIN + 1 + JFIN = JFIN + NROWS( K ) + DO 70 J = JINI, JFIN + DO 60 I = JINI, M + A( I, J ) = A( I, J )*MUL + 60 CONTINUE + 70 CONTINUE + 80 CONTINUE + END IF +C + ELSE IF( ITYPE.EQ.2 ) THEN +C + IF ( NOBLC ) THEN +C +C Upper triangular matrix +C + DO 100 J = 1, N + DO 90 I = 1, MIN( J, M ) + A( I, J ) = A( I, J )*MUL + 90 CONTINUE + 100 CONTINUE +C + ELSE +C +C Block upper triangular matrix +C + JFIN = 0 + DO 130 K = 1, NBL + JINI = JFIN + 1 + JFIN = JFIN + NROWS( K ) + IF ( K.EQ.NBL ) JFIN = N + DO 120 J = JINI, JFIN + DO 110 I = 1, MIN( JFIN, M ) + A( I, J ) = A( I, J )*MUL + 110 CONTINUE + 120 CONTINUE + 130 CONTINUE + END IF +C + ELSE IF( ITYPE.EQ.3 ) THEN +C + IF ( NOBLC ) THEN +C +C Upper Hessenberg matrix +C + DO 150 J = 1, N + DO 140 I = 1, MIN( J+1, M ) + A( I, J ) = A( I, J )*MUL + 140 CONTINUE + 150 CONTINUE +C + ELSE +C +C Block upper Hessenberg matrix +C + JFIN = 0 + DO 180 K = 1, NBL + JINI = JFIN + 1 + JFIN = JFIN + NROWS( K ) +C + IF ( K.EQ.NBL ) THEN + JFIN = N + IFIN = N + ELSE + IFIN = JFIN + NROWS( K+1 ) + END IF +C + DO 170 J = JINI, JFIN + DO 160 I = 1, MIN( IFIN, M ) + A( I, J ) = A( I, J )*MUL + 160 CONTINUE + 170 CONTINUE + 180 CONTINUE + END IF +C + ELSE IF( ITYPE.EQ.4 ) THEN +C +C Lower half of a symmetric band matrix +C + K3 = KL + 1 + K4 = N + 1 + DO 200 J = 1, N + DO 190 I = 1, MIN( K3, K4-J ) + A( I, J ) = A( I, J )*MUL + 190 CONTINUE + 200 CONTINUE +C + ELSE IF( ITYPE.EQ.5 ) THEN +C +C Upper half of a symmetric band matrix +C + K1 = KU + 2 + K3 = KU + 1 + DO 220 J = 1, N + DO 210 I = MAX( K1-J, 1 ), K3 + A( I, J ) = A( I, J )*MUL + 210 CONTINUE + 220 CONTINUE +C + ELSE IF( ITYPE.EQ.6 ) THEN +C +C Band matrix +C + K1 = KL + KU + 2 + K2 = KL + 1 + K3 = 2*KL + KU + 1 + K4 = KL + KU + 1 + M + DO 240 J = 1, N + DO 230 I = MAX( K1-J, K2 ), MIN( K3, K4-J ) + A( I, J ) = A( I, J )*MUL + 230 CONTINUE + 240 CONTINUE +C + END IF +C + IF( .NOT.DONE ) + $ GO TO 10 +C + RETURN +C *** Last line of MB01QD *** + END
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ctrbf/MB03OY.f Fri Oct 21 17:05:35 2011 +0000 @@ -0,0 +1,388 @@ + SUBROUTINE MB03OY( M, N, A, LDA, RCOND, SVLMAX, RANK, SVAL, JPVT, + $ TAU, DWORK, 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 a rank-revealing QR factorization of a real general +C M-by-N matrix A, which may be rank-deficient, and estimate its +C effective rank using incremental condition estimation. +C +C The routine uses a truncated QR factorization with column pivoting +C [ R11 R12 ] +C A * P = Q * R, where R = [ ], +C [ 0 R22 ] +C with R11 defined as the largest leading upper triangular submatrix +C whose estimated condition number is less than 1/RCOND. The order +C of R11, RANK, is the effective rank of A. Condition estimation is +C performed during the QR factorization process. Matrix R22 is full +C (but of small norm), or empty. +C +C MB03OY does not perform any scaling of the matrix A. +C +C ARGUMENTS +C +C Input/Output Parameters +C +C M (input) INTEGER +C The number of rows of the matrix A. M >= 0. +C +C N (input) INTEGER +C The number of columns of the matrix A. N >= 0. +C +C A (input/output) DOUBLE PRECISION array, dimension +C ( LDA, N ) +C On entry, the leading M-by-N part of this array must +C contain the given matrix A. +C On exit, the leading RANK-by-RANK upper triangular part +C of A contains the triangular factor R11, and the elements +C below the diagonal in the first RANK columns, with the +C array TAU, represent the orthogonal matrix Q as a product +C of RANK elementary reflectors. +C The remaining N-RANK columns contain the result of the +C QR factorization process used. +C +C LDA INTEGER +C The leading dimension of the array A. LDA >= max(1,M). +C +C RCOND (input) DOUBLE PRECISION +C RCOND is used to determine the effective rank of A, which +C is defined as the order of the largest leading triangular +C submatrix R11 in the QR factorization with pivoting of A, +C whose estimated condition number is less than 1/RCOND. +C 0 <= RCOND <= 1. +C NOTE that when SVLMAX > 0, the estimated rank could be +C less than that defined above (see SVLMAX). +C +C SVLMAX (input) DOUBLE PRECISION +C If A is a submatrix of another matrix B, and the rank +C decision should be related to that matrix, then SVLMAX +C should be an estimate of the largest singular value of B +C (for instance, the Frobenius norm of B). If this is not +C the case, the input value SVLMAX = 0 should work. +C SVLMAX >= 0. +C +C RANK (output) INTEGER +C The effective (estimated) rank of A, i.e., the order of +C the submatrix R11. +C +C SVAL (output) DOUBLE PRECISION array, dimension ( 3 ) +C The estimates of some of the singular values of the +C triangular factor R: +C SVAL(1): largest singular value of R(1:RANK,1:RANK); +C SVAL(2): smallest singular value of R(1:RANK,1:RANK); +C SVAL(3): smallest singular value of R(1:RANK+1,1:RANK+1), +C if RANK < MIN( M, N ), or of R(1:RANK,1:RANK), +C otherwise. +C If the triangular factorization is a rank-revealing one +C (which will be the case if the leading columns were well- +C conditioned), then SVAL(1) will also be an estimate for +C the largest singular value of A, and SVAL(2) and SVAL(3) +C will be estimates for the RANK-th and (RANK+1)-st singular +C values of A, respectively. +C By examining these values, one can confirm that the rank +C is well defined with respect to the chosen value of RCOND. +C The ratio SVAL(1)/SVAL(2) is an estimate of the condition +C number of R(1:RANK,1:RANK). +C +C JPVT (output) INTEGER array, dimension ( N ) +C If JPVT(i) = k, then the i-th column of A*P was the k-th +C column of A. +C +C TAU (output) DOUBLE PRECISION array, dimension ( MIN( M, N ) ) +C The leading RANK elements of TAU contain the scalar +C factors of the elementary reflectors. +C +C Workspace +C +C DWORK DOUBLE PRECISION array, dimension ( 3*N-1 ) +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 +C METHOD +C +C The routine computes a truncated QR factorization with column +C pivoting of A, A * P = Q * R, with R defined above, and, +C during this process, finds the largest leading submatrix whose +C estimated condition number is less than 1/RCOND, taking the +C possible positive value of SVLMAX into account. This is performed +C using the LAPACK incremental condition estimation scheme and a +C slightly modified rank decision test. The factorization process +C stops when RANK has been determined. +C +C The matrix Q is represented as a product of elementary reflectors +C +C Q = H(1) H(2) . . . H(k), where k = rank <= min(m,n). +C +C Each H(i) has the form +C +C H = I - tau * v * v' +C +C where tau is a real scalar, and v is a real vector with +C v(1:i-1) = 0 and v(i) = 1; v(i+1:m) is stored on exit in +C A(i+1:m,i), and tau in TAU(i). +C +C The matrix P is represented in jpvt as follows: If +C jpvt(j) = i +C then the jth column of P is the ith canonical unit vector. +C +C REFERENCES +C +C [1] Bischof, C.H. and P. Tang. +C Generalizing Incremental Condition Estimation. +C LAPACK Working Notes 32, Mathematics and Computer Science +C Division, Argonne National Laboratory, UT, CS-91-132, +C May 1991. +C +C [2] Bischof, C.H. and P. Tang. +C Robust Incremental Condition Estimation. +C LAPACK Working Notes 33, Mathematics and Computer Science +C Division, Argonne National Laboratory, UT, CS-91-133, +C May 1991. +C +C NUMERICAL ASPECTS +C +C The algorithm is backward stable. +C +C CONTRIBUTOR +C +C V. Sima, Katholieke Univ. Leuven, Belgium, Feb. 1998. +C +C REVISIONS +C +C V. Sima, Research Institute for Informatics, Bucharest, Jan. 2009. +C V. Sima, Jan. 2010, following Bujanovic and Drmac's suggestion. +C +C KEYWORDS +C +C Eigenvalue problem, matrix operations, orthogonal transformation, +C singular values. +C +C ****************************************************************** +C +C .. Parameters .. + INTEGER IMAX, IMIN + PARAMETER ( IMAX = 1, IMIN = 2 ) + DOUBLE PRECISION ZERO, ONE + PARAMETER ( ZERO = 0.0D0, ONE = 1.0D0 ) +C .. Scalar Arguments .. + INTEGER INFO, LDA, M, N, RANK + DOUBLE PRECISION RCOND, SVLMAX +C .. Array Arguments .. + INTEGER JPVT( * ) + DOUBLE PRECISION A( LDA, * ), DWORK( * ), SVAL( 3 ), TAU( * ) +C .. +C .. Local Scalars .. + INTEGER I, ISMAX, ISMIN, ITEMP, J, MN, PVT + DOUBLE PRECISION AII, C1, C2, S1, S2, SMAX, SMAXPR, SMIN, + $ SMINPR, TEMP, TEMP2, TOLZ +C .. +C .. External Functions .. + INTEGER IDAMAX + DOUBLE PRECISION DLAMCH, DNRM2 + EXTERNAL DLAMCH, DNRM2, IDAMAX +C .. External Subroutines .. + EXTERNAL DLAIC1, DLARF, DLARFG, DSCAL, DSWAP, XERBLA +C .. Intrinsic Functions .. + INTRINSIC ABS, MAX, MIN, SQRT +C .. +C .. Executable Statements .. +C +C Test the input scalar arguments. +C + INFO = 0 + IF( M.LT.0 ) THEN + INFO = -1 + ELSE IF( N.LT.0 ) THEN + INFO = -2 + ELSE IF( LDA.LT.MAX( 1, M ) ) THEN + INFO = -4 + ELSE IF( RCOND.LT.ZERO .OR. RCOND.GT.ONE ) THEN + INFO = -5 + ELSE IF( SVLMAX.LT.ZERO ) THEN + INFO = -6 + END IF +C + IF( INFO.NE.0 ) THEN + CALL XERBLA( 'MB03OY', -INFO ) + RETURN + END IF +C +C Quick return if possible. +C + MN = MIN( M, N ) + IF( MN.EQ.0 ) THEN + RANK = 0 + SVAL( 1 ) = ZERO + SVAL( 2 ) = ZERO + SVAL( 3 ) = ZERO + RETURN + END IF +C + TOLZ = SQRT( DLAMCH( 'Epsilon' ) ) + ISMIN = 1 + ISMAX = ISMIN + N +C +C Initialize partial column norms and pivoting vector. The first n +C elements of DWORK store the exact column norms. The already used +C leading part is then overwritten by the condition estimator. +C + DO 10 I = 1, N + DWORK( I ) = DNRM2( M, A( 1, I ), 1 ) + DWORK( N+I ) = DWORK( I ) + JPVT( I ) = I + 10 CONTINUE +C +C Compute factorization and determine RANK using incremental +C condition estimation. +C + RANK = 0 +C + 20 CONTINUE + IF( RANK.LT.MN ) THEN + I = RANK + 1 +C +C Determine ith pivot column and swap if necessary. +C + PVT = ( I-1 ) + IDAMAX( N-I+1, DWORK( I ), 1 ) +C + IF( PVT.NE.I ) THEN + CALL DSWAP( M, A( 1, PVT ), 1, A( 1, I ), 1 ) + ITEMP = JPVT( PVT ) + JPVT( PVT ) = JPVT( I ) + JPVT( I ) = ITEMP + DWORK( PVT ) = DWORK( I ) + DWORK( N+PVT ) = DWORK( N+I ) + END IF +C +C Save A(I,I) and generate elementary reflector H(i). +C + IF( I.LT.M ) THEN + AII = A( I, I ) + CALL DLARFG( M-I+1, A( I, I ), A( I+1, I ), 1, TAU( I ) ) + ELSE + TAU( M ) = ZERO + END IF +C + IF( RANK.EQ.0 ) THEN +C +C Initialize; exit if matrix is zero (RANK = 0). +C + SMAX = ABS( A( 1, 1 ) ) + IF ( SMAX.EQ.ZERO ) THEN + SVAL( 1 ) = ZERO + SVAL( 2 ) = ZERO + SVAL( 3 ) = ZERO + RETURN + END IF + SMIN = SMAX + SMAXPR = SMAX + SMINPR = SMIN + C1 = ONE + C2 = ONE + ELSE +C +C One step of incremental condition estimation. +C + CALL DLAIC1( IMIN, RANK, DWORK( ISMIN ), SMIN, A( 1, I ), + $ A( I, I ), SMINPR, S1, C1 ) + CALL DLAIC1( IMAX, RANK, DWORK( ISMAX ), SMAX, A( 1, I ), + $ A( I, I ), SMAXPR, S2, C2 ) + END IF +C + IF( SVLMAX*RCOND.LE.SMAXPR ) THEN + IF( SVLMAX*RCOND.LE.SMINPR ) THEN + IF( SMAXPR*RCOND.LE.SMINPR ) THEN +C +C Continue factorization, as rank is at least RANK. +C + IF( I.LT.N ) THEN +C +C Apply H(i) to A(i:m,i+1:n) from the left. +C + AII = A( I, I ) + A( I, I ) = ONE + CALL DLARF( 'Left', M-I+1, N-I, A( I, I ), 1, + $ TAU( I ), A( I, I+1 ), LDA, + $ DWORK( 2*N+1 ) ) + A( I, I ) = AII + END IF +C +C Update partial column norms. +C + DO 30 J = I + 1, N + IF( DWORK( J ).NE.ZERO ) THEN + TEMP = ABS( A( I, J ) ) / DWORK( J ) + TEMP = MAX( ( ONE + TEMP )*( ONE - TEMP ), ZERO) + TEMP2 = TEMP*( DWORK( J ) / DWORK( N+J ) )**2 + IF( TEMP2.LE.TOLZ ) THEN + IF( M-I.GT.0 ) THEN + DWORK( J ) = DNRM2( M-I, A( I+1, J ), 1 ) + DWORK( N+J ) = DWORK( J ) + ELSE + DWORK( J ) = ZERO + DWORK( N+J ) = ZERO + END IF + ELSE + DWORK( J ) = DWORK( J )*SQRT( TEMP ) + END IF + END IF + 30 CONTINUE +C + DO 40 I = 1, RANK + DWORK( ISMIN+I-1 ) = S1*DWORK( ISMIN+I-1 ) + DWORK( ISMAX+I-1 ) = S2*DWORK( ISMAX+I-1 ) + 40 CONTINUE +C + DWORK( ISMIN+RANK ) = C1 + DWORK( ISMAX+RANK ) = C2 + SMIN = SMINPR + SMAX = SMAXPR + RANK = RANK + 1 + GO TO 20 + END IF + END IF + END IF + END IF +C +C Restore the changed part of the (RANK+1)-th column and set SVAL. +C + IF ( RANK.LT.N ) THEN + IF ( I.LT.M ) THEN + CALL DSCAL( M-I, -A( I, I )*TAU( I ), A( I+1, I ), 1 ) + A( I, I ) = AII + END IF + END IF + IF ( RANK.EQ.0 ) THEN + SMIN = ZERO + SMINPR = ZERO + END IF + SVAL( 1 ) = SMAX + SVAL( 2 ) = SMIN + SVAL( 3 ) = SMINPR +C + RETURN +C *** Last line of MB03OY *** + END
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ctrbf/TB01UD.dat Fri Oct 21 17:05:35 2011 +0000 @@ -0,0 +1,9 @@ + TB01UD EXAMPLE PROGRAM DATA + 3 2 2 0.0 I + -1.0 0.0 0.0 + -2.0 -2.0 -2.0 + -1.0 0.0 -3.0 + 1.0 0.0 0.0 + 0.0 2.0 1.0 + 0.0 2.0 1.0 + 1.0 0.0 0.0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ctrbf/TB01UD.f Fri Oct 21 17:05:35 2011 +0000 @@ -0,0 +1,491 @@ + SUBROUTINE TB01UD( JOBZ, N, M, P, A, LDA, B, LDB, C, LDC, NCONT, + $ INDCON, NBLK, Z, LDZ, TAU, 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 find a controllable realization for the linear time-invariant +C multi-input system +C +C dX/dt = A * X + B * U, +C Y = C * X, +C +C where A, B, and C are N-by-N, N-by-M, and P-by-N matrices, +C respectively, and A and B are reduced by this routine to +C orthogonal canonical form using (and optionally accumulating) +C orthogonal similarity transformations, which are also applied +C to C. Specifically, the system (A, B, C) is reduced to the +C triplet (Ac, Bc, Cc), where Ac = Z' * A * Z, Bc = Z' * B, +C Cc = C * Z, with +C +C [ Acont * ] [ Bcont ] +C Ac = [ ], Bc = [ ], +C [ 0 Auncont ] [ 0 ] +C +C and +C +C [ A11 A12 . . . A1,p-1 A1p ] [ B1 ] +C [ A21 A22 . . . A2,p-1 A2p ] [ 0 ] +C [ 0 A32 . . . A3,p-1 A3p ] [ 0 ] +C Acont = [ . . . . . . . ], Bc = [ . ], +C [ . . . . . . ] [ . ] +C [ . . . . . ] [ . ] +C [ 0 0 . . . Ap,p-1 App ] [ 0 ] +C +C where the blocks B1, A21, ..., Ap,p-1 have full row ranks and +C p is the controllability index of the pair. The size of the +C block Auncont is equal to the dimension of the uncontrollable +C subspace of the pair (A, B). +C +C ARGUMENTS +C +C Mode Parameters +C +C JOBZ CHARACTER*1 +C Indicates whether the user wishes to accumulate in a +C matrix Z the orthogonal similarity transformations for +C reducing the system, as follows: +C = 'N': Do not form Z and do not store the orthogonal +C transformations; +C = 'F': Do not form Z, but store the orthogonal +C transformations in the factored form; +C = 'I': Z is initialized to the unit matrix and the +C orthogonal transformation matrix Z is returned. +C +C Input/Output Parameters +C +C N (input) INTEGER +C The order of the original state-space representation, +C i.e. the order of the matrix A. N >= 0. +C +C M (input) INTEGER +C The number of system inputs, or of columns of B. M >= 0. +C +C P (input) INTEGER +C The number of system outputs, or of rows of C. P >= 0. +C +C A (input/output) DOUBLE PRECISION array, dimension (LDA,N) +C On entry, the leading N-by-N part of this array must +C contain the original state dynamics matrix A. +C On exit, the leading NCONT-by-NCONT part contains the +C upper block Hessenberg state dynamics matrix Acont in Ac, +C given by Z' * A * Z, of a controllable realization for +C the original system. The elements below the first block- +C subdiagonal are set to zero. The leading N-by-N part +C contains the matrix Ac. +C +C LDA INTEGER +C The leading dimension of array A. LDA >= MAX(1,N). +C +C B (input/output) DOUBLE PRECISION array, dimension (LDB,M) +C On entry, the leading N-by-M part of this array must +C contain the input matrix B. +C On exit, the leading NCONT-by-M part of this array +C contains the transformed input matrix Bcont in Bc, given +C by Z' * B, with all elements but the first block set to +C zero. The leading N-by-M part contains the matrix Bc. +C +C LDB INTEGER +C The leading dimension of array B. LDB >= MAX(1,N). +C +C C (input/output) DOUBLE PRECISION array, dimension (LDC,N) +C On entry, the leading P-by-N part of this array must +C contain the output matrix C. +C On exit, the leading P-by-N part of this array contains +C the transformed output matrix Cc, given by C * Z. +C +C LDC INTEGER +C The leading dimension of array C. LDC >= MAX(1,P). +C +C NCONT (output) INTEGER +C The order of the controllable state-space representation. +C +C INDCON (output) INTEGER +C The controllability index of the controllable part of the +C system representation. +C +C NBLK (output) INTEGER array, dimension (N) +C The leading INDCON elements of this array contain the +C the orders of the diagonal blocks of Acont. +C +C Z (output) DOUBLE PRECISION array, dimension (LDZ,N) +C If JOBZ = 'I', then the leading N-by-N part of this +C array contains the matrix of accumulated orthogonal +C similarity transformations which reduces the given system +C to orthogonal canonical form. +C If JOBZ = 'F', the elements below the diagonal, with the +C array TAU, represent the orthogonal transformation matrix +C as a product of elementary reflectors. The transformation +C matrix can then be obtained by calling the LAPACK Library +C routine DORGQR. +C If JOBZ = 'N', the array Z is not referenced and can be +C supplied as a dummy array (i.e. set parameter LDZ = 1 and +C declare this array to be Z(1,1) in the calling program). +C +C LDZ INTEGER +C The leading dimension of array Z. If JOBZ = 'I' or +C JOBZ = 'F', LDZ >= MAX(1,N); if JOBZ = 'N', LDZ >= 1. +C +C TAU (output) DOUBLE PRECISION array, dimension (N) +C The elements of TAU contain the scalar factors of the +C elementary reflectors used in the reduction of B and A. +C +C Tolerances +C +C TOL DOUBLE PRECISION +C The tolerance to be used in rank determination when +C transforming (A, B). If the user sets TOL > 0, then +C the given value of TOL is used as a lower bound for the +C reciprocal condition number (see the description of the +C argument RCOND in the SLICOT routine MB03OD); a +C (sub)matrix whose estimated condition number is less than +C 1/TOL is considered to be of full rank. If the user sets +C TOL <= 0, then an implicitly computed, default tolerance, +C defined by TOLDEF = N*N*EPS, is used instead, where EPS +C is the machine precision (see LAPACK Library routine +C DLAMCH). +C +C Workspace +C +C IWORK INTEGER array, dimension (M) +C +C DWORK DOUBLE PRECISION array, dimension (LDWORK) +C On exit, if INFO = 0, DWORK(1) returns the optimal value +C of LDWORK. +C +C LDWORK INTEGER +C The length of the array DWORK. +C LDWORK >= MAX(1, N, 3*M, P). +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 +C METHOD +C +C Matrix B is first QR-decomposed and the appropriate orthogonal +C similarity transformation applied to the matrix A. Leaving the +C first rank(B) states unchanged, the remaining lower left block +C of A is then QR-decomposed and the new orthogonal matrix, Q1, +C is also applied to the right of A to complete the similarity +C transformation. By continuing in this manner, a completely +C controllable state-space pair (Acont, Bcont) is found for the +C given (A, B), where Acont is upper block Hessenberg with each +C subdiagonal block of full row rank, and Bcont is zero apart from +C its (independent) first rank(B) rows. +C All orthogonal transformations determined in this process are also +C applied to the matrix C, from the right. +C NOTE that the system controllability indices are easily +C calculated from the dimensions of the blocks of Acont. +C +C REFERENCES +C +C [1] Konstantinov, M.M., Petkov, P.Hr. and Christov, N.D. +C Orthogonal Invariants and Canonical Forms for Linear +C Controllable Systems. +C Proc. 8th IFAC World Congress, Kyoto, 1, pp. 49-54, 1981. +C +C [2] Paige, C.C. +C Properties of numerical algorithms related to computing +C controllablity. +C IEEE Trans. Auto. Contr., AC-26, pp. 130-138, 1981. +C +C [3] Petkov, P.Hr., Konstantinov, M.M., Gu, D.W. and +C Postlethwaite, I. +C Optimal Pole Assignment Design of Linear Multi-Input Systems. +C Leicester University, Report 99-11, May 1996. +C +C NUMERICAL ASPECTS +C 3 +C The algorithm requires 0(N ) operations and is backward stable. +C +C FURTHER COMMENTS +C +C If the system matrices A and B are badly scaled, it would be +C useful to scale them with SLICOT routine TB01ID, before calling +C the routine. +C +C CONTRIBUTOR +C +C V. Sima, Katholieke Univ. Leuven, Belgium, Feb. 1998. +C +C REVISIONS +C +C V. Sima, Katholieke Univ. Leuven, Belgium, May 1999, Nov. 2003. +C A. Varga, DLR Oberpfaffenhofen, March 2002, Nov. 2003. +C +C KEYWORDS +C +C Controllability, minimal realization, orthogonal canonical form, +C orthogonal transformation. +C +C ****************************************************************** +C +C .. Parameters .. + DOUBLE PRECISION ZERO, ONE + PARAMETER ( ZERO = 0.0D0, ONE = 1.0D0 ) +C .. Scalar Arguments .. + CHARACTER JOBZ + INTEGER INDCON, INFO, LDA, LDB, LDC, LDWORK, LDZ, M, N, + $ NCONT, P + DOUBLE PRECISION TOL +C .. Array Arguments .. + DOUBLE PRECISION A(LDA,*), B(LDB,*), C(LDC,*), DWORK(*), TAU(*), + $ Z(LDZ,*) + INTEGER IWORK(*), NBLK(*) +C .. Local Scalars .. + LOGICAL LJOBF, LJOBI, LJOBZ + INTEGER IQR, ITAU, J, MCRT, NBL, NCRT, NI, NJ, RANK, + $ WRKOPT + DOUBLE PRECISION ANORM, BNORM, FNRM, TOLDEF +C .. Local Arrays .. + DOUBLE PRECISION SVAL(3) +C .. External Functions .. + LOGICAL LSAME + DOUBLE PRECISION DLAMCH, DLANGE, DLAPY2 + EXTERNAL DLAMCH, DLANGE, DLAPY2, LSAME +C .. External Subroutines .. + EXTERNAL DCOPY, DLACPY, DLAPMT, DLASET, DORGQR, DORMQR, + $ MB01PD, MB03OY, XERBLA +C .. Intrinsic Functions .. + INTRINSIC DBLE, INT, MAX, MIN +C .. +C .. Executable Statements .. +C + INFO = 0 + LJOBF = LSAME( JOBZ, 'F' ) + LJOBI = LSAME( JOBZ, 'I' ) + LJOBZ = LJOBF.OR.LJOBI +C +C Test the input scalar arguments. +C + IF( .NOT.LJOBZ .AND. .NOT.LSAME( JOBZ, 'N' ) ) THEN + INFO = -1 + ELSE IF( N.LT.0 ) THEN + INFO = -2 + ELSE IF( M.LT.0 ) THEN + INFO = -3 + ELSE IF( P.LT.0 ) THEN + INFO = -4 + ELSE IF( LDA.LT.MAX( 1, N ) ) THEN + INFO = -6 + ELSE IF( LDB.LT.MAX( 1, N ) ) THEN + INFO = -8 + ELSE IF( LDC.LT.MAX( 1, P ) ) THEN + INFO = -10 + ELSE IF( .NOT.LJOBZ .AND. LDZ.LT.1 .OR. + $ LJOBZ .AND. LDZ.LT.MAX( 1, N ) ) THEN + INFO = -15 + ELSE IF( LDWORK.LT.MAX( 1, N, 3*M, P ) ) THEN + INFO = -20 + END IF +C + IF ( INFO.NE.0 ) THEN +C +C Error return. +C + CALL XERBLA( 'TB01UD', -INFO ) + RETURN + END IF +C + NCONT = 0 + INDCON = 0 +C +C Calculate the absolute norms of A and B (used for scaling). +C + ANORM = DLANGE( 'M', N, N, A, LDA, DWORK ) + BNORM = DLANGE( 'M', N, M, B, LDB, DWORK ) +C +C Quick return if possible. +C + IF ( MIN( N, M ).EQ.0 .OR. BNORM.EQ.ZERO ) THEN + IF( N.GT.0 ) THEN + IF ( LJOBI ) THEN + CALL DLASET( 'Full', N, N, ZERO, ONE, Z, LDZ ) + ELSE IF ( LJOBF ) THEN + CALL DLASET( 'Full', N, N, ZERO, ZERO, Z, LDZ ) + CALL DLASET( 'Full', N, 1, ZERO, ZERO, TAU, N ) + END IF + END IF + DWORK(1) = ONE + RETURN + END IF +C +C Scale (if needed) the matrices A and B. +C + CALL MB01PD( 'S', 'G', N, N, 0, 0, ANORM, 0, NBLK, A, LDA, INFO ) + CALL MB01PD( 'S', 'G', N, M, 0, 0, BNORM, 0, NBLK, B, LDB, INFO ) +C +C Compute the Frobenius norm of [ B A ] (used for rank estimation). +C + FNRM = DLAPY2( DLANGE( 'F', N, M, B, LDB, DWORK ), + $ DLANGE( 'F', N, N, A, LDA, DWORK ) ) +C + TOLDEF = TOL + IF ( TOLDEF.LE.ZERO ) THEN +C +C Use the default tolerance in controllability determination. +C + TOLDEF = DBLE( N*N )*DLAMCH( 'EPSILON' ) + END IF +C + IF ( FNRM.LT.TOLDEF ) + $ FNRM = ONE +C + WRKOPT = 1 + NI = 0 + ITAU = 1 + NCRT = N + MCRT = M + IQR = 1 +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 + 10 CONTINUE +C +C Rank-revealing QR decomposition with column pivoting. +C The calculation is performed in NCRT rows of B starting from +C the row IQR (initialized to 1 and then set to rank(B)+1). +C Workspace: 3*MCRT. +C + CALL MB03OY( NCRT, MCRT, B(IQR,1), LDB, TOLDEF, FNRM, RANK, + $ SVAL, IWORK, TAU(ITAU), DWORK, INFO ) +C + IF ( RANK.NE.0 ) THEN + NJ = NI + NI = NCONT + NCONT = NCONT + RANK + INDCON = INDCON + 1 + NBLK(INDCON) = RANK +C +C Premultiply and postmultiply the appropriate block row +C and block column of A by Q' and Q, respectively. +C Workspace: need NCRT; +C prefer NCRT*NB. +C + CALL DORMQR( 'Left', 'Transpose', NCRT, NCRT, RANK, + $ B(IQR,1), LDB, TAU(ITAU), A(NI+1,NI+1), LDA, + $ DWORK, LDWORK, INFO ) + WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) ) +C +C Workspace: need N; +C prefer N*NB. +C + CALL DORMQR( 'Right', 'No transpose', N, NCRT, RANK, + $ B(IQR,1), LDB, TAU(ITAU), A(1,NI+1), LDA, + $ DWORK, LDWORK, INFO ) + WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) ) +C +C Postmultiply the appropriate block column of C by Q. +C Workspace: need P; +C prefer P*NB. +C + CALL DORMQR( 'Right', 'No transpose', P, NCRT, RANK, + $ B(IQR,1), LDB, TAU(ITAU), C(1,NI+1), LDC, + $ DWORK, LDWORK, INFO ) + WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) ) +C +C If required, save transformations. +C + IF ( LJOBZ.AND.NCRT.GT.1 ) THEN + CALL DLACPY( 'L', NCRT-1, MIN( RANK, NCRT-1 ), + $ B(IQR+1,1), LDB, Z(NI+2,ITAU), LDZ ) + END IF +C +C Zero the subdiagonal elements of the current matrix. +C + IF ( RANK.GT.1 ) + $ CALL DLASET( 'L', RANK-1, RANK-1, ZERO, ZERO, B(IQR+1,1), + $ LDB ) +C +C Backward permutation of the columns of B or A. +C + IF ( INDCON.EQ.1 ) THEN + CALL DLAPMT( .FALSE., RANK, M, B(IQR,1), LDB, IWORK ) + IQR = RANK + 1 + ELSE + DO 20 J = 1, MCRT + CALL DCOPY( RANK, B(IQR,J), 1, A(NI+1,NJ+IWORK(J)), + $ 1 ) + 20 CONTINUE + END IF +C + ITAU = ITAU + RANK + IF ( RANK.NE.NCRT ) THEN + MCRT = RANK + NCRT = NCRT - RANK + CALL DLACPY( 'G', NCRT, MCRT, A(NCONT+1,NI+1), LDA, + $ B(IQR,1), LDB ) + CALL DLASET( 'G', NCRT, MCRT, ZERO, ZERO, + $ A(NCONT+1,NI+1), LDA ) + GO TO 10 + END IF + END IF +C +C If required, accumulate transformations. +C Workspace: need N; prefer N*NB. +C + IF ( LJOBI ) THEN + CALL DORGQR( N, N, ITAU-1, Z, LDZ, TAU, DWORK, + $ LDWORK, INFO ) + WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) ) + END IF +C +C Annihilate the trailing blocks of B. +C + IF( IQR.LE.N ) + $ CALL DLASET( 'G', N-IQR+1, M, ZERO, ZERO, B(IQR,1), LDB ) +C +C Annihilate the trailing elements of TAU, if JOBZ = 'F'. +C + IF ( LJOBF ) THEN + DO 30 J = ITAU, N + TAU(J) = ZERO + 30 CONTINUE + END IF +C +C Undo scaling of A and B. +C + IF ( INDCON.LT.N ) THEN + NBL = INDCON + 1 + NBLK(NBL) = N - NCONT + ELSE + NBL = 0 + END IF + CALL MB01PD( 'U', 'H', N, N, 0, 0, ANORM, NBL, NBLK, A, LDA, + $ INFO ) + CALL MB01PD( 'U', 'G', NBLK(1), M, 0, 0, BNORM, 0, NBLK, B, LDB, + $ INFO ) +C +C Set optimal workspace dimension. +C + DWORK(1) = WRKOPT + RETURN +C *** Last line of TB01UD *** + END
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ctrbf/TB01UD.res Fri Oct 21 17:05:35 2011 +0000 @@ -0,0 +1,25 @@ + TB01UD EXAMPLE PROGRAM RESULTS + + The order of the controllable state-space representation = 2 + + The transformed state dynamics matrix of a controllable realization is + -3.0000 2.2361 + 0.0000 -1.0000 + + and the dimensions of its diagonal blocks are + 2 + + The transformed input/state matrix B of a controllable realization is + 0.0000 -2.2361 + 1.0000 0.0000 + + The transformed output/state matrix C of a controllable realization is + -2.2361 0.0000 + 0.0000 1.0000 + + The controllability index of the transformed system representation = 1 + + The similarity transformation matrix Z is + 0.0000 1.0000 0.0000 + -0.8944 0.0000 -0.4472 + -0.4472 0.0000 0.8944
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ctrbf/common.cc Fri Oct 21 17:05:35 2011 +0000 @@ -0,0 +1,53 @@ +/* + +Copyright (C) 2010 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/>. + +Common code for oct-files. + +Author: Lukas Reichlin <lukas.reichlin@gmail.com> +Created: April 2010 +Version: 0.1 + +*/ + + +int max (int a, int b) +{ + if (a > b) + return a; + else + return b; +} + +int max (int a, int b, int c) +{ + return max (max (a, b), c); +} + +int max (int a, int b, int c, int d) +{ + return max (max (a, b), max (c, d)); +} + +int min (int a, int b) +{ + if (a < b) + return a; + else + return b; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ctrbf/makefile_ctrbf.m Fri Oct 21 17:05:35 2011 +0000 @@ -0,0 +1,2 @@ +mkoctfile sltb01ud.cc \ + TB01UD.f MB01PD.f MB03OY.f MB01QD.f \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ctrbf/sltb01ud.cc Fri Oct 21 17:05:35 2011 +0000 @@ -0,0 +1,140 @@ +/* + +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/>. + +Orthogonal canonical form. +Uses SLICOT TB01UD by courtesy of NICONET e.V. +<http://www.slicot.org> + +Author: Lukas Reichlin <lukas.reichlin@gmail.com> +Created: October 2011 +Version: 0.1 + +*/ + +#include <octave/oct.h> +#include <f77-fcn.h> +#include "common.cc" + +extern "C" +{ + int F77_FUNC (tb01ud, TB01UD) + (char& JOBZ, + int& N, int& M, int& P, + double* A, int& LDA, + double* B, int& LDB, + double* C, int& LDC, + int& NCONT, int& INDCON, + int* NBLK, + double* Z, int& LDZ, + double* TAU, + double& TOL, + int* IWORK, + double* DWORK, int& LDWORK, + int& INFO); +} + +DEFUN_DLD (sltb01ud, args, nargout, + "-*- texinfo -*-\n\ +Slicot TB01UD 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 + char jobz = 'I'; + + Matrix a = args(0).matrix_value (); + Matrix b = args(1).matrix_value (); + Matrix c = args(2).matrix_value (); + double tol = args(3).double_value (); + + int n = a.rows (); // n: number of states + int m = b.columns (); // m: number of inputs + int p = c.rows (); // p: number of outputs + + int lda = max (1, n); + int ldb = max (1, n); + int ldc = max (1, p); + int ldz = max (1, n); + + // arguments out + Matrix z (ldz, n); + + int ncont; + int indcon; + + OCTAVE_LOCAL_BUFFER (int, nblk, n); + OCTAVE_LOCAL_BUFFER (double, tau, n); + + // workspace + int ldwork = max (1, n, 3*m, p); + + OCTAVE_LOCAL_BUFFER (int, iwork, m); + OCTAVE_LOCAL_BUFFER (double, dwork, ldwork); + + // error indicators + int info; + + + // SLICOT routine TB01UD + F77_XFCN (tb01ud, TB01UD, + (jobz, + n, m, p, + a.fortran_vec (), lda, + b.fortran_vec (), ldb, + c.fortran_vec (), ldc, + ncont, indcon, + nblk, + z.fortran_vec (), ldz, + tau, + tol, + iwork, + dwork, ldwork, + info)); + + if (f77_exception_encountered) + error ("sltb01ud: exception in SLICOT subroutine TB01UD"); + + if (info != 0) + error ("sltb01ud: TB01UD returned info = %d", info); + + // resize + a.resize (n, n); + b.resize (n, m); + c.resize (p, n); + z.resize (n, n); + + // return values + retval(0) = a; + retval(1) = b; + retval(2) = c; + retval(3) = z; + retval(4) = octave_value (ncont); + } + + return retval; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/control/devel/ctrbf/test_ctrbf.m Fri Oct 21 17:05:35 2011 +0000 @@ -0,0 +1,82 @@ +a = [ -1.0 0.0 0.0 + -2.0 -2.0 -2.0 + -1.0 0.0 -3.0 ]; + +b = [ 1.0 0.0 0.0 + 0.0 2.0 1.0 ].'; + + +c = [ 0.0 2.0 1.0 + 1.0 0.0 0.0 ]; + + +[ac, bc, cc, z, ncont] = sltb01ud (a, b, c, 0.0) + +a = [ 1 1 + 4 -2 ]; + +b = [ 1 -1 + 1 -1 ]; + +c = [ 1 0 + 0 1 ]; + +[ac, bc, cc, z, ncont] = sltb01ud (a, b, c, 0.0) + +%{ + The transformed state dynamics matrix of a controllable realization is + -3.0000 2.2361 + 0.0000 -1.0000 + + and the dimensions of its diagonal blocks are + 2 + + The transformed input/state matrix B of a controllable realization is + 0.0000 -2.2361 + 1.0000 0.0000 + + The transformed output/state matrix C of a controllable realization is + -2.2361 0.0000 + 0.0000 1.0000 + + The controllability index of the transformed system representation = 1 + + The similarity transformation matrix Z is + 0.0000 1.0000 0.0000 + -0.8944 0.0000 -0.4472 + -0.4472 0.0000 0.8944 +%} +%{ +A = + 1 1 + 4 -2 + +B = + 1 -1 + 1 -1 + +C = + 1 0 + 0 1 +and locate the uncontrollable mode. + +[Abar,Bbar,Cbar,T,k]=ctrbf(A,B,C) + +Abar = + -3.0000 0 + -3.0000 2.0000 + +Bbar = + 0.0000 0.0000 + 1.4142 -1.4142 + +Cbar = + -0.7071 0.7071 + 0.7071 0.7071 + +T = + -0.7071 0.7071 + 0.7071 0.7071 +k = + 1 0 +%} \ No newline at end of file