## FB01QD

### Time-varying square root covariance Kalman filter (dense matrices)

[Specification] [Arguments] [Method] [References] [Comments] [Example]

Purpose

      To calculate a combined measurement and time update of one
      iteration of the time-varying Kalman filter. This update is given
      for the square root covariance filter, using dense matrices.

Specification

          SUBROUTINE FB01QD( JOBK, MULTBQ, N, M, P, S, LDS, A, LDA, B,
         $                   LDB, Q, LDQ, C, LDC, R, LDR, K, LDK, TOL,
         $                   IWORK, DWORK, LDWORK, INFO )
    C     .. Scalar Arguments ..
          CHARACTER         JOBK, MULTBQ
          INTEGER           INFO, LDA, LDB, LDC, LDK, LDQ, LDR, LDS, LDWORK,
         $                  M, N, P
          DOUBLE PRECISION  TOL
    C     .. Array Arguments ..
          INTEGER           IWORK(*)
          DOUBLE PRECISION  A(LDA,*), B(LDB,*), C(LDC,*), DWORK(*),
         $                  K(LDK,*), Q(LDQ,*), R(LDR,*), S(LDS,*)

Arguments

Mode Parameters

      JOBK    CHARACTER*1
              Indicates whether the user wishes to compute the Kalman
              filter gain matrix K  as follows:
                                  i
              = 'K':  K  is computed and stored in array K;
                       i
              = 'N':  K  is not required.
                       i

      MULTBQ  CHARACTER*1                    1/2
              Indicates how matrices B  and Q    are to be passed to
                                      i      i
              the routine as follows:
              = 'P':  Array Q is not used and the array B must contain
                                     1/2
                      the product B Q   ;
                                   i i
              = 'N':  Arrays B and Q must contain the matrices as
                      described below.

Input/Output Parameters

      N       (input) INTEGER
              The actual state dimension, i.e., the order of the
              matrices S    and A .  N >= 0.
                        i-1      i

      M       (input) INTEGER
              The actual input dimension, i.e., the order of the matrix
               1/2
              Q   .  M >= 0.
               i

      P       (input) INTEGER
              The actual output dimension, i.e., the order of the matrix
               1/2
              R   .  P >= 0.
               i

      S       (input/output) DOUBLE PRECISION array, dimension (LDS,N)
              On entry, the leading N-by-N lower triangular part of this
              array must contain S   , the square root (left Cholesky
                                  i-1
              factor) of the state covariance matrix at instant (i-1).
              On exit, the leading N-by-N lower triangular part of this
              array contains S , the square root (left Cholesky factor)
                              i
              of the state covariance matrix at instant i.
              The strict upper triangular part of this array is not
              referenced.

      LDS     INTEGER
              The leading dimension of array S.  LDS >= MAX(1,N).

      A       (input) DOUBLE PRECISION array, dimension (LDA,N)
              The leading N-by-N part of this array must contain A ,
                                                                  i
              the state transition matrix of the discrete system at
              instant i.

      LDA     INTEGER
              The leading dimension of array A.  LDA >= MAX(1,N).

      B       (input) DOUBLE PRECISION array, dimension (LDB,M)
              The leading N-by-M part of this array must contain B ,
                                                         1/2      i
              the input weight matrix (or the product B Q    if
                                                       i i
              MULTBQ = 'P') of the discrete system at instant i.

      LDB     INTEGER
              The leading dimension of array B.  LDB >= MAX(1,N).

      Q       (input) DOUBLE PRECISION array, dimension (LDQ,*)
              If MULTBQ = 'N', then the leading M-by-M lower triangular
                                               1/2
              part of this array must contain Q   , the square root
                                               i
              (left Cholesky factor) of the input (process) noise
              covariance matrix at instant i.
              The strict upper triangular part of this array is not
              referenced.
              If MULTBQ = 'P', Q is not referenced and can be supplied
              as a dummy array (i.e., set parameter LDQ = 1 and declare
              this array to be Q(1,1) in the calling program).

      LDQ     INTEGER
              The leading dimension of array Q.
              LDQ >= MAX(1,M) if MULTBQ = 'N';
              LDQ >= 1        if MULTBQ = 'P'.

      C       (input) DOUBLE PRECISION array, dimension (LDC,N)
              The leading P-by-N part of this array must contain C , the
                                                                  i
              output weight matrix of the discrete system at instant i.

      LDC     INTEGER
              The leading dimension of array C.  LDC >= MAX(1,P).

      R       (input/output) DOUBLE PRECISION array, dimension (LDR,P)
              On entry, the leading P-by-P lower triangular part of this
                                  1/2
              array must contain R   , the square root (left Cholesky
                                  i
              factor) of the output (measurement) noise covariance
              matrix at instant i.
              On exit, the leading P-by-P lower triangular part of this
                                     1/2
              array contains (RINOV )   , the square root (left Cholesky
                                   i
              factor) of the covariance matrix of the innovations at
              instant i.
              The strict upper triangular part of this array is not
              referenced.

      LDR     INTEGER
              The leading dimension of array R.  LDR >= MAX(1,P).

      K       (output) DOUBLE PRECISION array, dimension (LDK,P)
              If JOBK = 'K', and INFO = 0, then the leading N-by-P part
              of this array contains K , the Kalman filter gain matrix
                                      i
              at instant i.
              If JOBK = 'N', or JOBK = 'K' and INFO = 1, then the
              leading N-by-P part of this array contains AK , a matrix
                                                           i
              related to the Kalman filter gain matrix at instant i (see
                                                             -1/2
              METHOD). Specifically, AK  = A P     C'(RINOV')    .
                                       i    i i|i-1 i      i

      LDK     INTEGER
              The leading dimension of array K.   LDK >= MAX(1,N).

