SUBROUTINE CKBF(NR,N,M,L,A,G,C,R,D,P,F,FCL,GDG,WXYZ,AT, 1 CT,E,RI,RS,S,U,WK,ALFI,ALFR,BETA, 2 CPERM,CSCALE,RSD,RTOL,MAXIT, 3 RSTRUC,IBAL,IND,IERR,ERRMSG) C C FUNCTION: CF CF The subroutine CKBF designs a steady state Kalman-Bucy CF Filter of the form CF CF Xhat'(t) = A*Xhat(t) + F*(Y(t) - Xhat(t)) CF CF for the continous-time linear system: CF CF X'(t) = A*X(t) + Gd(t) CF Y(t) = C*X(t) + r(t) CF CF where CF A is the N by N state matrix. CF C is the L by N output matrix. CF G is an N by M matrix, usually the identity. CF d(t) is the input noise process with CF M by M input noise covariance matrix CF D. The input noise is assumed to CF be white Gaussian. CF r(t) is the observation noise process CF with L by L observation noise co- CF variance matrix R, also assumed CF to be white Gaussian. CF CF The filter gain matrix F is of the form CF CF T -1 CF F = P * C * R CF CF where P is the solution to the continuous time CF Algebraic Riccati Equation (ARE): CF CF T T -1 T CF A*P + P*A - P*C *R *C*P + G*D*G = 0 CF CF The closed loop gain matrix FCL of the system is then given CF by: CF FCL = A - F*C. CF C USAGE: CU CU The subroutine CKBF can be used to design a Kalman filter for CU a given system. The subroutine is invoked by the call CU CU SUBROUTINE CKBF(NR,N,M,L,A,G,C,R,D,P,F,FCL,GDG,WXYZ,AT, CU 1 CT,E,RI,RS,S,U,WK,ALFI,ALFR,BETA, CU 2 CPERM,CSCALE,RSD,RTOL,MAXIT, CU 3 RSTRUC,IBAL,IND,IERR,ERRMSG) CU CU where the variables are described below. When the subroutine CU is called, it is only necessary to have NR, N, M, L, A, G, C, CU R, D, RTOL, MAXIT, and RSTRUC initialized. CU C INPUTS: CI CI NR - INTEGER variable which is the row dimension of CI all the arrays as declared in the calling program. CI CI N - INTEGER variable; the order the system. CI CI M - INTEGER variable; the number of system inputs. CI CI L - INTEGER variable; the number of system outputs. CI CI A - DP variable; the N by N `A' (state) matrix of the system. CI CI C - DP variable; the L by N `C' (output) matrix of the system. CI CI R - DP variable; the L by L control weighting matrix. CI CI WXYZ - DP variable; a scratch array. CI CI G - DP variable; the N by M input noise matrix. CI CI D - DP variable; the M by M input noise covariance matrix. CI CI AT - DP variable; a scratch array. CI CI CT - DP variable; a scratch array. CI CI GDG - DP variable; a scratch array. CI CI E - DP variable; a scratch array. CI CI S - DP variable; a scratch array. CI CI U - DP variable; a scratch array. CI CI WK - DP variable; a scratch array. CI CI RTOL - DP variable; set this to the worst condition R can be CI and still be considered invertible. The condition RTOL CI is compared to is 1/RCOND where RCOND is the condition CI estimate used in LINPACK. If RTOL=0, then a default CI value of 1.0D+6 is used. CI CI MAXIT - the maximum number of iterations to use for improving CI the accuracy of the solution by Newton's method. CI CI RSTRUC - INTEGER variable indicating the structure of the CI R matrix: CI RSTRUC = 0 if R is the identity; CI RSTRUC = 1 if R is diagonal but CI not the identity; CI RSTRUC = 2 otherwise. CI CI IBAL - INTEGER variable determining if the equation is CI balanced: CI IBAL = 1 for WARD (pre-QZ) balancing. CI IBAL = 0 for no balancing. CI C OUTPUTS: CO CO P - DP variable; P contains the N by N solution to the ARE. CO CO F - DP variable; F contains the N by L optimal gain matrix. CO CO FCL - DP variable; FCL contains the N by N closed loop CO matrix for the system, which is A-F*C. CO CO RI - DP variable; contains the L by L matrix R-inverse; CO however, the program may not have have used CO R-inverse in the calculation of the ARE solution. CO CO AT - DP variable; contains the N by N matrix A-transpose. CO CO CT - DP variable; contains the L by N matrix C-transpose. CO CO RS - DP variable; contains the N by N residual matrix of CO of the ARE with F as the solution. CO CO RSD - DP variable; the 1-Norm of the residual matrix RS CO divided by the 1-Norm of the solution matrix F. CO CO MAXIT - INTEGER variable; on output, MAXIT contains the CO number Newton iterations actually performed by CO NEWT. A number less than the original MAXIT is CO not necessarily bad since the solution may have CO converged. CO CO GDG - The matrix contains the product G*D*(G-transpose). CO CO ALFR - DP variable; this matrix contains the real parts of CO the eigenvalues of FCL. CO CO ALFI - DP variable; this matrix contains the imaginary parts CO of the eigenvalues of FCL. CO CO IERR - An INTEGER variable indicating whether an error or CO warning has occured during execution. CO IERR = 0 for a normal return. CO IERR < 0 is a warning, indicating abnormal CO but nonfatal conditions. NOTE that if CO a warning occurs, then it is the last CO warning condition which occured, and CO therefore may be misleading, since an CO earlier warning may have caused other CO warnings and then been masked. CO IERR > 0 indicates a fatal error. CO CO ERRMSG = a CHARACTER variable which is only meaningful if CO IERR .NE. 0, it contains a description of the CO error or warning. CO CO C ALGORITHM: CA CA The algorithm and much of the code is adapted (copied where CA possible) from the interactive driver program to RICPACK. CA See: CA 1. Arnold, W. F., and A. J. Laub, "Generalized CA eigenproblem algorithms and software for CA algebraic Riccati equations," Proceedings of CA the IEEE, vol. 72, no. 12, December 1984, CA pp. 1746-1754. CA CA 2. The RICPACK code. CA CA Note that this program currently only solves the ARE for CA the case where S=0 and E=I. CA C MACHINE DEPENDENCIES: CM CM None. CM C HISTORY: CH CH written by: Bobby Bodenheimer CH date: September 1986 CH current version: 1.2 CH modifications: zero'ed out cross term S - jdb - 5/10/88 CH defined undefined variables - jdb - 5/13/88 CH C ROUTINES CALLED: CC CC RINV, RICSOL, CMPRS, SAVE, MADD, MULB, TRNATB, CC MLINEQ, FBGAIN, NEWT, RESID, MSUB, ICNVRT. CC 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 Global Variables. C INTEGER NR INTEGER RSTRUC INTEGER IERR INTEGER IND(NR) INTEGER IBAL INTEGER INFO INTEGER L INTEGER M INTEGER MAXIT INTEGER N C CHARACTER*(*) ERRMSG C DOUBLE PRECISION A(NR,NR) DOUBLE PRECISION C(NR,NR) DOUBLE PRECISION D(NR,NR) DOUBLE PRECISION E(NR,NR) DOUBLE PRECISION P(NR,NR) DOUBLE PRECISION G(NR,NR) DOUBLE PRECISION GDG(NR,NR) DOUBLE PRECISION R(NR,NR) DOUBLE PRECISION RI(NR,NR) DOUBLE PRECISION RS(NR,NR) DOUBLE PRECISION S(NR,NR) DOUBLE PRECISION U(NR,NR) DOUBLE PRECISION WK(NR,NR) DOUBLE PRECISION F(NR,NR) DOUBLE PRECISION WXYZ(NR,NR) DOUBLE PRECISION FCL(NR,NR) DOUBLE PRECISION AT(NR,NR) DOUBLE PRECISION CT(NR,NR) C DOUBLE PRECISION ALFI(NR) DOUBLE PRECISION ALFR(NR) DOUBLE PRECISION BETA(NR) DOUBLE PRECISION CPERM(NR) DOUBLE PRECISION CSCALE(NR) C DOUBLE PRECISION RSD DOUBLE PRECISION RTOL C C Local Variables. C INTEGER I INTEGER J INTEGER LENGTH INTEGER MERR INTEGER NN INTEGER NNPL INTEGER NOUT C DOUBLE PRECISION SEP DOUBLE PRECISION COND C PARAMETER(NOUT=6) C CHARACTER RDFLG CHARACTER RFLAG CHARACTER*30 STRING C C Begin C IERR = 0 IF (RTOL.EQ.0.0D0) RTOL = 1.0D+6 NN = 2*N NNPL = NN + L C C Check system size. C IF (NNPL.GT.NR) THEN CALL ICNVRT(0,NNPL,STRING,LENGTH,MERR) IERR = 1 ERRMSG = 'CKBF: Fatal Error: System too large.'// 1 ' For this system, NR should be at '// 2 ' at least '//STRING(1:LENGTH) RETURN END IF C C Set structure flags for R. C IF (RSTRUC.EQ.0) THEN RFLAG = 'N' RDFLG = 'Y' ELSE IF (RSTRUC.EQ.1) THEN RFLAG = 'Y' RDFLG = 'Y' ELSE IF (RSTRUC.EQ.2) THEN RFLAG = 'Y' RDFLG = 'N' ELSE IERR = 2 ERRMSG = ' CKBF: Fatal Error: RSTRUC must be '// 1 '0, 1, or 2.' RETURN END IF C C Zero out the cross term S C DO 4 J = 1, L DO 4 I = 1, N S(I,J) = 0.D0 4 CONTINUE C C Make E the identity. C DO 6 J=1,N DO 5 I=1,N E(I,J) = 0.0D0 5 CONTINUE E(J,J) = 1.0D0 6 CONTINUE C C Compute A-transpose, store it in AT; compute C C-transpose and store it in CT. C CALL TRNATB(NR,NR,N,N,A,AT) CALL TRNATB(NR,NR,L,N,C,CT) C C Compute the matrix product G*D*(G-transpose) and store it C in GDG. C CALL TRNATB(NR,NR,N,M,G,GDG) CALL MULB(NR,NR,M,M,N,D,GDG,CSCALE) CALL MULB(NR,NR,N,M,N,G,GDG,CSCALE) C IF (RSTRUC.EQ.0) THEN C C R = Identity. C CALL SAVE(NR,NR,L,L,R,RI) CALL RINV(NR,NR,N,NN,L, 1 E,AT,CT,GDG,RI,WXYZ,P,RS,CPERM, 2 RDFLG,RFLAG,'N',.TRUE.) C C R is diagonal. C ELSE IF (RSTRUC.EQ.1) THEN DO 10 I=1,L RI(I,I) = 1.0D0/R(I,I) 10 CONTINUE CALL RINV(NR,NR,N,NN,L, 1 E,AT,CT,GDG,RI,WXYZ,P,RS,CPERM, 2 RDFLG,RFLAG,'N',.TRUE.) C ELSE IF (RSTRUC.EQ.2) THEN C C No special structure in R to exploit. C DO 30 J=1,L DO 20 I=1,L RI(I,J) = 0.0D0 20 CONTINUE RI(J,J) = 1.0D0 30 CONTINUE C C Check the condition of R with respect to inversion. C RI contains R-inverse, but if R is ill-conditioned, C we won't use it. C CALL MLINEQ(NR,NR,L,L,R,RI, 1 COND,IND,CPERM) C C R ill-conditioned. C IF (COND.GE.RTOL) THEN CALL CMPRS(NR,NR,NR,N,NN, 1 NNPL,L,E,AT,CT,GDG,R, 2 S,WXYZ,P,U,WK,CPERM,CSCALE, 3 BETA,'N','N',INFO) IF (INFO.NE.0) THEN IERR = 100 + INFO ERRMSG = 'CKBF: Fatal Error: Attempt to '// 1 'compress R failed. IERR = 100 +'// 2 'INFO, INFO returned from DSVDC.' RETURN END IF ELSE C C R not ill-conditioned. C CALL RINV(NR,NR,N,NN,L, 1 E,AT,CT,GDG,RI,WXYZ,P,RS,CPERM, 2 RDFLG,RFLAG,'N',.TRUE.) END IF END IF C C Compute the Riccati solution. C CALL RICSOL(NR,NR,NN,N,WXYZ,P,E,F, 1 ALFR,ALFI,BETA,CPERM,CSCALE, 2 IND,-1,IBAL,.TRUE.,'N') C C Compute the generalized eigenvalues in case the routine C bombs later. C DO 40, I=1,N ALFR(I) = ALFR(I)/BETA(I) ALFI(I) = ALFI(I)/BETA(I) 40 CONTINUE C C Check the RICSOL computation for errors. C IF (IND(1).NE.0) THEN IERR = 200 + IND(1) ERRMSG = 'CKBF: Fatal Error: More than 50 '// 1 'iterations required by QZITW. '// 2 'IERR = 200 + IND(1), where IND(1) '// 3 ' is the ierr from QZITW.' RETURN END IF IF (CPERM(1).EQ.1.0D+20) THEN CALL SAVE(NR,NR,N,N,F,P) IERR = 300 ERRMSG = ' CKBF: Fatal Error: Schur Vector '// 1 'singular to working precision, '// 2 'probably because the original system '// 3 'is not stabilizable. Hence solution '// 4 'by this method is not possible. The '// 5 'Schur matrix is returned in P.' RETURN END IF IF (IND(2).NE.0) THEN IERR = 400 ERRMSG = 'CKBF: Fatal Error: Convergence '// 1 ' failure in ordering routine '// 2 'ORDER. ' RETURN END IF C C Attempt to improve the solution by iterating C with Newton's method. C CALL SAVE(NR,NR,N,N,P,U) CALL NEWT(NR,NR,NR,NR,N,L,E,AT,CT,GDG,R,S,RI,RS,U, 1 WXYZ,P,F,ALFR,ALFI,BETA,IND,RTOL,'N',RFLAG, 2 RDFLG,'N',SEP,.TRUE.,MAXIT,INFO,NOUT) IF (INFO.NE.0) THEN IERR = -200 - INFO ERRMSG = 'CKBF: Warning: Newton iteration '// 1 'failed. Solution after last '// 2 'iteration given. IERR = -200 - INFO '// 3 'where INFO is returned from NEWT.' END IF C C Compute the Residual of the refined ARE solution. C CALL RESID(NR,NR,NR,NR,N,L,E,AT,CT,GDG,R,S,RI,RS, 1 U,WXYZ,F,CPERM,IND,RTOL,'N',RFLAG,RDFLG, 2 'N',RSD,.TRUE.,NOUT) IF (IND(1).NE.0) THEN IERR = -300 ERRMSG = 'CKBF: Warning: Residual estimation '// 1 'may be invalid. IPVT(1) from RESID '// 2 'is non-zero.' END IF CALL SAVE(NR,NR,N,N,U,P) C C T -1 C Compute the filter gain matrix F = P*C *R. C IF (RSTRUC.EQ.0.OR.RSTRUC.EQ.1) THEN DO 70 J=1,L DO 60 I=1,L F(I,J) = 0.0D0 60 CONTINUE F(J,J) = RI(J,J) 70 CONTINUE ELSE IF(RSTRUC.EQ.2) THEN CALL SAVE(NR,NR,L,L,RI,F) END IF CALL MULB(NR,NR,N,L,L,CT,F,CSCALE) CALL MULB(NR,NR,N,N,L,P,F,CSCALE) C C Calculate the Closed Loop Filter Matrix for the system. C FCL = A - F*C C CALL MMUL(NR,NR,NR,N,N,L,F,C,WXYZ) CALL MSUB(NR,NR,NR,N,N,A,WXYZ,FCL) C C Last Lines of CKBF. C RETURN END