1
SUBROUTINE SB10FD( N, M, NP, NCON, NMEAS, GAMMA, A, LDA, B, LDB,
2
$ C, LDC, D, LDD, AK, LDAK, BK, LDBK, CK, LDCK,
3
$ DK, LDDK, RCOND, TOL, IWORK, DWORK, LDWORK,
6
C RELEASE 4.0, WGS COPYRIGHT 1999.
10
C To compute the matrices of an H-infinity (sub)optimal n-state
17
C using modified Glover's and Doyle's 1988 formulas, for the system
19
C | A | B1 B2 | | A | B |
20
C P = |----|---------| = |---|---|
21
C | C1 | D11 D12 | | C | D |
24
C and for a given value of gamma, where B2 has as column size the
25
C number of control inputs (NCON) and C2 has as row size the number
26
C of measurements (NMEAS) being provided to the controller.
30
C (A1) (A,B2) is stabilizable and (C2,A) is detectable,
32
C (A2) D12 is full column rank and D21 is full row rank,
34
C (A3) | A-j*omega*I B2 | has full column rank for all omega,
37
C (A4) | A-j*omega*I B1 | has full row rank for all omega.
42
C Input/Output Parameters
45
C The order of the system. N >= 0.
48
C The column size of the matrix B. M >= 0.
51
C The row size of the matrix C. NP >= 0.
53
C NCON (input) INTEGER
54
C The number of control inputs (M2). M >= NCON >= 0,
57
C NMEAS (input) INTEGER
58
C The number of measurements (NP2). NP >= NMEAS >= 0,
61
C GAMMA (input) DOUBLE PRECISION
62
C The value of gamma. It is assumed that gamma is
63
C sufficiently large so that the controller is admissible.
66
C A (input) DOUBLE PRECISION array, dimension (LDA,N)
67
C The leading N-by-N part of this array must contain the
68
C system state matrix A.
71
C The leading dimension of the array A. LDA >= max(1,N).
73
C B (input) DOUBLE PRECISION array, dimension (LDB,M)
74
C The leading N-by-M part of this array must contain the
75
C system input matrix B.
78
C The leading dimension of the array B. LDB >= max(1,N).
80
C C (input) DOUBLE PRECISION array, dimension (LDC,N)
81
C The leading NP-by-N part of this array must contain the
82
C system output matrix C.
85
C The leading dimension of the array C. LDC >= max(1,NP).
87
C D (input) DOUBLE PRECISION array, dimension (LDD,M)
88
C The leading NP-by-M part of this array must contain the
89
C system input/output matrix D.
92
C The leading dimension of the array D. LDD >= max(1,NP).
94
C AK (output) DOUBLE PRECISION array, dimension (LDAK,N)
95
C The leading N-by-N part of this array contains the
96
C controller state matrix AK.
99
C The leading dimension of the array AK. LDAK >= max(1,N).
101
C BK (output) DOUBLE PRECISION array, dimension (LDBK,NMEAS)
102
C The leading N-by-NMEAS part of this array contains the
103
C controller input matrix BK.
106
C The leading dimension of the array BK. LDBK >= max(1,N).
108
C CK (output) DOUBLE PRECISION array, dimension (LDCK,N)
109
C The leading NCON-by-N part of this array contains the
110
C controller output matrix CK.
113
C The leading dimension of the array CK.
114
C LDCK >= max(1,NCON).
116
C DK (output) DOUBLE PRECISION array, dimension (LDDK,NMEAS)
117
C The leading NCON-by-NMEAS part of this array contains the
118
C controller input/output matrix DK.
121
C The leading dimension of the array DK.
122
C LDDK >= max(1,NCON).
124
C RCOND (output) DOUBLE PRECISION array, dimension (4)
125
C RCOND(1) contains the reciprocal condition number of the
126
C control transformation matrix;
127
C RCOND(2) contains the reciprocal condition number of the
128
C measurement transformation matrix;
129
C RCOND(3) contains an estimate of the reciprocal condition
130
C number of the X-Riccati equation;
131
C RCOND(4) contains an estimate of the reciprocal condition
132
C number of the Y-Riccati equation.
136
C TOL DOUBLE PRECISION
137
C Tolerance used for controlling the accuracy of the applied
138
C transformations for computing the normalized form in
139
C SLICOT Library routine SB10PD. Transformation matrices
140
C whose reciprocal condition numbers are less than TOL are
141
C not allowed. If TOL <= 0, then a default value equal to
142
C sqrt(EPS) is used, where EPS is the relative machine
147
C IWORK INTEGER array, dimension (LIWORK), where
148
C LIWORK = max(2*max(N,M-NCON,NP-NMEAS,NCON),N*N)
150
C DWORK DOUBLE PRECISION array, dimension (LDWORK)
151
C On exit, if INFO = 0, DWORK(1) contains the optimal
155
C The dimension of the array DWORK.
156
C LDWORK >= N*M + NP*(N+M) + M2*M2 + NP2*NP2 +
157
C max(1,LW1,LW2,LW3,LW4,LW5,LW6), where
158
C LW1 = (N+NP1+1)*(N+M2) + max(3*(N+M2)+N+NP1,5*(N+M2)),
159
C LW2 = (N+NP2)*(N+M1+1) + max(3*(N+NP2)+N+M1,5*(N+NP2)),
160
C LW3 = M2 + NP1*NP1 + max(NP1*max(N,M1),3*M2+NP1,5*M2),
161
C LW4 = NP2 + M1*M1 + max(max(N,NP1)*M1,3*NP2+M1,5*NP2),
162
C LW5 = 2*N*N + N*(M+NP) +
163
C max(1,M*M + max(2*M1,3*N*N+max(N*M,10*N*N+12*N+5)),
164
C NP*NP + max(2*NP1,3*N*N +
165
C max(N*NP,10*N*N+12*N+5))),
166
C LW6 = 2*N*N + N*(M+NP) +
167
C max(1, M2*NP2 + NP2*NP2 + M2*M2 +
168
C max(D1*D1 + max(2*D1, (D1+D2)*NP2),
169
C D2*D2 + max(2*D2, D2*M2), 3*N,
171
C max(2*N*M2, M2*NP2 +
172
C max(M2*M2+3*M2, NP2*(2*NP2+
173
C M2+max(NP2,N)))))),
174
C with D1 = NP1 - M2, D2 = M1 - NP2,
175
C NP1 = NP - NP2, M1 = M - M2.
176
C For good performance, LDWORK must generally be larger.
177
C Denoting Q = max(M1,M2,NP1,NP2), an upper bound is
178
C 2*Q*(3*Q+2*N)+max(1,(N+Q)*(N+Q+6),Q*(Q+max(N,Q,5)+1),
179
C 2*N*(N+2*Q)+max(1,4*Q*Q+
180
C max(2*Q,3*N*N+max(2*N*Q,10*N*N+12*N+5)),
181
C Q*(3*N+3*Q+max(2*N,4*Q+max(N,Q))))).
183
C BWORK LOGICAL array, dimension (2*N)
188
C = 0: successful exit;
189
C < 0: if INFO = -i, the i-th argument had an illegal
191
C = 1: if the matrix | A-j*omega*I B2 | had not full
193
C column rank in respect to the tolerance EPS;
194
C = 2: if the matrix | A-j*omega*I B1 | had not full row
196
C rank in respect to the tolerance EPS;
197
C = 3: if the matrix D12 had not full column rank in
198
C respect to the tolerance TOL;
199
C = 4: if the matrix D21 had not full row rank in respect
200
C to the tolerance TOL;
201
C = 5: if the singular value decomposition (SVD) algorithm
202
C did not converge (when computing the SVD of one of
203
C the matrices |A B2 |, |A B1 |, D12 or D21).
205
C = 6: if the controller is not admissible (too small value
207
C = 7: if the X-Riccati equation was not solved
208
C successfully (the controller is not admissible or
209
C there are numerical difficulties);
210
C = 8: if the Y-Riccati equation was not solved
211
C successfully (the controller is not admissible or
212
C there are numerical difficulties);
213
C = 9: if the determinant of Im2 + Tu*D11HAT*Ty*D22 is
218
C The routine implements the Glover's and Doyle's 1988 formulas [1],
219
C [2] modified to improve the efficiency as described in [3].
223
C [1] Glover, K. and Doyle, J.C.
224
C State-space formulae for all stabilizing controllers that
225
C satisfy an Hinf norm bound and relations to risk sensitivity.
226
C Systems and Control Letters, vol. 11, pp. 167-172, 1988.
228
C [2] Balas, G.J., Doyle, J.C., Glover, K., Packard, A., and
230
C mu-Analysis and Synthesis Toolbox.
231
C The MathWorks Inc., Natick, Mass., 1995.
233
C [3] Petkov, P.Hr., Gu, D.W., and Konstantinov, M.M.
234
C Fortran 77 routines for Hinf and H2 design of continuous-time
235
C linear control systems.
236
C Rep. 98-14, Department of Engineering, Leicester University,
237
C Leicester, U.K., 1998.
241
C The accuracy of the result depends on the condition numbers of the
242
C input and output transformations and on the condition numbers of
243
C the two Riccati equations, as given by the values of RCOND(1),
244
C RCOND(2), RCOND(3) and RCOND(4), respectively.
248
C P.Hr. Petkov, D.W. Gu and M.M. Konstantinov, October 1998.
252
C V. Sima, Research Institute for Informatics, Bucharest, May 1999,
253
C Sept. 1999, Feb. 2000.
257
C Algebraic Riccati equation, H-infinity optimal control, robust
260
C ******************************************************************
263
DOUBLE PRECISION ZERO, ONE
264
PARAMETER ( ZERO = 0.0D+0, ONE = 1.0D+0 )
266
C .. Scalar Arguments ..
267
INTEGER INFO, LDA, LDAK, LDB, LDBK, LDC, LDCK, LDD,
268
$ LDDK, LDWORK, M, N, NCON, NMEAS, NP
269
DOUBLE PRECISION GAMMA, TOL
271
C .. Array Arguments ..
274
DOUBLE PRECISION A( LDA, * ), AK( LDAK, * ), B( LDB, * ),
275
$ BK( LDBK, * ), C( LDC, * ), CK( LDCK, * ),
276
$ D( LDD, * ), DK( LDDK, * ), DWORK( * ),
279
C .. Local Scalars ..
280
INTEGER INFO2, IWC, IWD, IWF, IWH, IWRK, IWTU, IWTY,
281
$ IWX, IWY, LW1, LW2, LW3, LW4, LW5, LW6,
282
$ LWAMAX, M1, M2, MINWRK, ND1, ND2, NP1, NP2
283
DOUBLE PRECISION TOLL
285
C .. External Functions ..
286
DOUBLE PRECISION DLAMCH
289
C .. External Subroutines ..
290
EXTERNAL DLACPY, SB10PD, SB10QD, SB10RD, XERBLA
292
C .. Intrinsic Functions ..
293
INTRINSIC DBLE, INT, MAX, SQRT
295
C .. Executable Statements ..
297
C Decode and Test input parameters.
307
ELSE IF( M.LT.0 ) THEN
309
ELSE IF( NP.LT.0 ) THEN
311
ELSE IF( NCON.LT.0 .OR. M1.LT.0 .OR. M2.GT.NP1 ) THEN
313
ELSE IF( NMEAS.LT.0 .OR. NP1.LT.0 .OR. NP2.GT.M1 ) THEN
315
ELSE IF( GAMMA.LT.ZERO ) THEN
317
ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
319
ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
321
ELSE IF( LDC.LT.MAX( 1, NP ) ) THEN
323
ELSE IF( LDD.LT.MAX( 1, NP ) ) THEN
325
ELSE IF( LDAK.LT.MAX( 1, N ) ) THEN
327
ELSE IF( LDBK.LT.MAX( 1, N ) ) THEN
329
ELSE IF( LDCK.LT.MAX( 1, M2 ) ) THEN
331
ELSE IF( LDDK.LT.MAX( 1, M2 ) ) THEN
339
LW1 = ( N + NP1 + 1 )*( N + M2 ) + MAX( 3*( N + M2 ) + N + NP1,
341
LW2 = ( N + NP2 )*( N + M1 + 1 ) + MAX( 3*( N + NP2 ) + N +
342
$ M1, 5*( N + NP2 ) )
343
LW3 = M2 + NP1*NP1 + MAX( NP1*MAX( N, M1 ), 3*M2 + NP1, 5*M2 )
344
LW4 = NP2 + M1*M1 + MAX( MAX( N, NP1 )*M1, 3*NP2 + M1, 5*NP2 )
345
LW5 = 2*N*N + N*( M + NP ) +
346
$ MAX( 1, M*M + MAX( 2*M1, 3*N*N +
347
$ MAX( N*M, 10*N*N + 12*N + 5 ) ),
348
$ NP*NP + MAX( 2*NP1, 3*N*N +
349
$ MAX( N*NP, 10*N*N + 12*N + 5 ) ) )
350
LW6 = 2*N*N + N*( M + NP ) +
351
$ MAX( 1, M2*NP2 + NP2*NP2 + M2*M2 +
352
$ MAX( ND1*ND1 + MAX( 2*ND1, ( ND1 + ND2 )*NP2 ),
353
$ ND2*ND2 + MAX( 2*ND2, ND2*M2 ), 3*N,
355
$ MAX( 2*N*M2, M2*NP2 +
356
$ MAX( M2*M2 + 3*M2, NP2*( 2*NP2 +
357
$ M2 + MAX( NP2, N ) ) ) ) ) )
358
MINWRK = N*M + NP*( N + M ) + M2*M2 + NP2*NP2 +
359
$ MAX( 1, LW1, LW2, LW3, LW4, LW5, LW6 )
360
IF( LDWORK.LT.MINWRK )
364
CALL XERBLA( 'SB10FD', -INFO )
368
C Quick return if possible.
370
IF( N.EQ.0 .OR. M.EQ.0 .OR. NP.EQ.0 .OR. M1.EQ.0 .OR. M2.EQ.0
371
$ .OR. NP1.EQ.0 .OR. NP2.EQ.0 ) THEN
381
IF( TOLL.LE.ZERO ) THEN
383
C Set the default value of the tolerance.
385
TOLL = SQRT( DLAMCH( 'Epsilon' ) )
394
IWRK = IWTY + NP2*NP2
396
CALL DLACPY( 'Full', N, M, B, LDB, DWORK, N )
397
CALL DLACPY( 'Full', NP, N, C, LDC, DWORK( IWC ), NP )
398
CALL DLACPY( 'Full', NP, M, D, LDD, DWORK( IWD ), NP )
400
C Transform the system so that D12 and D21 satisfy the formulas
401
C in the computation of the Hinf (sub)optimal controller.
403
CALL SB10PD( N, M, NP, NCON, NMEAS, A, LDA, DWORK, N,
404
$ DWORK( IWC ), NP, DWORK( IWD ), NP, DWORK( IWTU ),
405
$ M2, DWORK( IWTY ), NP2, RCOND, TOLL, DWORK( IWRK ),
406
$ LDWORK-IWRK+1, INFO2 )
407
IF( INFO2.GT.0 ) THEN
411
LWAMAX = INT( DWORK( IWRK ) ) + IWRK - 1
419
C Compute the (sub)optimal state feedback and output injection
422
CALL SB10QD( N, M, NP, NCON, NMEAS, GAMMA, A, LDA, DWORK, N,
423
$ DWORK( IWC ), NP, DWORK( IWD ), NP, DWORK( IWF ),
424
$ M, DWORK( IWH ), N, DWORK( IWX ), N, DWORK( IWY ),
425
$ N, RCOND(3), IWORK, DWORK( IWRK ), LDWORK-IWRK+1,
427
IF( INFO2.GT.0 ) THEN
431
LWAMAX = MAX( INT( DWORK( IWRK ) ) + IWRK - 1, LWAMAX )
433
C Compute the Hinf (sub)optimal controller.
435
CALL SB10RD( N, M, NP, NCON, NMEAS, GAMMA, A, LDA, DWORK, N,
436
$ DWORK( IWC ), NP, DWORK( IWD ), NP, DWORK( IWF ),
437
$ M, DWORK( IWH ), N, DWORK( IWTU ), M2, DWORK( IWTY ),
438
$ NP2, DWORK( IWX ), N, DWORK( IWY ), N, AK, LDAK, BK,
439
$ LDBK, CK, LDCK, DK, LDDK, IWORK, DWORK( IWRK ),
440
$ LDWORK-IWRK+1, INFO2 )
441
IF( INFO2.EQ.1 ) THEN
444
ELSE IF( INFO2.EQ.2 ) THEN
448
LWAMAX = MAX( INT( DWORK( IWRK ) ) + IWRK - 1, LWAMAX )
450
DWORK( 1 ) = DBLE( LWAMAX )
452
C *** Last line of SB10FD ***