Tolerances

      TOL     DOUBLE PRECISION
              If JOBK = 'K', then TOL is used to test for near
                                                1/2
              singularity of the matrix (RINOV )   . If the user sets
                                              i
              TOL > 0, then the given value of TOL is used as a
              lower bound for the reciprocal condition number of that
              matrix; a matrix whose estimated condition number is less
              than 1/TOL is considered to be nonsingular. If the user
              sets TOL <= 0, then an implicitly computed, default
              tolerance, defined by TOLDEF = P*P*EPS, is used instead,
              where EPS is the machine precision (see LAPACK Library
              routine DLAMCH).
              Otherwise, TOL is not referenced.

Workspace

      IWORK   INTEGER array, dimension (LIWORK),
              where LIWORK = P if JOBK = 'K',
              and   LIWORK = 1 otherwise.

      DWORK   DOUBLE PRECISION array, dimension (LDWORK)
              On exit, if INFO = 0, DWORK(1) returns the optimal value
              of LDWORK.  If INFO = 0 and JOBK = 'K', DWORK(2) returns
              an estimate of the reciprocal of the condition number
                                         1/2
              (in the 1-norm) of (RINOV )   .
                                       i

      LDWORK  The length of the array DWORK.
              LDWORK >= MAX(1,N*(P+N)+2*P,N*(N+M+2)),     if JOBK = 'N';
              LDWORK >= MAX(2,N*(P+N)+2*P,N*(N+M+2),3*P), if JOBK = 'K'.
              For optimum performance LDWORK should be larger.

Error Indicator

      INFO    INTEGER
              = 0:  successful exit;
              < 0:  if INFO = -i, the i-th argument had an illegal
                    value;
                                                         1/2
              = 1:  if JOBK = 'K' and the matrix (RINOV )   is singular,
                                                       i           1/2
                    i.e., the condition number estimate of (RINOV )
                                                                 i
                    (in the 1-norm) exceeds 1/TOL.  The matrices S, AK ,
                                1/2                                   i
                    and (RINOV )    have been computed.
                              i

Method

      The routine performs one recursion of the square root covariance
      filter algorithm, summarized as follows:

       |  1/2                      |     |         1/2          |
       | R      C x S      0       |     | (RINOV )     0     0 |
       |  i      i   i-1           |     |       i              |
       |                      1/2  | T = |                      |
       | 0      A x S    B x Q     |     |     AK       S     0 |
       |         i   i-1  i   i    |     |       i       i      |

           (Pre-array)                      (Post-array)

      where T is an orthogonal transformation triangularizing the
      pre-array.

      The state covariance matrix P    is factorized as
                                   i|i-1
         P     = S  S'
          i|i-1   i  i

      and one combined time and measurement update for the state X
                                                                  i|i-1
      is given by

         X     = A X      + K (Y - C X     ),
          i+1|i   i i|i-1    i  i   i i|i-1

                           -1/2
      where K = AK (RINOV )     is the Kalman filter gain matrix and Y
             i    i      i                                            i
      is the observed output of the system.

      The triangularization is done entirely via Householder
      transformations exploiting the zero pattern of the pre-array.

