SUBROUTINE LTRKBF(SYSFIL,INSFIL,LQRFIL,COVFIL, 1 OUTFIL,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 B1(SIZE2,SIZE2) DOUBLE PRECISION G(SIZE2,SIZE2) DOUBLE PRECISION ACL(SIZE2,SIZE2) C DOUBLE PRECISION W0(SIZE2,SIZE2) DOUBLE PRECISION V(SIZE2,SIZE2) C DOUBLE PRECISION RTOL DOUBLE PRECISION RSD DOUBLE PRECISION Q2 DOUBLE PRECISION EPS DOUBLE PRECISION EPSLON DOUBLE PRECISION SIGMAX C DOUBLE PRECISION ID(SIZE2,SIZE2) 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 RI(SIZE2,SIZE2) DOUBLE PRECISION RS(SIZE2,SIZE2) DOUBLE PRECISION S(SIZE2,SIZE2) DOUBLE PRECISION U(SIZE2,SIZE2) DOUBLE PRECISION WK(SIZE2,SIZE2) DOUBLE PRECISION CPERM(SIZE2) DOUBLE PRECISION CSCALE(SIZE2) DOUBLE PRECISION ER(SIZE2) DOUBLE PRECISION EI(SIZE2) DOUBLE PRECISION W1(SIZE2) C DOUBLE PRECISION BBT(SIZE2,SIZE2) DOUBLE PRECISION FCL(SIZE2,SIZE2) DOUBLE PRECISION F(SIZE2,SIZE2) DOUBLE PRECISION WORK(SIZE2) DOUBLE PRECISION SIGMA(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 NINPS2 INTEGER NDMNUL C INTEGER IND(SIZE2) INTEGER IERR INTEGER MAXIT INTEGER RSTRUC INTEGER ERRCD C CHARACTER*(*) SYSFIL CHARACTER*(*) INSFIL CHARACTER*(*) LQRFIL CHARACTER*(*) COVFIL CHARACTER*(*) OUTFIL CHARACTER*(*) ERRMSG C INCLUDE 'dpcom.f' C C--define MAXIT, RSTRUC, and RTOL. C ERRCD = 0 IERR = 0 MAXIT = 5 RSTRUC = 2 RTOL = 0.0D0 C C--get original system description C CALL INSYS (SYSFIL,NINPS,NOUTS,NSTATS, 2 SIZE2,SIZE2,SIZE2,A,B,C,D,ERRCD) CLOSE (UNIT=UNIT1) IF (ERRCD .NE. 0) THEN ERRCD = 100 + ERRCD ERRMSG = 'LTRKBF: Fatal error from INSYS reading '// 1 ' system file '//SYSFIL RETURN END IF C C--get lqr design; note that a, b, and d are the same--we only need g C CALL INSYS (LQRFIL,NINPS,NINPS,NSTATS, 1 SIZE2,SIZE2,SIZE2,A,B,G,D,ERRCD) IF (ERRCD .NE. 0) THEN ERRCD = 200 + ERRCD ERRMSG = 'LTRKBF: Fatal error from INSYS reading '// 1 ' the LQR file '//LQRFIL CLOSE (UNIT=UNIT1) RETURN END IF C C--and get the closed loop regulator dynamics matrix. C DO 10, I = 1,NSTATS READ (UNIT1,*,END=9990,ERR=9990) (ACL(I,J),J=1,NSTATS) 10 CONTINUE C CLOSE (UNIT=UNIT1) C C--if nouts=ninps, system is already square, so we can ignore the insfil C IF (NOUTS .NE. NINPS) THEN CALL INSYS (INSFIL,NINPS2, 1 NOUTS,NSTATS, 2 SIZE2,SIZE2,SIZE2,A,B1,C,D,ERRCD) IF (ERRCD .NE. 0) THEN ERRCD = 300 + ERRCD ERRMSG = 'LTRKBF: Fatal error from INSYS reading the '// 1 'square system file '//INSFIL CLOSE (UNIT=UNIT1) RETURN END IF C IF (NINPS2 .NE. NOUTS) THEN ERRCD = 400 ERRMSG = 'LTRKBF: Fatal Error INSFIL system '// 1 'not square, i.e., does not have equal '// 2 'numbers of inputs and outputs.' CLOSE (UNIT=UNIT1) RETURN END IF CLOSE (UNIT=UNIT1) C C--if ninps = nouts, just copy b to b1 for later use C ELSE C CALL SAVE (SIZE2,SIZE2,NSTATS,NINPS,B,B1) C END IF C C--read the covariance matrices. C--note that since the matrices are symmetric, we can C--read them column-wise, gaining some efficiency C OPEN (UNIT=UNIT1,FILE=COVFIL,STATUS='OLD',ERR=9991) C DO 20, I = 1, NSTATS READ (UNIT1,*,END=9991,ERR=9991) (W0(J,I),J=1,NSTATS) 20 CONTINUE C DO 30, I = 1, NOUTS READ (UNIT1,*,END=9991,ERR=9991) (V(J,I),J=1,NOUTS) 30 CONTINUE C CLOSE (UNIT=UNIT1) C C--end of input C--start of kbf design C C--form process noise covariance w = w0 + q2*b1*b1:t C--(store in w0) C CALL XYT (SIZE2,SIZE2,SIZE2,NSTATS, 2 NOUTS,NSTATS,B1,B1,BBT) C CALL MSCALE (SIZE2,NSTATS,NSTATS,Q2,BBT) C CALL MADD (SIZE2,SIZE2,SIZE2,NSTATS,NSTATS, 2 BBT,W0,W0) C C--now calculate the Kalman filter C DO 70, I=1,NSTATS DO 60, J=1,NSTATS ID(I,J) = 0.0D0 60 CONTINUE ID(I,I) = 1.0D0 70 CONTINUE C CALL CKBF(SIZE2,NSTATS,NSTATS,NOUTS,A,ID,C,V,W0,SIGMA,F,FCL, 1 T1,T2,T3,Z1,Z2,RI,RS,S,U,WK,EI,ER,W1,CPERM, 2 CSCALE,RSD,RTOL,MAXIT,RSTRUC,IBAL,IND,IERR,ERRMSG) C IF (IERR.GT.0) THEN ERRCD = IERR RETURN END IF C C C C--form alqg, blqg, clqg C CALL MMUL (SIZE2,SIZE2,SIZE2,NSTATS, 1 NSTATS,NOUTS,F,C,ALQG(1,NSTATS+1)) CALL MSUB (SIZE2,SIZE2,SIZE2,NSTATS,NSTATS, 1 ACL,ALQG(1,NSTATS+1),ALQG(1,1)) CALL SAVE (SIZE2,SIZE2,NSTATS,NSTATS, 1 A,ALQG(NSTATS+1,NSTATS+1)) C 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,NINPS, 1 B,BLQG(NSTATS+1,1)) CALL SAVE (SIZE2,SIZE2,NSTATS,NINPS, 1 ALQG(NSTATS+1,1),BLQG(1,1)) C CALL SAVE (SIZE2,SIZE2,NINPS,NSTATS,G,CLQG(1,1)) CALL SAVE (SIZE2,SIZE2,NINPS,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,NINPS, 1 NINPS,2*NSTATS, 2 SIZE2,SIZE2,SIZE2,ALQG,BLQG,CLQG,D,ERRCD) IF (ERRCD .NE. 0) THEN ERRCD = 500 ERRMSG = 'LTRKBF: Fatal error from OUTSYS.' CLOSE (UNIT=UNIT1) RETURN END IF C C--write out Kalman filter solution C DO 80, I = 1, NSTATS WRITE (UNIT1,*,ERR=9993) (FCL(I,J),J=1,NSTATS) 80 CONTINUE C DO 90, I = 1, NSTATS WRITE (UNIT1,*,ERR=9993) (F(I,J),J=1,NOUTS) 90 CONTINUE C DO 100, I = 1, NSTATS WRITE (UNIT1,*,ERR=9993) (SIGMA(I,J),J=1,NSTATS) 100 CONTINUE C C--write out filter eigenvalues C DO 110, I = 1, NSTATS WRITE (UNIT1,*,ERR=9993) ER(I),EI(I) 110 CONTINUE C C--close output file C CLOSE (UNIT=UNIT1,ERR=9993) C C--the end C RETURN C C--error trap for i/o problems C 9990 ERRCD = 600 ERRMSG = 'LTRKBF: Error reading additional '// 1 'information from LQR file '//LQRFIL RETURN 9991 ERRCD = 700 ERRMSG = 'LTRKBF: Error reading from covariance '// 1 'file '//COVFIL RETURN 9993 ERRCD = 800 ERRMSG = 'LTRKBF: Error writing to output file '//OUTFIL RETURN END