        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