SUBROUTINE SB10ED( N, M, NP, NCON, NMEAS, A, LDA, B, LDB, C, LDC,
$ D, LDD, AK, LDAK, BK, LDBK, CK, LDCK, DK, LDDK,
$ RCOND, TOL, IWORK, DWORK, LDWORK, BWORK, INFO )
C
C PURPOSE
C
C To compute the matrices of the H2 optimal n-state controller
C
C | AK | BK |
C K = |----|----|
C | CK | DK |
C
C for the discrete-time system
C
C | A | B1 B2 | | A | B |
C P = |----|---------| = |---|---| ,
C | C1 | 0 D12 | | C | D |
C | C2 | D21 D22 |
C
C where B2 has as column size the number of control inputs (NCON)
C and C2 has as row size the number of measurements (NMEAS) being
C provided to the controller.
C
C It is assumed that
C
C (A1) (A,B2) is stabilizable and (C2,A) is detectable,
C
C (A2) D12 is full column rank and D21 is full row rank,
C
C j*Theta
C (A3) | A-e *I B2 | has full column rank for all
C | C1 D12 |
C
C 0 <= Theta < 2*Pi ,
C
C
C j*Theta
C (A4) | A-e *I B1 | has full row rank for all
C | C2 D21 |
C
C 0 <= Theta < 2*Pi .
C
C ARGUMENTS
C
C Input/Output Parameters
C
C N (input) INTEGER
C The order of the system. 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 NCON (input) INTEGER
C The number of control inputs (M2). M >= NCON >= 0,
C NP-NMEAS >= NCON.
C
C NMEAS (input) INTEGER
C The number of measurements (NP2). NP >= NMEAS >= 0,
C M-NCON >= NMEAS.
C
C A (input/worksp.) DOUBLE PRECISION array, dimension (LDA,N)
C The leading N-by-N part of this array must contain the
C system state matrix A.
C This array is modified internally, but it is restored on
C exit.
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.
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.
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.
C
C LDD INTEGER
C The leading dimension of the array D. LDD >= max(1,NP).
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,NMEAS)
C The leading N-by-NMEAS 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 NCON-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.
C LDCK >= max(1,NCON).
C
C DK (output) DOUBLE PRECISION array, dimension (LDDK,NMEAS)
C The leading NCON-by-NMEAS part of this array contains the
C controller input/output matrix DK.
C
C LDDK INTEGER
C The leading dimension of the array DK.
C LDDK >= max(1,NCON).
C
C RCOND (output) DOUBLE PRECISION array, dimension (7)
C RCOND contains estimates the reciprocal condition
C numbers of the matrices which are to be inverted and the
C reciprocal condition numbers of the Riccati equations
C which have to be solved during the computation of the
C controller. (See the description of the algorithm in [2].)
C RCOND(1) contains the reciprocal condition number of the
C control transformation matrix TU;
C RCOND(2) contains the reciprocal condition number of the
C measurement transformation matrix TY;
C RCOND(3) contains the reciprocal condition number of the
C matrix Im2 + B2'*X2*B2;
C RCOND(4) contains the reciprocal condition number of the
C matrix Ip2 + C2*Y2*C2';
C RCOND(5) contains the reciprocal condition number of the
C X-Riccati equation;
C RCOND(6) contains the reciprocal condition number of the
C Y-Riccati equation;
C RCOND(7) contains the reciprocal condition number of the
C matrix Im2 + DKHAT*D22 .
C
C Tolerances
C
C TOL DOUBLE PRECISION
C Tolerance used for controlling the accuracy of the
C transformations applied for diagonalizing D12 and D21,
C and for checking the nonsingularity of the matrices to be
C inverted. If TOL <= 0, then a default value equal to
C sqrt(EPS) is used, where EPS is the relative machine
C precision.
C
C Workspace
C
C IWORK INTEGER array, dimension (max(2*M2,2*N,N*N,NP2))
C
C DWORK DOUBLE PRECISION array, dimension (LDWORK)
C On exit, if INFO = 0, DWORK(1) contains the optimal
C LDWORK.
C
C LDWORK INTEGER
C The dimension of the array DWORK.
C LDWORK >= N*M + NP*(N+M) + M2*M2 + NP2*NP2 +
C max(1,LW1,LW2,LW3,LW4,LW5,LW6), where
C LW1 = (N+NP1+1)*(N+M2) + max(3*(N+M2)+N+NP1,5*(N+M2)),
C LW2 = (N+NP2)*(N+M1+1) + max(3*(N+NP2)+N+M1,5*(N+NP2)),
C LW3 = M2 + NP1*NP1 + max(NP1*max(N,M1),3*M2+NP1,5*M2),
C LW4 = NP2 + M1*M1 + max(max(N,NP1)*M1,3*NP2+M1,5*NP2),
C LW5 = 2*N*N+max(1,14*N*N+6*N+max(14*N+23,16*N),M2*(N+M2+
C max(3,M1)),NP2*(N+NP2+3)),
C LW6 = max(N*M2,N*NP2,M2*NP2,M2*M2+4*M2),
C with M1 = M - M2 and NP1 = NP - NP2.
C For good performance, LDWORK must generally be larger.
C Denoting Q = max(M1,M2,NP1,NP2), an upper bound is
C 2*Q*(3*Q+2*N)+max(1,(N+Q)*(N+Q+6),Q*(Q+max(N,Q,5)+1),
C 2*N*N+max(1,14*N*N+6*N+max(14*N+23,16*N),
C Q*(N+Q+max(Q,3)))).
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 j*Theta
C = 1: if the matrix | A-e *I B2 | had not full
C | C1 D12 |
C column rank in respect to the tolerance EPS;
C j*Theta
C = 2: if the matrix | A-e *I B1 | had not full
C | C2 D21 |
C row rank in respect to the tolerance EPS;
C = 3: if the matrix D12 had not full column rank in
C respect to the tolerance TOL;
C = 4: if the matrix D21 had not full row rank in respect
C to the tolerance TOL;
C = 5: if the singular value decomposition (SVD) algorithm
C did not converge (when computing the SVD of one of
C the matrices |A-I B2 |, |A-I B1 |, D12 or D21).
C |C1 D12| |C2 D21|
C = 6: if the X-Riccati equation was not solved
C successfully;
C = 7: if the matrix Im2 + B2'*X2*B2 is not positive
C definite, or it is numerically singular (with
C respect to the tolerance TOL);
C = 8: if the Y-Riccati equation was not solved
C successfully;
C = 9: if the matrix Ip2 + C2*Y2*C2' is not positive
C definite, or it is numerically singular (with
C respect to the tolerance TOL);
C =10: if the matrix Im2 + DKHAT*D22 is singular, or its
C estimated condition number is larger than or equal
C to 1/TOL.
C
C METHOD
C
C The routine implements the formulas given in [1].
C
C REFERENCES
C
C [1] Zhou, K., Doyle, J.C., and Glover, K.
C Robust and Optimal Control.
C Prentice-Hall, Upper Saddle River, NJ, 1996.
C
C [2] Petkov, P.Hr., Gu, D.W., and Konstantinov, M.M.
C Fortran 77 routines for Hinf and H2 design of linear
C discrete-time control systems.
C Report 99-8, Department of Engineering, Leicester University,
C April 1999.
C
C NUMERICAL ASPECTS
C
C The accuracy of the result depends on the condition numbers of the
C matrices which are to be inverted and on the condition numbers of
C the matrix Riccati equations which are to be solved in the
C computation of the controller. (The corresponding reciprocal
C condition numbers are given in the output array RCOND.)
C
C CONTRIBUTORS
C
C P.Hr. Petkov, D.W. Gu and M.M. Konstantinov, May 1999.
C
C REVISIONS
C
C V. Sima, Research Institute for Informatics, Bucharest, May 1999,
C Sept. 1999, Feb. 2000, Nov. 2005.
C
C KEYWORDS
C
C Algebraic Riccati equation, H2 optimal control, optimal regulator,
C 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, NCON, NMEAS, NP
DOUBLE PRECISION TOL
C ..
C .. Array Arguments ..
INTEGER IWORK( * )
DOUBLE PRECISION A( LDA, * ), AK( LDAK, * ), B( LDB, * ),
$ BK( LDBK, * ), C( LDC, * ), CK( LDCK, * ),
$ D( LDD, * ), DK( LDDK, * ), DWORK( * ),
$ RCOND( * )
LOGICAL BWORK( * )
C ..
C .. Local Scalars ..
INTEGER I, INFO2, IWC, IWD, IWRK, IWTU, IWTY, IWX, IWY,
$ LW1, LW2, LW3, LW4, LW5, LW6, LWAMAX, M1, M2,
$ M2L, MINWRK, NL, NLP, NP1, NP2, NPL
DOUBLE PRECISION TOLL
C ..
C .. External Functions ..
DOUBLE PRECISION DLAMCH
EXTERNAL DLAMCH
C ..
C .. External Subroutines ..
EXTERNAL DLACPY, SB10PD, SB10SD, SB10TD, XERBLA
C ..
C .. Intrinsic Functions ..
INTRINSIC DBLE, INT, MAX, SQRT
C ..
C .. Executable Statements ..
C
C Decode and Test input parameters.
C
M1 = M - NCON
M2 = NCON
NP1 = NP - NMEAS
NP2 = NMEAS
NL = MAX( 1, N )
NPL = MAX( 1, NP )
M2L = MAX( 1, M2 )
NLP = MAX( 1, NP2 )
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( NCON.LT.0 .OR. M1.LT.0 .OR. M2.GT.NP1 ) THEN
INFO = -4
ELSE IF( NMEAS.LT.0 .OR. NP1.LT.0 .OR. NP2.GT.M1 ) THEN
INFO = -5
ELSE IF( LDA.LT.NL ) THEN
INFO = -7
ELSE IF( LDB.LT.NL ) THEN
INFO = -9
ELSE IF( LDC.LT.NPL ) THEN
INFO = -11
ELSE IF( LDD.LT.NPL ) THEN
INFO = -13
ELSE IF( LDAK.LT.NL ) THEN
INFO = -15
ELSE IF( LDBK.LT.NL ) THEN
INFO = -17
ELSE IF( LDCK.LT.M2L ) THEN
INFO = -19
ELSE IF( LDDK.LT.M2L ) THEN
INFO = -21
ELSE
C
C Compute workspace.
C
LW1 = ( N + NP1 + 1 )*( N + M2 ) + MAX( 3*( N + M2 ) + N + NP1,
$ 5*( N + M2 ) )
LW2 = ( N + NP2 )*( N + M1 + 1 ) + MAX( 3*( N + NP2 ) + N +
$ M1, 5*( N + NP2 ) )
LW3 = M2 + NP1*NP1 + MAX( NP1*MAX( N, M1 ), 3*M2 + NP1, 5*M2 )
LW4 = NP2 + M1*M1 + MAX( MAX( N, NP1 )*M1, 3*NP2 + M1, 5*NP2 )
LW5 = 2*N*N + MAX( 1, 14*N*N +
$ 6*N + MAX( 14*N + 23, 16*N ),
$ M2*( N + M2 + MAX( 3, M1 ) ),
$ NP2*( N + NP2 + 3 ) )
LW6 = MAX( N*M2, N*NP2, M2*NP2, M2*M2 + 4*M2 )
MINWRK = N*M + NP*( N + M ) + M2*M2 + NP2*NP2 +
$ MAX( 1, LW1, LW2, LW3, LW4, LW5, LW6 )
IF( LDWORK.LT.MINWRK )
$ INFO = -26
END IF
IF( INFO.NE.0 ) THEN
CALL XERBLA( 'SB10ED', -INFO )
RETURN
END IF
C
C Quick return if possible.
C
IF( N.EQ.0 .AND. MAX( M2, NP2 ).EQ.0 ) THEN
RCOND( 1 ) = ONE
RCOND( 2 ) = ONE
RCOND( 3 ) = ONE
RCOND( 4 ) = ONE
RCOND( 5 ) = ONE
RCOND( 6 ) = ONE
RCOND( 7 ) = ONE
DWORK( 1 ) = ONE
RETURN
END IF
C
TOLL = TOL
IF( TOLL.LE.ZERO ) THEN
C
C Set the default value of the tolerance for rank tests.
C
TOLL = SQRT( DLAMCH( 'Epsilon' ) )
END IF
C
C Workspace usage.
C
IWC = N*M + 1
IWD = IWC + NP*N
IWTU = IWD + NP*M
IWTY = IWTU + M2*M2
IWRK = IWTY + NP2*NP2
C
CALL DLACPY( 'Full', N, M, B, LDB, DWORK, NL )
CALL DLACPY( 'Full', NP, N, C, LDC, DWORK( IWC ), NPL )
CALL DLACPY( 'Full', NP, M, D, LDD, DWORK( IWD ), NPL )
C
C Transform the system so that D12 and D21 satisfy the formulas
C in the computation of the H2 optimal controller.
C Since SLICOT Library routine SB10PD performs the tests
C corresponding to the continuous-time counterparts of the
C assumptions (A3) and (A4), for the frequency w = 0, the
C next SB10PD routine call uses A - I.
C
DO 10 I = 1, N
A(I,I) = A(I,I) - ONE
10 CONTINUE
C
CALL SB10PD( N, M, NP, NCON, NMEAS, A, LDA, DWORK, NL,
$ DWORK( IWC ), NPL, DWORK( IWD ), NPL, DWORK( IWTU ),
$ M2L, DWORK( IWTY ), NLP, RCOND, TOLL, DWORK( IWRK ),
$ LDWORK-IWRK+1, INFO2 )
C
DO 20 I = 1, N
A(I,I) = A(I,I) + ONE
20 CONTINUE
C
IF( INFO2.GT.0 ) THEN
INFO = INFO2
RETURN
END IF
LWAMAX = INT( DWORK( IWRK ) ) + IWRK - 1
C
IWX = IWRK
IWY = IWX + N*N
IWRK = IWY + N*N
C
C Compute the optimal H2 controller for the normalized system.
C
CALL SB10SD( N, M, NP, NCON, NMEAS, A, LDA, DWORK, NL,
$ DWORK( IWC ), NPL, DWORK( IWD ), NPL, AK, LDAK, BK,
$ LDBK, CK, LDCK, DK, LDDK, DWORK( IWX ), NL,
$ DWORK( IWY ), NL, RCOND( 3 ), TOLL, IWORK,
$ DWORK( IWRK ), LDWORK-IWRK+1, BWORK, INFO2 )
IF( INFO2.GT.0 ) THEN
INFO = INFO2 + 5
RETURN
END IF
LWAMAX = MAX( INT( DWORK( IWRK ) ) + IWRK - 1, LWAMAX )
C
IWRK = IWX
C
C Compute the H2 optimal controller for the original system.
C
CALL SB10TD( N, M, NP, NCON, NMEAS, DWORK( IWD ), NPL,
$ DWORK( IWTU ), M2L, DWORK( IWTY ), NLP, AK, LDAK, BK,
$ LDBK, CK, LDCK, DK, LDDK, RCOND( 7 ), TOLL, IWORK,
$ DWORK( IWRK ), LDWORK-IWRK+1, INFO2 )
IF( INFO2.GT.0 ) THEN
INFO = 10
RETURN
END IF
C
DWORK( 1 ) = DBLE( LWAMAX )
RETURN
C *** Last line of SB10ED ***
END