      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