References

      [1] Anderson, B.D.O. and Moore, J.B.
          Optimal Filtering.
          Prentice Hall, Englewood Cliffs, New Jersey, 1979.

      [2] Verhaegen, M.H.G. and Van Dooren, P.
          Numerical Aspects of Different Kalman Filter Implementations.
          IEEE Trans. Auto. Contr., AC-31, pp. 907-917, Oct. 1986.

      [3] Vanbegin, M., Van Dooren, P., and Verhaegen, M.H.G.
          Algorithm 675: FORTRAN Subroutines for Computing the Square
          Root Covariance Filter and Square Root Information Filter in
          Dense or Hessenberg Forms.
          ACM Trans. Math. Software, 15, pp. 243-256, 1989.

Numerical Aspects

      The algorithm requires

            3    2                               2   2
      (7/6)N  + N  x (5/2 x P + M) + N x (1/2 x M + P )

      operations and is backward stable (see [2]).

Further Comments

      None

Example

Program Text

    *     FB01QD EXAMPLE PROGRAM TEXT
    *
    *     .. Parameters ..
          INTEGER          NIN, NOUT
          PARAMETER        ( NIN = 5, NOUT = 6 )
          INTEGER          NMAX, MMAX, PMAX
          PARAMETER        ( NMAX = 20, MMAX = 20, PMAX = 20 )
          INTEGER          LDA, LDB, LDC, LDK, LDQ, LDR, LDS
          PARAMETER        ( LDA = NMAX, LDB = NMAX, LDC = PMAX,
         $                   LDK = NMAX, LDQ = MMAX, LDR = PMAX,
         $                   LDS = NMAX )
          INTEGER          LDWORK
          PARAMETER        ( LDWORK = MAX( NMAX*(PMAX+NMAX)+2*PMAX,
         $                                 NMAX*(NMAX+MMAX+2), 3*PMAX ) )
    *     .. Local Scalars ..
          DOUBLE PRECISION TOL
          INTEGER          I, INFO, ISTEP, J, M, N, P
          CHARACTER*1      JOBK, MULTBQ
    *     .. Local Arrays ..
          DOUBLE PRECISION A(LDA,NMAX), B(LDB,MMAX), C(LDC,NMAX),
         $                 DIAG(PMAX), DWORK(LDWORK), K(LDK,PMAX),
         $                 Q(LDQ,MMAX), R(LDR,PMAX), S(LDS,NMAX)
          INTEGER          IWORK(PMAX)
    *     .. External Functions ..
          LOGICAL          LSAME
          EXTERNAL         LSAME
    *     .. External Subroutines ..
          EXTERNAL         DCOPY, FB01QD
    *     .. Intrinsic Functions ..
          INTRINSIC        MAX
    *     .. Executable Statements ..
    *
          WRITE ( NOUT, FMT = 99999 )
    *     Skip the heading in the data file and read the data.
          READ ( NIN, FMT = '()' )
          READ ( NIN, FMT = * ) N, M, P, JOBK, TOL, MULTBQ
          IF ( N.LE.0 .OR. N.GT.NMAX ) THEN
             WRITE ( NOUT, FMT = 99994 ) N
          ELSE
             READ ( NIN, FMT = * ) ( ( S(I,J), J = 1,N ), I = 1,N )
             READ ( NIN, FMT = * ) ( ( A(I,J), J = 1,N ), I = 1,N )
             IF ( M.LE.0 .OR. M.GT.MMAX ) THEN
                WRITE ( NOUT, FMT = 99993 ) M
             ELSE
                READ ( NIN, FMT = * ) ( ( B(I,J), J = 1,M ), I = 1,N )
                IF ( LSAME( MULTBQ, 'N' ) ) READ ( NIN, FMT = *)
         $                               ( ( Q(I,J), J = 1,M ), I = 1,M )
                IF ( P.LE.0 .OR. P.GT.PMAX ) THEN
                   WRITE ( NOUT, FMT = 99992 ) P
                ELSE
                   READ ( NIN, FMT = * ) ( ( C(I,J), J = 1,N ), I = 1,P )
                   READ ( NIN, FMT = * ) ( ( R(I,J), J = 1,P ), I = 1,P )
    *              Save the strict lower triangle of R in its strict upper
    *              triangle and the diagonal in the array DIAG.
                   DO 10 I = 2, P
                      CALL DCOPY( I, R(I,1), LDR, R(1,I), 1 )
       10          CONTINUE
                   CALL DCOPY( P, R, LDR+1, DIAG, 1 )
    *              Perform three iterations of the (Kalman) filter recursion
    *              (in square root covariance form).
                   ISTEP = 1
       20          CONTINUE
                      CALL FB01QD( JOBK, MULTBQ, N, M, P, S, LDS, A, LDA,
         $                         B, LDB, Q, LDQ, C, LDC, R, LDR, K, LDK,
         $                         TOL, IWORK, DWORK, LDWORK, INFO )
                      ISTEP = ISTEP + 1
                      IF ( INFO.EQ.0 .AND. ISTEP.LE.3 ) THEN
    *                    Restore the lower triangle of R.
                         DO 30 I = 2, P
                            CALL DCOPY( I, R(1,I), 1, R(I,1), LDR )
       30                CONTINUE
                         CALL DCOPY( P, DIAG, 1, R, LDR+1 )
                         GO TO 20
                      END IF
    *
                   IF ( INFO.NE.0 ) THEN
                      WRITE ( NOUT, FMT = 99998 ) INFO
                   ELSE
                      WRITE ( NOUT, FMT = 99997 )
                      DO 40 I = 1, N
                         WRITE ( NOUT, FMT = 99995 ) ( S(I,J), J = 1,N )
       40             CONTINUE
                      IF ( LSAME( JOBK, 'K' ) ) THEN
                         WRITE ( NOUT, FMT = 99996 )
                         DO 60 I = 1, N
                            WRITE ( NOUT, FMT = 99995 ) ( K(I,J), J = 1,P )
       60                CONTINUE
                      END IF
                   END IF
                END IF
             END IF
          END IF
          STOP
    *
    99999 FORMAT (' FB01QD EXAMPLE PROGRAM RESULTS',/1X)
    99998 FORMAT (' INFO on exit from FB01QD = ',I2)
    99997 FORMAT (' The square root of the state covariance matrix is ')
    99996 FORMAT (/' The Kalman gain matrix is ')
    99995 FORMAT (20(1X,F8.4))
    99994 FORMAT (/' N is out of range.',/' N = ',I5)
    99993 FORMAT (/' M is out of range.',/' M = ',I5)
    99992 FORMAT (/' P is out of range.',/' P = ',I5)
          END

