SUBROUTINE SB10AD( JOB, N, M, NP, NCON, NMEAS, GAMMA, A, LDA,
$ B, LDB, C, LDC, D, LDD, AK, LDAK, BK, LDBK, CK,
$ LDCK, DK, LDDK, AC, LDAC, BC, LDBC, CC, LDCC,
$ DC, LDDC, RCOND, GTOL, ACTOL, IWORK, LIWORK,
$ DWORK, LDWORK, BWORK, LBWORK, INFO )
C
C PURPOSE
C
C To compute the matrices of an H-infinity optimal n-state
C controller
C
C | AK | BK |
C K = |----|----|,
C | CK | DK |
C
C using modified Glover's and Doyle's 1988 formulas, for the system
C
C | A | B1 B2 | | A | B |
C P = |----|---------| = |---|---|
C | C1 | D11 D12 | | C | D |
C | C2 | D21 D22 |
C
C and for the estimated minimal possible value of gamma with respect
C to GTOL, where B2 has as column size the number of control inputs
C (NCON) and C2 has as row size the number of measurements (NMEAS)
C being provided to the controller, and then to compute the matrices
C of the closed-loop system
C
C | AC | BC |
C G = |----|----|,
C | CC | DC |
C
C if the stabilizing controller exists.
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 (A3) | A-j*omega*I B2 | has full column rank for all omega,
C | C1 D12 |
C
C (A4) | A-j*omega*I B1 | has full row rank for all omega.
C | C2 D21 |
C
C ARGUMENTS
C
C Input/Output Parameters
C
C JOB (input) INTEGER
C Indicates the strategy for reducing the GAMMA value, as
C follows:
C = 1: Use bisection method for decreasing GAMMA from GAMMA
C to GAMMAMIN until the closed-loop system leaves
C stability.
C = 2: Scan from GAMMA to 0 trying to find the minimal GAMMA
C for which the closed-loop system retains stability.
C = 3: First bisection, then scanning.
C = 4: Find suboptimal controller only.
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 GAMMA (input/output) DOUBLE PRECISION
C The initial value of gamma on input. It is assumed that
C gamma is sufficiently large so that the controller is
C admissible. GAMMA >= 0.
C On output it contains the minimal estimated gamma.
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.
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 AC (output) DOUBLE PRECISION array, dimension (LDAC,2*N)
C The leading 2*N-by-2*N part of this array contains the
C closed-loop system state matrix AC.
C
C LDAC INTEGER
C The leading dimension of the array AC.
C LDAC >= max(1,2*N).
C
C BC (output) DOUBLE PRECISION array, dimension (LDBC,M-NCON)
C The leading 2*N-by-(M-NCON) part of this array contains
C the closed-loop system input matrix BC.
C
C LDBC INTEGER
C The leading dimension of the array BC.
C LDBC >= max(1,2*N).
C
C CC (output) DOUBLE PRECISION array, dimension (LDCC,2*N)
C The leading (NP-NMEAS)-by-2*N part of this array contains
C the closed-loop system output matrix CC.
C
C LDCC INTEGER
C The leading dimension of the array CC.
C LDCC >= max(1,NP-NMEAS).
C
C DC (output) DOUBLE PRECISION array, dimension (LDDC,M-NCON)
C The leading (NP-NMEAS)-by-(M-NCON) part of this array
C contains the closed-loop system input/output matrix DC.
C
C LDDC INTEGER
C The leading dimension of the array DC.
C LDDC >= max(1,NP-NMEAS).
C
C RCOND (output) DOUBLE PRECISION array, dimension (4)
C For the last successful step:
C RCOND(1) contains the reciprocal condition number of the
C control transformation matrix;
C RCOND(2) contains the reciprocal condition number of the
C measurement transformation matrix;
C RCOND(3) contains an estimate of the reciprocal condition
C number of the X-Riccati equation;
C RCOND(4) contains an estimate of the reciprocal condition
C number of the Y-Riccati equation.
C
C Tolerances
C
C GTOL DOUBLE PRECISION
C Tolerance used for controlling the accuracy of GAMMA
C and its distance to the estimated minimal possible
C value of GAMMA.
C If GTOL <= 0, then a default value equal to sqrt(EPS)
C is used, where EPS is the relative machine precision.
C
C ACTOL DOUBLE PRECISION
C Upper bound for the poles of the closed-loop system
C used for determining if it is stable.
C ACTOL <= 0 for stable systems.
C
C Workspace
C
C IWORK INTEGER array, dimension (LIWORK)
C
C LIWORK INTEGER
C The dimension of the array IWORK.
C LIWORK >= max(2*max(N,M-NCON,NP-NMEAS,NCON,NMEAS),N*N)
C
C DWORK DOUBLE PRECISION array, dimension (LDWORK)
C On exit, if INFO = 0, DWORK(1) contains the optimal
C value of LDWORK.
C
C LDWORK INTEGER
C The dimension of the array DWORK.
C LDWORK >= LW1 + max(1,LW2,LW3,LW4,LW5 + MAX(LW6,LW7)),
C where
C LW1 = N*M + NP*N + NP*M + M2*M2 + NP2*NP2;
C LW2 = max( ( N + NP1 + 1 )*( N + M2 ) +
C max( 3*( N + M2 ) + N + NP1, 5*( N + M2 ) ),
C ( N + NP2 )*( N + M1 + 1 ) +
C max( 3*( N + NP2 ) + N + M1, 5*( N + NP2 ) ),
C M2 + NP1*NP1 + max( NP1*max( N, M1 ),
C 3*M2 + NP1, 5*M2 ),
C NP2 + M1*M1 + max( max( N, NP1 )*M1,
C 3*NP2 + M1, 5*NP2 ) );
C LW3 = max( ND1*M1 + max( 4*min( ND1, M1 ) + max( ND1,M1 ),
C 6*min( ND1, M1 ) ),
C NP1*ND2 + max( 4*min( NP1, ND2 ) +
C max( NP1,ND2 ),
C 6*min( NP1, ND2 ) ) );
C LW4 = 2*M*M + NP*NP + 2*M*N + M*NP + 2*N*NP;
C LW5 = 2*N*N + M*N + N*NP;
C LW6 = max( M*M + max( 2*M1, 3*N*N +
C max( N*M, 10*N*N + 12*N + 5 ) ),
C NP*NP + max( 2*NP1, 3*N*N +
C max( N*NP, 10*N*N + 12*N + 5 ) ));
C LW7 = M2*NP2 + NP2*NP2 + M2*M2 +
C max( ND1*ND1 + max( 2*ND1, ( ND1 + ND2 )*NP2 ),
C ND2*ND2 + max( 2*ND2, ND2*M2 ), 3*N,
C N*( 2*NP2 + M2 ) +
C max( 2*N*M2, M2*NP2 +
C max( M2*M2 + 3*M2, NP2*( 2*NP2 +
C M2 + max( NP2, N ) ) ) ) );
C M1 = M - M2, NP1 = NP - NP2,
C ND1 = NP1 - M2, ND2 = M1 - NP2.
C For good performance, LDWORK must generally be larger.
C
C BWORK LOGICAL array, dimension (LBWORK)
C
C LBWORK INTEGER
C The dimension of the array BWORK. LBWORK >= 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 matrix | A-j*omega*I B2 | had not full
C | C1 D12 |
C column rank in respect to the tolerance EPS;
C = 2: if the matrix | A-j*omega*I B1 | had not full row
C | C2 D21 |
C rank in respect to the tolerance EPS;
C = 3: if the matrix D12 had not full column rank in
C respect to the tolerance SQRT(EPS);
C = 4: if the matrix D21 had not full row rank in respect
C to the tolerance SQRT(EPS);
C = 5: if the singular value decomposition (SVD) algorithm
C did not converge (when computing the SVD of one of
C the matrices |A B2 |, |A B1 |, D12 or D21);
C |C1 D12| |C2 D21|
C = 6: if the controller is not admissible (too small value
C of gamma);
C = 7: if the X-Riccati equation was not solved
C successfully (the controller is not admissible or
C there are numerical difficulties);
C = 8: if the Y-Riccati equation was not solved
C successfully (the controller is not admissible or
C there are numerical difficulties);
C = 9: if the determinant of Im2 + Tu*D11HAT*Ty*D22 is
C zero [3];
C = 10: if there are numerical problems when estimating
C singular values of D1111, D1112, D1111', D1121';
C = 11: if the matrices Inp2 - D22*DK or Im2 - DK*D22
C are singular to working precision;
C = 12: if a stabilizing controller cannot be found.
C
C METHOD
C
C The routine implements the Glover's and Doyle's 1988 formulas [1],
C [2], modified to improve the efficiency as described in [3].
C
C JOB = 1: It tries with a decreasing value of GAMMA, starting with
C the given, and with the newly obtained controller estimates of the
C closed-loop system. If it is stable, (i.e., max(eig(AC)) < ACTOL)
C the iterations can be continued until the given tolerance between
C GAMMA and the estimated GAMMAMIN is reached. Otherwise, in the
C next step GAMMA is increased. The step in the all next iterations
C is step = step/2. The closed-loop system is obtained by the
C formulas given in [2].
C
C JOB = 2: The same as for JOB = 1, but with non-varying step till
C GAMMA = 0, step = max(0.1, GTOL).
C
C JOB = 3: Combines the JOB = 1 and JOB = 2 cases for a quicker
C procedure.
C
C JOB = 4: Suboptimal controller for current GAMMA only.
C
C REFERENCES
C
C [1] Glover, K. and Doyle, J.C.
C State-space formulae for all stabilizing controllers that
C satisfy an Hinf norm bound and relations to risk sensitivity.
C Systems and Control Letters, vol. 11, pp. 167-172, 1988.
C
C [2] Balas, G.J., Doyle, J.C., Glover, K., Packard, A., and
C Smith, R.
C mu-Analysis and Synthesis Toolbox.
C The MathWorks Inc., Natick, MA, 1995.
C
C [3] Petkov, P.Hr., Gu, D.W., and Konstantinov, M.M.
C Fortran 77 routines for Hinf and H2 design of continuous-time
C linear control systems.
C Rep. 98-14, Department of Engineering, Leicester University,
C Leicester, U.K., 1998.
C
C NUMERICAL ASPECTS
C
C The accuracy of the result depends on the condition numbers of the
C input and output transformations and on the condition numbers of
C the two Riccati equations, as given by the values of RCOND(1),
C RCOND(2), RCOND(3) and RCOND(4), respectively.
C This approach by estimating the closed-loop system and checking
C its poles seems to be reliable.
C
C CONTRIBUTORS
C
C A. Markovski, P.Hr. Petkov, D.W. Gu and M.M. Konstantinov,
C July 2003.
C
C REVISIONS
C
C V. Sima, Research Institute for Informatics, Bucharest, Aug. 2003.
C
C KEYWORDS
C
C Algebraic Riccati equation, H-infinity optimal control, robust
C control.
C
C ******************************************************************
C
C .. Parameters ..
DOUBLE PRECISION ZERO, ONE, TWO, P1, THOUS
PARAMETER ( ZERO = 0.0D+0, ONE = 1.0D+0, TWO = 2.0D+0,
$ P1 = 0.1D+0, THOUS = 1.0D+3 )
C ..
C .. Scalar Arguments ..
INTEGER INFO, JOB, LBWORK, LDA, LDAC, LDAK, LDB, LDBC,
$ LDBK, LDC, LDCC, LDCK, LDD, LDDC, LDDK, LDWORK,
$ LIWORK, M, N, NCON, NMEAS, NP
DOUBLE PRECISION ACTOL, GAMMA, GTOL
C ..
C .. Array Arguments ..
LOGICAL BWORK( * )
INTEGER IWORK( * )
DOUBLE PRECISION A( LDA, * ), AC( LDAC, * ), AK( LDAK, * ),
$ B( LDB, * ), BC( LDBC, * ), BK( LDBK, * ),
$ C( LDC, * ), CC( LDCC, * ), CK( LDCK, * ),
$ D( LDD, * ), DC( LDDC, * ), DK( LDDK, * ),
$ DWORK( * ), RCOND( 4 )
C ..
C .. Local Scalars ..
INTEGER I, INF, INFO2, INFO3, IWAC, IWC, IWD, IWD1,
$ IWF, IWH, IWRE, IWRK, IWS1, IWS2, IWTU, IWTY,
$ IWWI, IWWR, IWX, IWY, LW1, LW2, LW3, LW4, LW5,
$ LW6, LW7, LWAMAX, M1, M11, M2, MINWRK, MODE,
$ NP1, NP11, NP2
DOUBLE PRECISION GAMABS, GAMAMN, GAMAMX, GTOLL, MINEAC, STEPG,
$ TOL2
C ..
C .. External Functions ..
LOGICAL SELECT
DOUBLE PRECISION DLAMCH
EXTERNAL DLAMCH, SELECT
C ..
C .. External Subroutines ..
EXTERNAL DGEES, DGESVD, DLACPY, SB10LD, SB10PD, SB10QD,
$ SB10RD, XERBLA
C ..
C .. Intrinsic Functions ..
INTRINSIC DBLE, INT, MAX, MIN, SQRT
C ..
C .. Executable Statements ..
C
C Decode and test input parameters.
C
M1 = M - NCON
M2 = NCON
NP1 = NP - NMEAS
NP2 = NMEAS
NP11 = NP1 - M2
M11 = M1 - NP2
C
INFO = 0
IF ( JOB.LT.1 .OR. JOB.GT.4 ) THEN
INFO = -1
ELSE IF( N.LT.0 ) THEN
INFO = -2
ELSE IF( M.LT.0 ) THEN
INFO = -3
ELSE IF( NP.LT.0 ) THEN
INFO = -4
ELSE IF( NCON.LT.0 .OR. M1.LT.0 .OR. M2.GT.NP1 ) THEN
INFO = -5
ELSE IF( NMEAS.LT.0 .OR. NP1.LT.0 .OR. NP2.GT.M1 ) THEN
INFO = -6
ELSE IF( GAMMA.LT.ZERO ) THEN
INFO = -7
ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
INFO = -9
ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
INFO = -11
ELSE IF( LDC.LT.MAX( 1, NP ) ) THEN
INFO = -13
ELSE IF( LDD.LT.MAX( 1, NP ) ) THEN
INFO = -15
ELSE IF( LDAK.LT.MAX( 1, N ) ) THEN
INFO = -17
ELSE IF( LDBK.LT.MAX( 1, N ) ) THEN
INFO = -19
ELSE IF( LDCK.LT.MAX( 1, M2 ) ) THEN
INFO = -21
ELSE IF( LDDK.LT.MAX( 1, M2 ) ) THEN
INFO = -23
ELSE IF( LDAC.LT.MAX( 1, 2*N ) ) THEN
INFO = -25
ELSE IF( LDBC.LT.MAX( 1, 2*N ) ) THEN
INFO = -27
ELSE IF( LDCC.LT.MAX( 1, NP1 ) ) THEN
INFO = -29
ELSE IF( LDDC.LT.MAX( 1, NP1 ) ) THEN
INFO = -31
ELSE
C
C Compute workspace.
C
LW1 = N*M + NP*N + NP*M + M2*M2 + NP2*NP2
LW2 = MAX( ( N + NP1 + 1 )*( N + M2 ) +
$ MAX( 3*( N + M2 ) + N + NP1, 5*( N + M2 ) ),
$ ( N + NP2 )*( N + M1 + 1 ) +
$ MAX( 3*( N + NP2 ) + N + M1, 5*( N + NP2 ) ),
$ M2 + NP1*NP1 + MAX( NP1*MAX( N, M1 ), 3*M2 + NP1,
$ 5*M2 ),
$ NP2 + M1*M1 + MAX( MAX( N, NP1 )*M1, 3*NP2 + M1,
$ 5*NP2 ) )
LW3 = MAX( NP11*M1 + MAX( 4*MIN( NP11, M1 ) + MAX( NP11, M1 ),
$ 6*MIN( NP11, M1 ) ),
$ NP1*M11 + MAX( 4*MIN( NP1, M11 ) + MAX( NP1, M11 ),
$ 6*MIN( NP1, M11 ) ) )
LW4 = 2*M*M + NP*NP + 2*M*N + M*NP + 2*N*NP
LW5 = 2*N*N + M*N + N*NP
LW6 = MAX( M*M + MAX( 2*M1, 3*N*N +
$ MAX( N*M, 10*N*N + 12*N + 5 ) ),
$ NP*NP + MAX( 2*NP1, 3*N*N +
$ MAX( N*NP, 10*N*N + 12*N + 5 ) ) )
LW7 = M2*NP2 + NP2*NP2 + M2*M2 +
$ MAX( NP11*NP11 + MAX( 2*NP11, ( NP11 + M11 )*NP2 ),
$ M11*M11 + MAX( 2*M11, M11*M2 ), 3*N,
$ N*( 2*NP2 + M2 ) +
$ MAX( 2*N*M2, M2*NP2 +
$ MAX( M2*M2 + 3*M2, NP2*( 2*NP2 +
$ M2 + MAX( NP2, N ) ) ) ) )
MINWRK = LW1 + MAX( 1, LW2, LW3, LW4, LW5 + MAX( LW6, LW7 ) )
IF( LDWORK.LT.MINWRK ) THEN
INFO = -38
ELSE IF( LIWORK.LT.MAX( 2*MAX( N, M1, NP1, M2, NP2 ),
$ N*N ) ) THEN
INFO = -36
ELSE IF( LBWORK.LT.2*N ) THEN
INFO = -40
END IF
END IF
C
IF ( INFO.NE.0 ) THEN
C
C Error return.
C
CALL XERBLA( 'SB10AD', -INFO )
RETURN
END IF
C
C Quick return if possible.
C
IF( N.EQ.0 .OR. M.EQ.0 .OR. NP.EQ.0 .OR. M1.EQ.0 .OR. M2.EQ.0
$ .OR. NP1.EQ.0 .OR. NP2.EQ.0 ) THEN
RCOND( 1 ) = ONE
RCOND( 2 ) = ONE
RCOND( 3 ) = ONE
RCOND( 4 ) = ONE
DWORK( 1 ) = ONE
RETURN
END IF
C
MODE = JOB
IF ( MODE.GT.2 )
$ MODE = 1
GTOLL = GTOL
IF( GTOLL.LE.ZERO ) THEN
C
C Set the default value of the tolerance for GAMMA.
C
GTOLL = SQRT( DLAMCH( 'Epsilon' ) )
END IF
C
C Workspace usage 1.
C
IWC = 1 + N*M
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, N )
C
CALL DLACPY( 'Full', NP, N, C, LDC, DWORK( IWC ), NP )
C
CALL DLACPY( 'Full', NP, M, D, LDD, DWORK( IWD ), NP )
C
C Transform the system so that D12 and D21 satisfy the formulas
C in the computation of the Hinf optimal controller.
C Workspace: need LW1 + MAX(1,LWP1,LWP2,LWP3,LWP4),
C prefer larger,
C where
C LW1 = N*M + NP*N + NP*M + M2*M2 + NP2*NP2
C LWP1 = (N+NP1+1)*(N+M2) + MAX(3*(N+M2)+N+NP1,5*(N+M2)),
C LWP2 = (N+NP2)*(N+M1+1) + MAX(3*(N+NP2)+N+M1,5*(N+NP2)),
C LWP3 = M2 + NP1*NP1 + MAX(NP1*MAX(N,M1),3*M2+NP1,5*M2),
C LWP4 = NP2 + M1*M1 + MAX(MAX(N,NP1)*M1,3*NP2+M1,5*NP2),
C with M1 = M - M2 and NP1 = NP - NP2.
C Denoting Q = MAX(M1,M2,NP1,NP2), an upper bound is
C LW1 + MAX(1,(N+Q)*(N+Q+6),Q*(Q+MAX(N,Q,5)+1).
C
TOL2 = -ONE
C
CALL SB10PD( N, M, NP, NCON, NMEAS, A, LDA, DWORK, N,
$ DWORK( IWC ), NP, DWORK( IWD ), NP, DWORK( IWTU ),
$ M2, DWORK( IWTY ), NP2, RCOND, TOL2, DWORK( IWRK ),
$ LDWORK-IWRK+1, INFO2 )
C
LWAMAX = INT( DWORK( IWRK ) ) + IWRK - 1
C
IF ( INFO2.NE.0 ) THEN
INFO = INFO2
RETURN
END IF
C
C Workspace usage 2.
C
IWD1 = IWRK
IWS1 = IWD1 + NP11*M1
C
C Check if GAMMA < max(sigma[D1111,D1112],sigma[D1111',D1121']).
C Workspace: need LW1 + MAX(1, LWS1, LWS2),
C prefer larger,
C where
C LWS1 = NP11*M1 + MAX(4*MIN(NP11,M1)+MAX(NP11,M1),6*MIN(NP11,M1))
C LWS2 = NP1*M11 + MAX(4*MIN(NP1,M11)+MAX(NP1,M11),6*MIN(NP1,M11))
C
INFO2 = 0
INFO3 = 0
C
IF ( NP11.NE.0 .AND. M1.NE.0 ) THEN
IWRK = IWS1 + MIN( NP11, M1 )
CALL DLACPY( 'Full', NP11, M1, DWORK(IWD), LDD, DWORK(IWD1),
$ NP11 )
CALL DGESVD( 'N', 'N', NP11, M1, DWORK(IWD1), NP11,
$ DWORK(IWS1), DWORK(IWS1), 1, DWORK(IWS1), 1,
$ DWORK( IWRK ), LDWORK-IWRK+1, INFO2 )
LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 )
ELSE
DWORK(IWS1) = ZERO
END IF
C
IWS2 = IWD1 + NP1*M11
IF ( NP1.NE.0 .AND. M11.NE.0 ) THEN
IWRK = IWS2 + MIN( NP1, M11 )
CALL DLACPY( 'Full', NP1, M11, DWORK(IWD), LDD, DWORK(IWD1),
$ NP1 )
CALL DGESVD( 'N', 'N', NP1, M11, DWORK(IWD1), NP1, DWORK(IWS2),
$ DWORK(IWS2), 1, DWORK(IWS2), 1, DWORK( IWRK ),
$ LDWORK-IWRK+1, INFO3 )
LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 )
ELSE
DWORK(IWS2) = ZERO
END IF
C
GAMAMN = MAX( DWORK(IWS1), DWORK(IWS2) )
C
IF ( INFO2.GT.0 .OR. INFO3.GT.0 ) THEN
INFO = 10
RETURN
ELSE IF ( GAMMA.LE.GAMAMN ) THEN
INFO = 6
RETURN
END IF
C
C Workspace usage 3.
C
IWX = IWD1
IWY = IWX + N*N
IWF = IWY + N*N
IWH = IWF + M*N
IWRK = IWH + N*NP
IWAC = IWD1
IWWR = IWAC + 4*N*N
IWWI = IWWR + 2*N
IWRE = IWWI + 2*N
C
C Prepare some auxiliary variables for the gamma iteration.
C
STEPG = GAMMA - GAMAMN
GAMABS = GAMMA
GAMAMX = GAMMA
INF = 0
C
C ###############################################################
C
C Begin the gamma iteration.
C
10 CONTINUE
STEPG = STEPG/TWO
C
C Try to compute the state feedback and output injection
C matrices for the current GAMMA.
C
CALL SB10QD( N, M, NP, NCON, NMEAS, GAMMA, A, LDA, DWORK, N,
$ DWORK( IWC ), NP, DWORK( IWD ), NP, DWORK( IWF ),
$ M, DWORK( IWH ), N, DWORK( IWX ), N, DWORK( IWY ),
$ N, RCOND(3), IWORK, DWORK( IWRK ), LDWORK-IWRK+1,
$ BWORK, INFO2 )
C
IF ( INFO2.NE.0 ) GOTO 30
C
C Try to compute the Hinf suboptimal (yet) controller.
C
CALL SB10RD( N, M, NP, NCON, NMEAS, GAMMA, A, LDA, DWORK, N,
$ DWORK( IWC ), NP, DWORK( IWD ), NP, DWORK( IWF ),
$ M, DWORK( IWH ), N, DWORK( IWTU ), M2,
$ DWORK( IWTY ), NP2, DWORK( IWX ), N, DWORK( IWY ),
$ N, AK, LDAK, BK, LDBK, CK, LDCK, DK, LDDK, IWORK,
$ DWORK( IWRK ), LDWORK-IWRK+1, INFO2 )
C
IF ( INFO2.NE.0 ) GOTO 30
C
C Compute the closed-loop system.
C Workspace: need LW1 + 2*M*M + NP*NP + 2*M*N + M*NP + 2*N*NP;
C prefer larger.
C
CALL SB10LD( N, M, NP, NCON, NMEAS, A, LDA, B, LDB, C, LDC, D,
$ LDD, AK, LDAK, BK, LDBK, CK, LDCK, DK, LDDK, AC,
$ LDAC, BC, LDBC, CC, LDCC, DC, LDDC, IWORK,
$ DWORK( IWD1 ), LDWORK-IWD1+1, INFO2 )
C
IF ( INFO2.NE.0 ) GOTO 30
C
LWAMAX = MAX( LWAMAX, INT( DWORK( IWD1 ) ) + IWD1 - 1 )
C
C Compute the poles of the closed-loop system.
C Workspace: need LW1 + 4*N*N + 4*N + max(1,6*N);
C prefer larger.
C
CALL DLACPY( 'Full', 2*N, 2*N, AC, LDAC, DWORK(IWAC), 2*N )
C
CALL DGEES( 'N', 'N', SELECT, 2*N, DWORK(IWAC), 2*N, IWORK,
$ DWORK(IWWR), DWORK(IWWI), DWORK(IWRE), 1,
$ DWORK(IWRE), LDWORK-IWRE+1, BWORK, INFO2 )
C
LWAMAX = MAX( LWAMAX, INT( DWORK( IWRE ) ) + IWRE - 1 )
C
C Now DWORK(IWWR+I)=Re(Lambda), DWORK(IWWI+I)=Im(Lambda),
C for I=0,2*N-1.
C
MINEAC = -THOUS
C
DO 20 I = 0, 2*N - 1
MINEAC = MAX( MINEAC, DWORK(IWWR+I) )
20 CONTINUE
C
C Check if the closed-loop system is stable.
C
30 IF ( MODE.EQ.1 ) THEN
IF ( INFO2.EQ.0 .AND. MINEAC.LT.ACTOL ) THEN
GAMABS = GAMMA
GAMMA = GAMMA - STEPG
INF = 1
ELSE
GAMMA = MIN( GAMMA + STEPG, GAMAMX )
END IF
ELSE IF ( MODE.EQ.2 ) THEN
IF ( INFO2.EQ.0 .AND. MINEAC.LT.ACTOL ) THEN
GAMABS = GAMMA
INF = 1
END IF
GAMMA = GAMMA - MAX( P1, GTOLL )
END IF
C
C More iterations?
C
IF ( MODE.EQ.1 .AND. JOB.EQ.3 .AND. TWO*STEPG.LT.GTOLL ) THEN
MODE = 2
GAMMA = GAMABS
END IF
C
IF ( JOB.NE.4 .AND.
$ ( MODE.EQ.1 .AND. TWO*STEPG.GE.GTOLL .OR.
$ MODE.EQ.2 .AND. GAMMA.GT.ZERO ) ) THEN
GOTO 10
END IF
C
C ###############################################################
C
C End of the gamma iteration - Return if no stabilizing controller
C was found.
C
IF ( INF.EQ.0 ) THEN
INFO = 12
RETURN
END IF
C
C Now compute the state feedback and output injection matrices
C using GAMABS.
C
GAMMA = GAMABS
C
C Integer workspace: need max(2*max(N,M-NCON,NP-NMEAS),N*N).
C Workspace: need LW1P +
C max(1,M*M + max(2*M1,3*N*N +
C max(N*M,10*N*N+12*N+5)),
C NP*NP + max(2*NP1,3*N*N +
C max(N*NP,10*N*N+12*N+5)));
C prefer larger,
C where LW1P = LW1 + 2*N*N + M*N + N*NP.
C An upper bound of the second term after LW1P is
C max(1,4*Q*Q+max(2*Q,3*N*N + max(2*N*Q,10*N*N+12*N+5))).
C
CALL SB10QD( N, M, NP, NCON, NMEAS, GAMMA, A, LDA, DWORK, N,
$ DWORK( IWC ), NP, DWORK( IWD ), NP, DWORK( IWF ),
$ M, DWORK( IWH ), N, DWORK( IWX ), N, DWORK( IWY ),
$ N, RCOND(3), IWORK, DWORK( IWRK ), LDWORK-IWRK+1,
$ BWORK, INFO2 )
C
LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 )
C
IF ( INFO2.GT.0 ) THEN
INFO = INFO2 + 5
RETURN
END IF
C
C Compute the Hinf optimal controller.
C Integer workspace: need max(2*(max(NP,M)-M2-NP2,M2,N),NP2).
C Workspace: need LW1P +
C max(1, M2*NP2 + NP2*NP2 + M2*M2 +
C max(D1*D1 + max(2*D1, (D1+D2)*NP2),
C D2*D2 + max(2*D2, D2*M2), 3*N,
C N*(2*NP2 + M2) +
C max(2*N*M2, M2*NP2 +
C max(M2*M2+3*M2, NP2*(2*NP2+
C M2+max(NP2,N))))))
C where D1 = NP1 - M2 = NP11, D2 = M1 - NP2 = M11;
C prefer larger.
C An upper bound of the second term after LW1P is
C max( 1, Q*(3*Q + 3*N + max(2*N, 4*Q + max(Q, N)))).
C
CALL SB10RD( N, M, NP, NCON, NMEAS, GAMMA, A, LDA, DWORK, N,
$ DWORK( IWC ), NP, DWORK( IWD ), NP, DWORK( IWF ),
$ M, DWORK( IWH ), N, DWORK( IWTU ), M2, DWORK( IWTY ),
$ NP2, DWORK( IWX ), N, DWORK( IWY ), N, AK, LDAK, BK,
$ LDBK, CK, LDCK, DK, LDDK, IWORK, DWORK( IWRK ),
$ LDWORK-IWRK+1, INFO2 )
C
LWAMAX = MAX( LWAMAX, INT( DWORK( IWRK ) ) + IWRK - 1 )
C
IF( INFO2.EQ.1 ) THEN
INFO = 6
RETURN
ELSE IF( INFO2.EQ.2 ) THEN
INFO = 9
RETURN
END IF
C
C Integer workspace: need 2*max(NCON,NMEAS).
C Workspace: need 2*M*M + NP*NP + 2*M*N + M*NP + 2*N*NP;
C prefer larger.
C
CALL SB10LD( N, M, NP, NCON, NMEAS, A, LDA, B, LDB, C, LDC, D,
$ LDD, AK, LDAK, BK, LDBK, CK, LDCK, DK, LDDK, AC,
$ LDAC, BC, LDBC, CC, LDCC, DC, LDDC, IWORK, DWORK,
$ LDWORK, INFO2 )
C
IF( INFO2.GT.0 ) THEN
INFO = 11
RETURN
END IF
C
DWORK( 1 ) = DBLE( LWAMAX )
RETURN
C *** Last line of SB10AD ***
END