SUBROUTINE BALSVD(SYSFIL,BALFIL,RGORFL,BWHERE,WDUM,
1 ERRCD,ERRMSG)
C
C FUNCTION:
CF
CF This tool calculates the balancing transformation and the weighting matrix
CF used by the tools lqr and kbf to design a linear regulator or filter. This
CF transformation ensures that the non-zero singular values of the return
CF ratio of either the lqr or the kbf system are equal at the specified
CF frequency.
CF
C USAGE:
CU
CU
C INPUTS:
CI
CI
C OUTPUTS:
CO
CO
C ALGORITHM:
CA
CA The code in GFBA is used except for balancing at zero frequency, where there
CA are two choices. '0' specifies the use of GFBA, and 'L' or 'l' specifies
CA the use a simple pseudo-inverse. The cascade expert system uses 'L', not '0'.
CA
C MACHINE DEPENDENCIES:
CM
CM
C HISTORY:
CH
CH written by:
CH date:
CH current version:
CH modifications: 6/16/88: jdb -- changed zero-frequency balancing
CH method.
CH added dpcom: 7/16/88 jdb
CH corrected real,cmplx: 7/27/88 jdb
CH
C ROUTINES CALLED:
CC
CC
C COMMON MEMORY USED:
CM
CM DPCOM -- see dpcommon.f and dpcom.f
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 number 37B-07685C S13
C organization: The University of Tennessee
C----------------------------------------------------------------------
C THIS SOFTWARE IS IN THE PUBLIC DOMAIN
C NO RESTRICTIONS ON ITS USE ARE IMPLIED
C----------------------------------------------------------------------
C
C
INCLUDE 'Parameter.f'
C
CHARACTER*(*) SYSFIL
CHARACTER*(*) BALFIL
CHARACTER*(*) ERRMSG
C
CHARACTER*(*) RGORFL
CHARACTER*(*) BWHERE
C
DOUBLE PRECISION A(SIZE,SIZE)
DOUBLE PRECISION B(SIZE,SIZE)
DOUBLE PRECISION C(SIZE,SIZE)
DOUBLE PRECISION D(SIZE,SIZE)
C
DOUBLE PRECISION Q(SIZE,SIZE)
DOUBLE PRECISION BTEMP(SIZE,SIZE)
DOUBLE PRECISION EPS
C
DOUBLE PRECISION DREAL
C
DOUBLE COMPLEX W
DOUBLE COMPLEX WDUM
DOUBLE COMPLEX SYSA(SIZE,SIZE)
DOUBLE COMPLEX SYSB(SIZE,SIZE)
DOUBLE COMPLEX U(SIZE,SIZE)
DOUBLE COMPLEX V(SIZE,SIZE)
DOUBLE COMPLEX S(SIZE)
DOUBLE COMPLEX E(SIZE)
DOUBLE COMPLEX WORK(SIZE)
C
DOUBLE COMPLEX DCMPLX
C
INTEGER NSTATS
INTEGER NOUTS
INTEGER NINPS
INTEGER ERRCD
INTEGER IERR
C
LOGICAL INF
C
INCLUDE 'dpcom.f'
C
CALL INSYS(SYSFIL,NINPS,NOUTS,NSTATS,
2 SIZE,SIZE,SIZE,A,B,C,D,ERRCD)
CLOSE (UNIT=UNIT1)
IF (ERRCD .NE. 0) THEN
ERRMSG = 'BALSVD: Fatal error from INSYS '//
1 'accessing '//SYSFIL
RETURN
END IF
C
C--calculate machine epsilon
C
EPS = 1.D0
10 IF (EPS + 1.D0 .LE. 1.D0) GO TO 15
EPS = EPS / 2.D0
GO TO 10
15 EPS = EPS * 2.D0
C
C--set frequency for balancing at zero only if balancing at Low (0)
C--or High (infinite) frequency
C
IF (BWHERE(1:1) .EQ. 'F' .OR. BWHERE(1:1) .EQ. 'f') THEN
W = WDUM
ELSE
W = DCMPLX(0.D0,0.D0)
END IF
C
C--parameters describe:
C--RGORFL: 'F' or 'R' in byte 1, implying balance for filter or
C-- regulator design
C--BWHERE: '0', 'F', 'H' or 'L' in byte 1, implying balance at zero, finite,
C-- or infinite frequency, using the GFBA
C-- algorithm, or balance at zero frequency
C-- using the pseudo-inverse (old method),
C-- respectively.
C--WDUM : real(frequency) imag(frequency); double precision complex,
C-- specifying the real and
C-- imaginary frequency components;
C-- used only if record 2 is 'F'.
C
C--if balancing for filter design, use (trans(a),trans(c))
C
IF (RGORFL(1:1) .EQ. 'F' .OR. RGORFL(1:1) .EQ. 'f') THEN
CALL TRNATB (SIZE,SIZE,NSTATS,NSTATS,A,A)
CALL TRNATB (SIZE,SIZE,NOUTS,NSTATS,C,BTEMP)
NK = NOUTS
ELSE
C
C--if balancing for regulator design, use (a,b)
C
CALL SAVE (SIZE,SIZE,NSTATS,NINPS,B,BTEMP)
NK = NINPS
END IF
C
C--set infinite frequency flag
C
IF (BWHERE(1:1) .EQ. 'H' .OR. BWHERE(1:1) .EQ. 'h') THEN
INF = .TRUE.
ELSE
INF = .FALSE.
END IF
C
C--find the balancing weighting function Q
C
IF (BWHERE(1:1) .EQ. 'L' .OR. BWHERE(1:1) .EQ. 'l') THEN
C
C--use the quick & dirty pseudo-inverse method to balance at zero frequency
C--Note: assumes NK <= NSTATS (number of inputs/outpus <= number of states)
C
C
C--copy BTEMP into DOUBLE COMPLEX array
C
DO 60 J = 1, NK
DO 50 I = 1, NSTATS
SYSB(I,J) = DCMPLX(BTEMP(I,J),0.D0)
50 CONTINUE
60 CONTINUE
C
C--find SVD of BTEMP, saving only the left singular vectors
C--Note: the SVD is done in complex math only because of
C--compatibility with GFBA.
C
CALL ZSVDC (SYSB,SIZE,NSTATS,NK,S,E,U,SIZE,V,SIZE,
1 WORK,20,INFO)
IF (INFO .NE. 0) THEN
ERRCD = INFO
ERRMSG = 'BALSVD: Fatal error from ZSVDC.'//
1 ' ERRCD = INFO (from ZSVDC).'
RETURN
END IF
C
C--this is from laub's MULWOB, modified to use transpose(A), and
C--converted to DOUBLE COMPLEX. it computes transpose(A)*U, placing
C--the result in U.
C
DO 160 J=1,NK
DO 120 I=1,NSTATS
WORK(I)=0.0D0
120 CONTINUE
DO 140 I=1,NSTATS
DO 130 K=1,NSTATS
WORK(I)=WORK(I)+A(K,I)*U(K,J)
130 CONTINUE
140 CONTINUE
DO 150 I=1,NSTATS
U(I,J)=WORK(I)
150 CONTINUE
160 CONTINUE
C
C--end of MULWOB code
C
C
C--the following has been borrowed from GFBA (originally laub's mqf code)
C--to 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. NK .OR. DREAL(S(IRANK+1)) .LE. THRESH)
. GO TO 180
IRANK = IRANK + 1
GO TO 170
180 CONTINUE
C
DO 230 K=1,NSTATS
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,NSTATS
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 (NSTATS .NE. 1) THEN
C
C--determine the strict upper triangle of q by symmetry
C
DO 250 J=2,NSTATS
DO 240 I=1,J-1
Q(I,J)=Q(J,I)
240 CONTINUE
250 CONTINUE
END IF
C
C--end of code borrowed from GFBA
C
ELSE
C
C--use generalized balancing routine, "gfba", version 1.2
C
CALL GFBA (SIZE,NSTATS,NK,A,BTEMP,W,INF,Q,IERR,INFO,
& SYSA,SYSB,U,V,S,E,WORK)
IF (IERR .NE. 0) THEN
ERRCD = 1000 + IERR*10 + INFO
ERRMSG = 'BALSVD: Fatal error from GFBA calling ZSVDC.'//
1 ' ERRCD = 1000 + 10*IERR (from GFBA) + INFO '//
2 '(from ZSVDC).'
RETURN
END IF
END IF
C
C--open balance_filespec output file and write transformation
C--matrix and weighting matrix
C
OPEN (UNIT=UNIT8,FILE=BALFIL,ERR=9999)
C
C--the balancing matrix L (n_inputs x n_states)
C-- H
C-- of the complex Q = L *L when balancing the regulator, or
C-- T
C--the matrix R (n_states x n_outputs), of Q = R*R when balancing the filter
C--can be computed from
C-- H +
C-- L = R = U *S
C-- 1 1
C--where the "1" denotes the first n_inputs (for L) or n_outputs (for R)
C--columns of U and diagonal elements of S, as returned from GFBA.
C
C--multiply the first n_inputs (n_outputs) columns of U by corresponding
C--elements of S
C
IF (DREAL(S(1)) .LT. EPS) THEN
ERRCD = 999
ERRMSG = 'BALSVD: Solution to balancing problem is zero!'
RETURN
END IF
C
DO 1100 I = 1, NK
DO 1000 J = 1, NSTATS
U(J,I) = U(J,I) / S(I)
1000 CONTINUE
1100 CONTINUE
C
C
C--write out U as an n_states x n_outputs matrix (R) for the filter
C
IF (RGORFL(1:1) .EQ. 'F' .OR. RGORFL(1:1) .EQ. 'f') THEN
C
DO 20, I = 1, NSTATS
WRITE (UNIT8,*) (U(I,J),J=1,NK)
20 CONTINUE
C
ELSE
C
C--write out U as an n_inputs x n_states matrix (L) for the regulator
C
DO 30, I = 1, NK
WRITE (UNIT8,*) (U(J,I),J=1, NSTATS)
30 CONTINUE
C
END IF
C
DO 40,I = 1, NSTATS
WRITE (UNIT8,*) (Q(I,J),J=1, NSTATS)
40 CONTINUE
C
CLOSE (UNIT=UNIT8)
C
C-- THE END
C
RETURN
9999 ERRCD=1
ERRMSG = 'BALSVD: Fatal error accessing BALFIL '//BALFIL
CLOSE (UNIT=UNIT8)
RETURN
C
END