Program Data

     FB01QD EXAMPLE PROGRAM DATA
       4     2     2     K     0.0     N
       0.0000  0.0000  0.0000  0.0000
       0.0000  0.0000  0.0000  0.0000
       0.0000  0.0000  0.0000  0.0000
       0.0000  0.0000  0.0000  0.0000
       0.2113  0.8497  0.7263  0.8833
       0.7560  0.6857  0.1985  0.6525
       0.0002  0.8782  0.5442  0.3076
       0.3303  0.0683  0.2320  0.9329
       0.5618  0.5042
       0.5896  0.3493
       0.6853  0.3873
       0.8906  0.9222
       1.0000  0.0000
       0.0000  1.0000
       0.3616  0.5664  0.5015  0.2693
       0.2922  0.4826  0.4368  0.6325
       0.9488  0.0000
       0.3760  0.7340

Program Results

     FB01QD EXAMPLE PROGRAM RESULTS

     The square root of the state covariance matrix is
      -1.2936   0.0000   0.0000   0.0000
      -1.1382  -0.2579   0.0000   0.0000
      -0.9622  -0.1529   0.2974   0.0000
      -1.3076   0.0936   0.4508  -0.4897

     The Kalman gain matrix is
       0.3638   0.9469
       0.3532   0.8179
       0.2471   0.5542
       0.1982   0.6471

Return to index