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