1 SUBROUTINE TB01ZD( JOBZ, N, P, A, LDA, B, C, LDC, NCONT, Z, LDZ, 2 $ TAU, TOL, DWORK, LDWORK, INFO ) 3C 4C WARNING 5C 6C This file alters the SLICOT routine TB01ZD. It is intended 7C for use from the Octave Control Systems Package and modifies 8C the original SLICOT implementation. This file itself is *NOT* 9C part of SLICOT and the authors from NICONET e.V. are *NOT* 10C responsible for it. See file DESCRIPTION for the current 11C maintainer of the Octave control package. 12C 13C In altered implementation uses the algorithm described in [A] 14C to determine the minimum realization of a single-input system. 15C This implementation uses pivoting between subsequent Householder 16C reflections to achieve better numerical accuracy (see METHOD section 17C of the original routine for more information). Note that the original 18C algorthim will be used if the user asks for the factored form of the 19C similarity transformations (JOBZ = 'F'). 20C 21C [A] A. Hodel, P. Misra, 'Partial Pivoting in the Computation 22C of Krylov Subspaces of Large Sparse Systems', Proceedings of the 23C 42nd IEEE Conference on Decision and Control, December 2003. 24C 25C SLICOT RELEASE 5.0. 26C 27C Copyright (c) 2002-2010 NICONET e.V. 28C 29C This program is free software: you can redistribute it and/or 30C modify it under the terms of the GNU General Public License as 31C published by the Free Software Foundation, either version 2 of 32C the License, or (at your option) any later version. 33C 34C This program is distributed in the hope that it will be useful, 35C but WITHOUT ANY WARRANTY; without even the implied warranty of 36C MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 37C GNU General Public License for more details. 38C 39C You should have received a copy of the GNU General Public License 40C along with this program. If not, see 41C <http://www.gnu.org/licenses/>. 42C 43C PURPOSE 44C 45C To find a controllable realization for the linear time-invariant 46C single-input system 47C 48C dX/dt = A * X + B * U, 49C Y = C * X, 50C 51C where A is an N-by-N matrix, B is an N element vector, C is an 52C P-by-N matrix, and A and B are reduced by this routine to 53C orthogonal canonical form using (and optionally accumulating) 54C orthogonal similarity transformations, which are also applied 55C to C. 56C 57C ARGUMENTS 58C 59C Mode Parameters 60C 61C JOBZ CHARACTER*1 62C Indicates whether the user wishes to accumulate in a 63C matrix Z the orthogonal similarity transformations for 64C reducing the system, as follows: 65C = 'N': Do not form Z and do not store the orthogonal 66C transformations; 67C = 'F': Do not form Z, but store the orthogonal 68C transformations in the factored form; 69C = 'I': Z is initialized to the unit matrix and the 70C orthogonal transformation matrix Z is returned. 71C 72C Input/Output Parameters 73C 74C N (input) INTEGER 75C The order of the original state-space representation, 76C i.e. the order of the matrix A. N >= 0. 77C 78C P (input) INTEGER 79C The number of system outputs, or of rows of C. P >= 0. 80C 81C A (input/output) DOUBLE PRECISION array, dimension (LDA,N) 82C On entry, the leading N-by-N part of this array must 83C contain the original state dynamics matrix A. 84C On exit, the leading NCONT-by-NCONT upper Hessenberg 85C part of this array contains the canonical form of the 86C state dynamics matrix, given by Z' * A * Z, of a 87C controllable realization for the original system. The 88C elements below the first subdiagonal are set to zero. 89C 90C LDA INTEGER 91C The leading dimension of array A. LDA >= MAX(1,N). 92C 93C B (input/output) DOUBLE PRECISION array, dimension (N) 94C On entry, the original input/state vector B. 95C On exit, the leading NCONT elements of this array contain 96C canonical form of the input/state vector, given by Z' * B, 97C with all elements but B(1) set to zero. 98C 99C C (input/output) DOUBLE PRECISION array, dimension (LDC,N) 100C On entry, the leading P-by-N part of this array must 101C contain the output/state matrix C. 102C On exit, the leading P-by-N part of this array contains 103C the transformed output/state matrix, given by C * Z, and 104C the leading P-by-NCONT part contains the output/state 105C matrix of the controllable realization. 106C 107C LDC INTEGER 108C The leading dimension of array C. LDC >= MAX(1,P). 109C 110C NCONT (output) INTEGER 111C The order of the controllable state-space representation. 112C 113C Z (output) DOUBLE PRECISION array, dimension (LDZ,N) 114C If JOBZ = 'I', then the leading N-by-N part of this array 115C contains the matrix of accumulated orthogonal similarity 116C transformations which reduces the given system to 117C orthogonal canonical form. 118C If JOBZ = 'F', the elements below the diagonal, with the 119C array TAU, represent the orthogonal transformation matrix 120C as a product of elementary reflectors. The transformation 121C matrix can then be obtained by calling the LAPACK Library 122C routine DORGQR. 123C If JOBZ = 'N', the array Z is not referenced and can be 124C supplied as a dummy array (i.e. set parameter LDZ = 1 and 125C declare this array to be Z(1,1) in the calling program). 126C 127C LDZ INTEGER 128C The leading dimension of array Z. If JOBZ = 'I' or 129C JOBZ = 'F', LDZ >= MAX(1,N); if JOBZ = 'N', LDZ >= 1. 130C 131C TAU (output) DOUBLE PRECISION array, dimension (N) 132C The elements of TAU contain the scalar factors of the 133C elementary reflectors used in the reduction of B and A. 134C 135C Tolerances 136C 137C TOL DOUBLE PRECISION 138C The tolerance to be used in determining the 139C controllability of (A,B). If the user sets TOL > 0, then 140C the given value of TOL is used as an absolute tolerance; 141C elements with absolute value less than TOL are considered 142C neglijible. If the user sets TOL <= 0, then an implicitly 143C computed, default tolerance, defined by 144C TOLDEF = N*EPS*MAX( NORM(A), NORM(B) ) is used instead, 145C where EPS is the machine precision (see LAPACK Library 146C routine DLAMCH). 147C 148C Workspace 149C 150C DWORK DOUBLE PRECISION array, dimension (LDWORK) 151C On exit, if INFO = 0, DWORK(1) returns the optimal value 152C of LDWORK. 153C 154C LDWORK INTEGER 155C The length of the array DWORK. LDWORK >= MAX(1,N,P). 156C For optimum performance LDWORK should be larger. 157C 158C Error Indicator 159C 160C INFO INTEGER 161C = 0: successful exit; 162C < 0: if INFO = -i, the i-th argument had an illegal 163C value. 164C 165C METHOD 166C 167C The Householder matrix which reduces all but the first element 168C of vector B to zero is found and this orthogonal similarity 169C transformation is applied to the matrix A. The resulting A is then 170C reduced to upper Hessenberg form by a sequence of Householder 171C transformations. Finally, the order of the controllable state- 172C space representation (NCONT) is determined by finding the position 173C of the first sub-diagonal element of A which is below an 174C appropriate zero threshold, either TOL or TOLDEF (see parameter 175C TOL); if NORM(B) is smaller than this threshold, NCONT is set to 176C zero, and no computations for reducing the system to orthogonal 177C canonical form are performed. 178C All orthogonal transformations determined in this process are also 179C applied to the matrix C, from the right. 180C 181C REFERENCES 182C 183C [1] Konstantinov, M.M., Petkov, P.Hr. and Christov, N.D. 184C Orthogonal Invariants and Canonical Forms for Linear 185C Controllable Systems. 186C Proc. 8th IFAC World Congress, Kyoto, 1, pp. 49-54, 1981. 187C 188C [2] Hammarling, S.J. 189C Notes on the use of orthogonal similarity transformations in 190C control. 191C NPL Report DITC 8/82, August 1982. 192C 193C [3] Paige, C.C 194C Properties of numerical algorithms related to computing 195C controllability. 196C IEEE Trans. Auto. Contr., AC-26, pp. 130-138, 1981. 197C 198C NUMERICAL ASPECTS 199C 3 200C The algorithm requires 0(N ) operations and is backward stable. 201C 202C CONTRIBUTOR 203C 204C V. Sima, Katholieke Univ. Leuven, Belgium, Feb. 1998. 205C 206C REVISIONS 207C 208C V. Sima, Research Institute for Informatics, Bucharest, Oct. 2001, 209C Sept. 2003. 210C 211C KEYWORDS 212C 213C Controllability, minimal realization, orthogonal canonical form, 214C orthogonal transformation. 215C 216C ****************************************************************** 217C 218C .. Parameters .. 219 DOUBLE PRECISION ZERO, ONE 220 PARAMETER ( ZERO = 0.0D0, ONE = 1.0D0 ) 221C .. Scalar Arguments .. 222 CHARACTER JOBZ 223 INTEGER INFO, LDA, LDC, LDWORK, LDZ, N, NCONT, P 224 DOUBLE PRECISION TOL 225C .. Array Arguments .. 226 DOUBLE PRECISION A(LDA,*), B(*), C(LDC,*), DWORK(*), TAU(*), 227 $ Z(LDZ,*) 228C .. Local Scalars .. 229 LOGICAL LJOBF, LJOBI, LJOBZ, LPIV 230 INTEGER ITAU, J, IDXM 231 DOUBLE PRECISION ANORM, B1, BNORM, FANORM, FBNORM, H, THRESH, 232 $ TOLDEF, WRKOPT, MAXV, MINV 233C .. Local Arrays .. 234 DOUBLE PRECISION NBLK(1) 235C .. External Functions .. 236 LOGICAL LSAME 237 INTEGER IDAMAX 238 DOUBLE PRECISION DLAMCH, DLANGE 239 EXTERNAL DLAMCH, DLANGE, LSAME, IDAMAX 240C .. External Subroutines .. 241 EXTERNAL DGEHRD, DLACPY, DLARF, DLARFG, DLASET, DORGQR, 242 $ DORMHR, MB01PD, XERBLA, DSWAP 243C .. Intrinsic Functions .. 244 INTRINSIC ABS, DBLE, MAX 245C .. Executable Statements .. 246C 247 INFO = 0 248 LJOBF = LSAME( JOBZ, 'F' ) 249 LJOBI = LSAME( JOBZ, 'I' ) 250 LJOBZ = LJOBF.OR.LJOBI 251C 252C Test the input scalar arguments. 253C 254 IF( .NOT.LJOBZ .AND. .NOT.LSAME( JOBZ, 'N' ) ) THEN 255 INFO = -1 256 ELSE IF( N.LT.0 ) THEN 257 INFO = -2 258 ELSE IF( P.LT.0 ) THEN 259 INFO = -3 260 ELSE IF( LDA.LT.MAX( 1, N ) ) THEN 261 INFO = -5 262 ELSE IF( LDC.LT.MAX( 1, P ) ) THEN 263 INFO = -8 264 ELSE IF( LDZ.LT.1 .OR. ( LJOBZ .AND. LDZ.LT.N ) ) THEN 265 INFO = -11 266 ELSE IF( LDWORK.LT.MAX( 1, N, P ) ) THEN 267 INFO = -15 268 END IF 269C 270 IF ( INFO.NE.0 ) THEN 271C 272C Error return. 273C 274 CALL XERBLA( 'TB01ZD', -INFO ) 275 RETURN 276 END IF 277C 278C Quick return if possible. 279C 280 NCONT = 0 281 DWORK(1) = ONE 282 IF ( N.EQ.0 ) 283 $ RETURN 284C 285C (Note: Comments in the code beginning "Workspace:" describe the 286C minimal amount of real workspace needed at that point in the 287C code, as well as the preferred amount for good performance. 288C NB refers to the optimal block size for the immediately 289C following subroutine, as returned by ILAENV.) 290C 291 WRKOPT = MAX ( 1, N, P ) 292C 293C Calculate the absolute norms of A and B (used for scaling). 294C 295 ANORM = DLANGE( 'Max', N, N, A, LDA, DWORK ) 296 BNORM = DLANGE( 'Max', N, 1, B, N, DWORK ) 297C 298C Return if matrix B is zero. 299C 300 IF( BNORM.EQ.ZERO ) THEN 301 IF( LJOBF ) THEN 302 CALL DLASET( 'Full', N, N, ZERO, ZERO, Z, LDZ ) 303 CALL DLASET( 'Full', N, 1, ZERO, ZERO, TAU, N ) 304 ELSE IF( LJOBI ) THEN 305 CALL DLASET( 'Full', N, N, ZERO, ONE, Z, LDZ ) 306 END IF 307 RETURN 308 END IF 309C 310C Scale (if needed) the matrices A and B. 311C 312 CALL MB01PD( 'S', 'G', N, N, 0, 0, ANORM, 0, NBLK, A, LDA, INFO ) 313 CALL MB01PD( 'S', 'G', N, 1, 0, 0, BNORM, 0, NBLK, B, N, INFO ) 314C 315C Calculate the Frobenius norm of A and the 1-norm of B (used for 316C controlability test). 317C 318 FANORM = DLANGE( 'Frobenius', N, N, A, LDA, DWORK ) 319 FBNORM = DLANGE( '1-norm', N, 1, B, N, DWORK ) 320C 321 TOLDEF = TOL 322 IF ( TOLDEF.LE.ZERO ) THEN 323C 324C Use the default tolerance in controllability determination. 325C 326 THRESH = DBLE(N)*DLAMCH( 'EPSILON' ) 327 TOLDEF = THRESH*MAX( FANORM, FBNORM ) 328 END IF 329C 330 ITAU = 1 331 CALL DLASET( 'Full', N, 1, ZERO, ZERO, TAU, N ) 332 IF ( FBNORM.GT.TOLDEF ) THEN 333C 334C B is not negligible compared with A. 335C 336 IF ( N.GT.1 ) THEN 337C 338C If orthogonal transformations in factored form are not 339C requested, use pivoting. Exchange rows so biggest norm 340C element is in the first row. 341C 342 IF ( .NOT.LJOBF ) THEN 343 IDXM = IDAMAX( N, B, 1 ) 344 LPIV = IDXM.NE.1 345C Exchange rows and columns in A and C 346C 347 IF ( LPIV ) THEN 348 B1 = B(1) 349 B(1) = B(IDXM) 350 B(IDXM) = B1 351 CALL DSWAP( N, A, LDA, A(IDXM,1), LDA ) 352 CALL DSWAP( N, A, 1, A(1,IDXM), 1 ) 353 CALL DSWAP( P, C, 1, C(1,IDXM), 1 ) 354 END IF 355 END IF 356C 357C Transform B by a Householder matrix Z1: store vector 358C describing this temporarily in B and in the local scalar H. 359C 360 CALL DLARFG( N, B(1), B(2), 1, H ) 361C 362 B1 = B(1) 363 B(1) = ONE 364C 365C Form Z1 * A * Z1. 366C Workspace: need N. 367C 368 CALL DLARF( 'Right', N, N, B, 1, H, A, LDA, DWORK ) 369 CALL DLARF( 'Left', N, N, B, 1, H, A, LDA, DWORK ) 370C 371C Form C * Z1. 372C Workspace: need P. 373C 374 CALL DLARF( 'Right', P, N, B, 1, H, C, LDC, DWORK ) 375C 376C If orthogonal matrix is asked, form it. 377C Workspace: need N. 378C 379 IF ( LJOBI ) THEN 380 CALL DLASET( 'Full', N, N, ZERO, ONE, Z, LDZ ) 381 IF ( LPIV ) THEN 382 Z(1,1) = ZERO 383 Z(IDXM,IDXM) = ZERO 384 Z(1,IDXM) = ONE 385 Z(IDXM,1) = ONE 386 END IF 387 CALL DLARF( 'Right', N, N, B, 1, H, Z, LDZ, DWORK ) 388 END IF 389 390 B(1) = B1 391 TAU(1) = H 392 ITAU = ITAU + 1 393 ELSE 394 B1 = B(1) 395 TAU(1) = ZERO 396 END IF 397C 398C Reduce modified A to upper Hessenberg form by an orthogonal 399C similarity transformation. 400C 401 IF ( TOL.LE.ZERO ) 402 $ TOLDEF = THRESH*MAX( FANORM, ABS( B1 ) ) 403 404 IF ( LJOBF ) THEN 405C 406C If factored form transformation needed, switch to old 407C algorithm. Workspace: need N; prefer N*NB. 408C 409 CALL DGEHRD( N, 1, N, A, LDA, TAU(ITAU), DWORK, 410 $ LDWORK, INFO ) 411 WRKOPT = DWORK(1) 412C 413C Form C * Z2. 414C Workspace: need P; prefer P*NB. 415C 416 CALL DORMHR( 'Right', 'No transpose', P, N, 1, N, A, LDA, 417 $ TAU(ITAU), C, LDC, DWORK, LDWORK, INFO ) 418 WRKOPT = MAX( WRKOPT, DWORK(1) ) 419C 420C Save the orthogonal transformations used, so that they could 421C be accumulated by calling DORGQR routine. 422C 423 IF ( N.GT.1 ) 424 $ CALL DLACPY( 'Full', N-1, 1, B(2), N-1, Z(2,1), LDZ ) 425 IF ( N.GT.2 ) 426 $ CALL DLACPY( 'Lower', N-2, N-2, A(3,1), LDA, Z(3,2), 427 $ LDZ ) 428C 429C Annihilate the lower part of A and B. 430C 431 IF ( N.GT.2 ) 432 $ CALL DLASET( 'Lower', N-2, N-2, ZERO, ZERO, A(3,1), LDA ) 433 IF ( N.GT.1 ) 434 $ CALL DLASET( 'Full', N-1, 1, ZERO, ZERO, B(2), N-1 ) 435C 436C Find NCONT by checking sizes of the sub-diagonal elements of 437C transformed A. 438C 439 J = 1 440C 441C WHILE ( J < N and ABS( A(J+1,J) ) > TOLDEF ) DO 442C 443 10 CONTINUE 444 IF ( J.LT.N ) THEN 445 IF ( ABS( A(J+1,J) ).GT.TOLDEF ) THEN 446 J = J + 1 447 GO TO 10 448 END IF 449 END IF 450C 451C END WHILE 10 452C 453C First negligible sub-diagonal element found, if any: set NCONT. 454C 455 NCONT = J 456 457 ELSE 458C 459C Use pivoting between orthogonal transformations. 460C 461 J = 1 462 NCONT = N 463C 464C Iterate over matrix subdiagonal elements. 465C WHILE ( J < N-1 and MAX( ABS( A(J+1,:) ) ) > TOLDEF ) DO 466C 467 20 CONTINUE 468 IF ( J.LT.N-1 ) THEN 469 IDXM = IDAMAX( N-J, A(J+1,J), 1 )+J 470 MAXV = ABS( A(IDXM,J) ) 471 LPIV = IDXM.NE.J+1 472C 473C Check if next vector norm is big enough. If not stop. 474C 475 IF ( MAXV.GT.TOLDEF ) THEN 476C 477C Do the swaping 478C 479 IF ( LPIV ) THEN 480 CALL DSWAP( N+1-J, A(J+1,J), LDA, A(IDXM,J), LDA ) 481 CALL DSWAP( N, A(1,J+1), 1, A(1,IDXM), 1 ) 482 CALL DSWAP( P, C(1,J+1), 1, C(1,IDXM), 1 ) 483 END IF 484C 485C Calculate Householder reflection. 486C 487 CALL DLARFG( N-J, A(J+1,J), A(J+2,J), 1, H ) 488 B1 = A(J+1,J) 489 A(J+1,J) = ONE 490C 491C Apply reflection to matrix A. 492C Workspace: need N. 493C 494 CALL DLARF( 'Right', N, N-J, A(J+1,J), 1, H, 495 $ A(1,J+1), LDA, DWORK ) 496 CALL DLARF( 'Left', N-J, N-J, A(J+1,J), 1, H, 497 $ A(J+1,J+1), LDA, DWORK ) 498C 499C Apply reflection to matrix C. 500C Workspace: need P. 501C 502 CALL DLARF( 'Right', P, N-J, A(J+1,J), 1, H, 503 $ C(1,J+1), LDC, DWORK ) 504C 505C Form orthogonal transformation matrix Z 506C Workspace: need N. 507C 508 IF ( LJOBI ) THEN 509 IF ( LPIV ) 510 $ CALL DSWAP( N, Z(1,J+1), 1, Z(1,IDXM), 1 ) 511C 512 CALL DLARF( 'Right', N, N-J, A(J+1,J), 1, H, 513 $ Z(1,J+1), LDZ, DWORK ) 514 END IF 515C 516 A(J+1,J) = B1 517 TAU(ITAU) = H 518 ITAU = ITAU + 1 519 J = J + 1 520 GOTO 20 521 ELSE 522 NCONT = J 523 END IF 524 END IF 525C 526C END WHILE 20 527C 528C Annihilate the lower part of A and B. 529C 530 IF ( N.GT.2 ) 531 $ CALL DLASET( 'Lower', N-2, NCONT, ZERO, ZERO, 532 $ A(3,1), LDA ) 533 IF ( N.GT.1 ) 534 $ CALL DLASET( 'Full', N-1, 1, ZERO, ZERO, B(2), N-1 ) 535 536 END IF 537C 538C If tolerance is not provided by user, recheck 539C the subspace dimensions. 540C 541 IF ( TOL.LE.ZERO ) THEN 542C 543C Find the absolute value of the diagonal 544C of the controllability matrix. 545C Workspace: need N. 546C 547 H = ABS( B(1) ) 548 DWORK(1) = H 549 MINV = H 550 J = 2 551 30 CONTINUE 552 IF ( J.LE.NCONT ) THEN 553 H = H*ABS( A(J,J-1) ) 554 IF ( MINV.GT.H ) 555 $ MINV = H 556 DWORK(J) = MINV 557 J = J + 1 558 GOTO 30 559 END IF 560C 561C Check if controllability matrix singular. 562C If yes use tolerance given in "Matrix computation 563C - Golub & van Loan, 3rd Edition, p. 345". 564C 565 IF ( MINV.LT.TOLDEF ) THEN 566 TOLDEF = 4.D0*N*TOLDEF 567 J = 1 568C 569C WHILE ( J < NCONT and ABS( A(J+1,J) ) > TOLDEF ) DO 570C 571 40 CONTINUE 572 IF ( J.LT.NCONT ) THEN 573 IF ( ABS( A(J+1,J) ).GT.TOLDEF ) THEN 574 J = J + 1 575 GO TO 40 576 END IF 577 END IF 578C 579C END WHILE 40 580C 581 IF ( DWORK(J).GT.TOLDEF ) 582 $ NCONT = J 583 END IF 584 END IF 585C 586C Undo scaling of A and B. 587C 588 CALL MB01PD( 'U', 'H', NCONT, NCONT, 0, 0, ANORM, 0, NBLK, A, 589 $ LDA, INFO ) 590 CALL MB01PD( 'U', 'G', 1, 1, 0, 0, BNORM, 0, NBLK, B, N, INFO ) 591 IF ( NCONT.LT.N ) 592 $ CALL MB01PD( 'U', 'G', N, N-NCONT, 0, 0, ANORM, 0, NBLK, 593 $ A(1,NCONT+1), LDA, INFO ) 594 ELSE 595C 596C B is negligible compared with A. No computations for reducing 597C the system to orthogonal canonical form have been performed, 598C except scaling (which is undoed). 599C 600 CALL MB01PD( 'U', 'G', N, N, 0, 0, ANORM, 0, NBLK, A, LDA, 601 $ INFO ) 602 CALL MB01PD( 'U', 'G', N, 1, 0, 0, BNORM, 0, NBLK, B, N, INFO ) 603 IF( LJOBF ) THEN 604 CALL DLASET( 'Full', N, N, ZERO, ZERO, Z, LDZ ) 605 CALL DLASET( 'Full', N, 1, ZERO, ZERO, TAU, N ) 606 ELSE IF( LJOBI ) THEN 607 CALL DLASET( 'Full', N, N, ZERO, ONE, Z, LDZ ) 608 END IF 609 END IF 610C 611C Set optimal workspace dimension. 612C 613 DWORK(1) = WRKOPT 614C 615 RETURN 616C *** Last line of TB01ZD *** 617 END 618