SUBROUTINE GFBA (LDA,N,M,A,B,W,INF,Q,IERROR,INFO, & SYSA,SYSB,U,V,S,E,WORK) C C FUNCTION: CF CF General Frequency Balancing Algorithm. This subroutine computes CF a "balancing" matrix L, which is in general complex, and a weighting CF matrix Q, which is real. The matrix Q can be used as either the CF state weighting matrix for the LQR problem, or as the process CF noise covariance for the KBF problem, to produce a regulator or CF filter design which has all of the finite singular values of CF its return ratio equal at the specified frequency. All complex CF frequencies, including infinity, are allowable. CF C USAGE: CU CU CALL GFBA (LDA,N,M,A,B,W,INF,Q,IERROR,INFO, CU & SYSA,SYSB,U,V,S,E,WORK) CU C INPUTS: CI CI LDA integer; the leading dimension of all arrays in the CI main calling program. CI CI N integer; the number of states of the system. CI CI M integer; the number of inputs to the system. CI CI A double precision(LDA,N); array containing the CI N x N system dynamics matrix A. CI CI B double precision(LDA,M); array containing the CI N x M system input matrix B. CI CI W DOUBLE COMPLEX scalar; this is the complex-valued CI frequency parameter at which the singular values CI are to be balanced. For continuous time systems, CI this is normally of the form 0+j*w; for discrete CI time systems, this is normally of the form exp(j*theta). CI CI INF logical scalar; if this variable is TRUE, the CI subroutine assumes |w| is infinity, and solves for CI the limiting case, where L is the moore-penrose CI inverse of B, and L*inverse(wI-A)*B -> K/w as CI |w| -> infinity. CI C OUTPUTS: CO CO Q double precision(LDA,N); array containing the CO N x N positive semi-definite matrix to be used CO either as a state weighting matrix for the regulator, CO or as a process noise covariance matrix for the CO filter design. CO CO IERROR integer; a non-zero value on return indicates trouble. CO IERROR = 1 occurs if the SVD of (wI-A) failed; CO IERROR = 2 occurs if the SVD of pseudoinverse(wI-A)*B CO failed. In these cases, INFO contains the information CO returned from the LINPACK routine ZSVDC; see LINPACK CO documentation for further details. CO CO INFO integer; see description of IERROR CO CO SYSA DOUBLE COMPLEX(LDA,N); working array. CO CO SYSB DOUBLE COMPLEX(LDA,M); working array. CO CO U DOUBLE COMPLEX(LDA,N); working array. CO CO V DOUBLE COMPLEX(LDA,N); working array. CO CO S DOUBLE COMPLEX(N); working vector. CO CO E DOUBLE COMPLEX(N); working vector. CO CO WORK DOUBLE COMPLEX(N); working vector. CO C ALGORITHM: CA CA GFBA uses the Laub/Birdwell algorithm [1] for computing the CA solution L to L*inverse(wI-A)*B = K where K is diagonal with CA either 0 or 1 elements along the diagonal. L is never CA explicitly computed or returned. GFBA returns the CA H CA real matrix Q = real(L *L), to be used as a weighting or CA covariance matrix in later stages of an LQG/LTR design. This CA algorithm extends and supercedes all previous algorithms. CA CA For the case where |w| = infinity, GFBA computes the solution CA L to L*B = K, causing L*inverse(wI-A)*B -> K/w as |w| -> infinity. CA CA REFERENCE: CA CA [1] J. D. Birdwell and A. J. Laub, "Balanced singular values CA for LQG/LTR design," Proc. 1986 American Control Conference, CA Seattle, WA, June, 1986. CA C MACHINE DEPENDENCIES: CM CM requires DOUBLE COMPLEX arithmetic. CM C HISTORY: CH CH written by: J. Douglas Birdwell & Alan J. Laub CH date: September 18, 1985 CH current version: 1.2 CH modifications: 11/13/85 JDB: Removed VAX/VMS dependencies CH 11/13/85 JDB: Added |w| = infinity case CH 12/16/85 JDB: Corrected mp-inv(jwI-A) calc. CH 8/18/85 BB: Modified to make standard. CH 7/27/88 JDB: Corrected cmplx,real. CH C ROUTINES CALLED: CC CC ZSVDC (LINPACK) CC C COMMON MEMORY USED: CM CM NONE CM C---------------------------------------------------------------------- C written for: The CASCADE Project C Oak Ridge National Laboratory C U.S. Department of Energy C contract number DE-AC05-840R21400 C subcontract numbers 37B-7685 S13 & 11X-39060V C organization: University of Tennessee & AJL C---------------------------------------------------------------------- C THIS SOFTWARE IS IN THE PUBLIC DOMAIN C NO RESTRICTIONS ON ITS USE ARE IMPLIED C---------------------------------------------------------------------- C C--GLOBAL VARIABLES C INTEGER LDA INTEGER N INTEGER M DOUBLE PRECISION A(LDA,N) DOUBLE PRECISION B(LDA,M) DOUBLE COMPLEX W LOGICAL INF DOUBLE PRECISION Q(LDA,N) INTEGER IERROR INTEGER INFO DOUBLE COMPLEX SYSA(LDA,N) DOUBLE COMPLEX SYSB(LDA,M) DOUBLE COMPLEX U(LDA,N) DOUBLE COMPLEX V(LDA,N) DOUBLE COMPLEX S(N) DOUBLE COMPLEX E(N) DOUBLE COMPLEX WORK(N) C C--LOCAL VARIABLES C INTEGER IRANK INTEGER I INTEGER J INTEGER K DOUBLE PRECISION EPS DOUBLE PRECISION THRESH C DOUBLE PRECISION DREAL DOUBLE PRECISION DCMPLX C C--START OF CODE C C--calculate machine epsilon C EPS = 1.D0 10 IF (EPS + 1.D0 .LE. 1.D0) GO TO 20 EPS = EPS / 2.D0 GO TO 10 20 EPS = EPS * 2.D0 C C--if |w| = infinity, do not worry about wI-A C IF (.NOT. INF) THEN C C--form wI-A in sysa array C DO 40 J = 1, N DO 30 I = 1, N SYSA(I,J) = DCMPLX(-A(I,J),0.D0) 30 CONTINUE SYSA(J,J) = W + SYSA(J,J) 40 CONTINUE END IF C C--copy B into DOUBLE COMPLEX array C DO 60 J = 1, M DO 50 I = 1, N SYSB(I,J) = DCMPLX(B(I,J),0.D0) 50 CONTINUE 60 CONTINUE C C--do the following code only if |w| < infinity C IF (.NOT. INF) THEN C------------------------------------------------------------------------ C SVD-based method C------------------------------------------------------------------------ C C--solve (wI-A)*X = B for X using SVD C CALL ZSVDC (SYSA,LDA,N,N,S,E,U,LDA,V,LDA,WORK,21,INFO) IF (INFO .NE. 0) THEN IERROR = 1 C--error flag of 1; svd of (wI-A) failed; see info for details RETURN END IF C C--form the mp-inverse of (wI-A) in SYSA C THRESH = DREAL(S(1)) * SQRT(EPS) IF (DREAL(S(1)) .EQ. 0.D0) THRESH = EPS C DO 80 J = 1, N DO 70 I = 1, N SYSA(I,J) = DCMPLX(0.D0,0.D0) 70 CONTINUE 80 CONTINUE C DO 110 K = 1, N IF (DREAL(S(K)) .GE. THRESH) THEN DO 100 J = 1, N DO 90 I = 1, N SYSA(I,J) = SYSA(I,J) + V(I,K)*CONJG(U(J,K))/S(K) 90 CONTINUE 100 CONTINUE END IF 110 CONTINUE C C C--this is from laub's MULWOB, converted to DOUBLE COMPLEX C--and computes pseudoinverse(wI-A)*B, placing the result in SYSB C DO 160 J=1,M DO 120 I=1,N WORK(I)=0.0D0 120 CONTINUE DO 140 K=1,N DO 130 I=1,N WORK(I)=WORK(I)+SYSA(I,K)*SYSB(K,J) 130 CONTINUE 140 CONTINUE DO 150 I=1,N SYSB(I,J)=WORK(I) 150 CONTINUE 160 CONTINUE C C--end of code which is executed only when |w| < infinity C END IF C H C--for |w| < infinity, calculate SVD of X = U*S*V ; don't compute V C H C--for |w| = infinity, calculate SVD of B = U*S*V ; don't compute V C CALL ZSVDC (SYSB,LDA,N,M,S,E,U,LDA,V,LDA,WORK,20,INFO) IF (INFO .NE. 0) THEN IERROR = 2 C--error flag of 2; svd of X=pseudoinverse(wI-A)*B failed c--see info for details RETURN END IF C H + + H C--calculate Q = real(L * L) = real(U *S *S *U ) C 1 1 1 1 C--where C S is the upper left square matrix of all non-zero C 1 C singular values C C--and C U is the matrix containing the first rank(S) columns of U C 1 C C--the following has been borrowed from laub's mqf code C--compute the lower triangle of q C THRESH = DREAL(S(1)) * SQRT(EPS) IF (DREAL(S(1)) .EQ. 0.D0) THRESH = EPS C IRANK = 0 170 IF (IRANK .GE. M .OR. DREAL(S(IRANK+1)) .LE. THRESH) GO TO 180 IRANK = IRANK + 1 GO TO 170 180 CONTINUE C DO 230 K=1,N C DO 190 I = 1, IRANK WORK(I) = 0.0D0 190 CONTINUE C DO 200 I = 1, IRANK WORK(I) = WORK(I) + (CONJG(U(K,I))/S(I))/S(I) 200 CONTINUE C DO 220 I=K,N Q(I,K) = 0.0D0 DO 210 J = 1, IRANK Q(I,K) = Q(I,K) + DREAL(WORK(J)*U(I,J)) 210 CONTINUE 220 CONTINUE C 230 CONTINUE C IF (N .NE. 1) THEN C C--determine the strict upper triangle of q by symmetry C DO 250 J=2,N DO 240 I=1,J-1 Q(I,J)=Q(J,I) 240 CONTINUE 250 CONTINUE END IF C C--return C IERROR = 0 RETURN END