SUBROUTINE LTRLQR(SYSFIL,INFIL,KBFFIL,WGTFIL,OUTFIL, 1 Q2,ERRCD,ERRMSG) C C FUNCTION: CF CF C USAGE: CU CU C INPUTS: CI CI C OUTPUTS: CO CO C ALGORITHM: CA CA C MACHINE DEPENDENCIES: CM CM C HISTORY: CH CH written by: CH date: CH current version: CH modifications: CH added dpcom: 7/16/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 DOUBLE PRECISION A(SIZE2,SIZE2) DOUBLE PRECISION B(SIZE2,SIZE2) DOUBLE PRECISION C(SIZE2,SIZE2) DOUBLE PRECISION D(SIZE2,SIZE2) C DOUBLE PRECISION C1(SIZE2,SIZE2) DOUBLE PRECISION F(SIZE2,SIZE2) DOUBLE PRECISION FCL(SIZE2,SIZE2) C DOUBLE PRECISION Q0(SIZE2,SIZE2) DOUBLE PRECISION ID(SIZE2,SIZE2) DOUBLE PRECISION R(SIZE2,SIZE2) DOUBLE PRECISION S(SIZE2,SIZE2) DOUBLE PRECISION WORK(SIZE2,SIZE2) DOUBLE PRECISION CPERM(SIZE2) DOUBLE PRECISION CSCALE(SIZE2) DOUBLE PRECISION RS(SIZE2,SIZE2) C DOUBLE PRECISION Q2 DOUBLE PRECISION RSD DOUBLE PRECISION RTOL DOUBLE PRECISION EPS DOUBLE PRECISION EPSLON DOUBLE PRECISION SIGMAX C DOUBLE PRECISION RV1(SIZE2) DOUBLE PRECISION Z1(SIZE2,SIZE2) DOUBLE PRECISION Z2(SIZE2,SIZE2) DOUBLE PRECISION T1(SIZE2,SIZE2) DOUBLE PRECISION T2(SIZE2,SIZE2) DOUBLE PRECISION T3(SIZE2,SIZE2) DOUBLE PRECISION ER(SIZE2) DOUBLE PRECISION EI(SIZE2) DOUBLE PRECISION W1(SIZE2) C DOUBLE PRECISION CTC(SIZE2,SIZE2) DOUBLE PRECISION ACL(SIZE2,SIZE2) DOUBLE PRECISION G(SIZE2,SIZE2) DOUBLE PRECISION K(SIZE2,SIZE2) DOUBLE PRECISION ALQG(SIZE2,SIZE2) DOUBLE PRECISION BLQG(SIZE2,SIZE2) DOUBLE PRECISION CLQG(SIZE2,SIZE2) C INTEGER NINPS INTEGER NOUTS INTEGER NSTATS INTEGER NOUTS2 INTEGER NDIMNL C INTEGER ITYP(SIZE2) INTEGER IPVL(SIZE2) INTEGER IPVS(SIZE2) INTEGER MAXIT INTEGER RSTRUC INTEGER IERR INTEGER ERRCD C CHARACTER*(*) SYSFIL CHARACTER*(*) INFIL CHARACTER*(*) KBFFIL CHARACTER*(*) WGTFIL CHARACTER*(*) OUTFIL CHARACTER*(*) ERRMSG C INCLUDE 'dpcom.f' C C Initialize RSTRUC, MAXIT, and RTOL. C ERRCD = 0 RTOL = 0.0D0 MAXIT = 5 RSTRUC = 2 C C--get original system description C CALL INSYS (SYSFIL,NINPS,NOUTS,NSTATS, 2 SIZE2,SIZE2,SIZE2,A,B,C,D,ERRCD) IF (ERRCD .NE. 0) THEN ERRMSG = 'LTRLQR: Fatal error from INSYS reading '// 1 'system file '//SYSFIL CLOSE (UNIT=UNIT1) RETURN END IF CLOSE (UNIT=UNIT1) C C--get kbf design; note that a, c, and d are the same--we only need f C CALL INSYS (KBFFIL,NOUTS,NOUTS,NSTATS, 2 SIZE2,SIZE2,SIZE2,A,F,C,D,ERRCD) IF (ERRCD .NE. 0) THEN ERRMSG = 'LTRLQR: Fatal error from INSYS reading '// 1 'KBF file '//KBFFIL CLOSE (UNIT=UNIT1) RETURN END IF C C--and get the closed loop filter dynamics matrix. C DO 10, I =1,NSTATS READ (UNIT1,*,END=9990,ERR=9990) (FCL(I,J),J=1,NSTATS) 10 CONTINUE C CLOSE (UNIT=UNIT1) C C--if nouts=ninps, we can ignore the infil C IF (NOUTS .NE. NINPS) THEN CALL INSYS (INFIL,NINPS, 1 NOUTS2,NSTATS, 2 SIZE2,SIZE2,SIZE2,A,B,C1,D,ERRCD) IF (ERRCD .NE. 0) THEN ERRMSG = 'LTRLQR: Fatal error from INSYS reading '// 1 'from square system file '//INFIL CLOSE (UNIT=UNIT1) RETURN END IF C IF (NINPS .NE. NOUTS2) THEN ERRMSG = 'LTRLQR: Fatal error. INFIL does '// 2 'not have equal numbers of inputs and outputs.' CLOSE (UNIT=UNIT1) RETURN END IF CLOSE (UNIT=UNIT1) C C--if ninps = nouts, just copy c to c1 for later use C ELSE C CALL SAVE (SIZE2,SIZE2,NOUTS,NSTATS,C,C1) C END IF C C--read the weighting matrices. C--note that since the matrices are symmetric, we can C--read them column-wise, gaining some efficiency C OPEN (UNIT=UNIT1,FILE=WGTFIL,STATUS='OLD',ERR=9991) C DO 20, I = 1, NSTATS READ (UNIT1,*,END=9991,ERR=9991) (Q0(J,I),J=1,NSTATS) 20 CONTINUE C DO 30, I = 1, NINPS READ (UNIT1,*,END=9991,ERR=9991) (R(J,I),J=1,NINPS) 30 CONTINUE C CLOSE (UNIT=UNIT1) C C--end of parameter input C--start of lqr design C C C--form state weighting matrix q = q0 + q2*c:t*c C--(store in q0) C CALL XTY (SIZE2,SIZE2,SIZE2,NOUTS, 1 NSTATS,NSTATS,C1,C1,CTC) C CALL MSCALE (SIZE2,NSTATS,NSTATS,Q2,CTC) C CALL MADD (SIZE2,SIZE2,SIZE2,NSTATS,NSTATS, 1 CTC,Q0,Q0) C C--now calculate the regulator C DO 55 I=1,NSTATS DO 54 J=1,NSTATS ID(I,J) = 0.0D0 54 CONTINUE ID(I,I) = 1.0D0 55 CONTINUE C CALL CREG(SIZE2,NSTATS,NINPS,NSTATS,A,B,ID,R,Q0,T1,K,G, 1 ACL,T2,T3,Z1,RS,S,Z2,WORK,EI,ER,W1,CPERM,CSCALE, 2 RSD,RTOL,MAXIT,RSTRUC,IBAL,ITYP,IERR,ERRMSG) C IF (IERR .GT. 0) THEN ERRCD = IERR RETURN END IF C C--form alqg,blqg,clqg C CALL MMUL (SIZE2,SIZE2,SIZE2,NSTATS,NSTATS,NINPS, 1 B,G,ALQG(1,NSTATS+1)) CALL MSUB (SIZE2,SIZE2,SIZE2,NSTATS,NSTATS, 1 FCL,ALQG(1,NSTATS+1), 2 ALQG(NSTATS+1,NSTATS+1)) CALL SAVE (SIZE2,SIZE2,NSTATS,NSTATS,A,ALQG(1,1)) DO 1112 I = 1, NSTATS DO 1111 J = NSTATS+1, 2*NSTATS ALQG(J,I) = 0.D0 1111 CONTINUE 1112 CONTINUE C CALL SAVE (SIZE2,SIZE2,NSTATS,NOUTS, 1 F,BLQG(NSTATS+1,1)) CALL SAVE (SIZE2,SIZE2,NSTATS,NOUTS, 1 ALQG(NSTATS+1,1),BLQG(1,1)) C CALL SAVE (SIZE2,SIZE2,NOUTS,NSTATS,C,CLQG(1,1)) CALL SAVE (SIZE2,SIZE2,NOUTS,NSTATS, 1 ALQG(NSTATS+1,1),CLQG(1,NSTATS+1)) C C--and write out alqg,blqg,clqg,and d in new system file C CALL OUTSYS (OUTFIL,NOUTS, 1 NOUTS,2*NSTATS, 2 SIZE2,SIZE2,SIZE2,ALQG,BLQG,CLQG,D,ERRCD) IF (ERRCD .NE. 0) THEN ERRMSG = ' LTRLQR: Fatal error from OUTSYS '// 1 'writing '//OUTFIL CLOSE (UNIT=UNIT1) RETURN END IF C C--write out regulator solution C DO 60, I = 1, NSTATS WRITE (UNIT1,*,ERR=9999) (ACL(I,J),J=1,NSTATS) 60 CONTINUE C DO 70, I = 1, NINPS WRITE (UNIT1,*,ERR=9999) (G(I,J),J=1,NSTATS) 70 CONTINUE C DO 80, I = 1, NSTATS WRITE (UNIT1,*,ERR=9999) (K(I,J),J=1,NSTATS) 80 CONTINUE C C--write out filter eigenvalues C DO 90, I = 1, NSTATS WRITE (UNIT1,*,ERR=9999) ER(I),EI(I) 90 CONTINUE C C--close output file C CLOSE (UNIT=UNIT1) C C--the end C RETURN C C--ERROR TRAP FOR I/O PROBLEMS C 9990 ERRCD = 1 ERRMSG = 'LTRLQR: Fatal error reading FCL from '// 1 'KBF file '//KBFFIL RETURN 9991 ERRCD = 2 ERRMSG = 'LTRLQR: Fatal error reading from '// 1 'weighting file '//WGTFIL RETURN 9999 ERRCD = 3 ERRMSG = ' LTRLQR: Fatal error writing to '//OUTFIL RETURN END