## FB01TD

### Time-invariant square root information Kalman filter (controller Hessenberg form)

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

Purpose

      To calculate a combined measurement and time update of one
      iteration of the time-invariant Kalman filter. This update is
      given for the square root information filter, using the condensed
      controller Hessenberg form.

Specification

          SUBROUTINE FB01TD( JOBX, MULTRC, N, M, P, SINV, LDSINV, AINV,
         $                   LDAINV, AINVB, LDAINB, RINV, LDRINV, C, LDC,
         $                   QINV, LDQINV, X, RINVY, Z, E, TOL, IWORK,
         $                   DWORK, LDWORK, INFO )
    C     .. Scalar Arguments ..
          CHARACTER         JOBX, MULTRC
          INTEGER           INFO, LDAINB, LDAINV, LDC, LDQINV, LDRINV,
         $                  LDSINV, LDWORK, M, N, P
          DOUBLE PRECISION  TOL
    C     .. Array Arguments ..
          INTEGER           IWORK(*)
          DOUBLE PRECISION  AINV(LDAINV,*), AINVB(LDAINB,*), C(LDC,*),
         $                  DWORK(*), E(*), QINV(LDQINV,*), RINV(LDRINV,*),
         $                  RINVY(*), SINV(LDSINV,*), X(*), Z(*)

Arguments

Mode Parameters

      JOBX    CHARACTER*1
              Indicates whether X    is to be computed as follows:
                                 i+1
              = 'X':  X    is computed and stored in array X;
                       i+1
              = 'N':  X    is not required.
                       i+1

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

Input/Output Parameters

      N       (input) INTEGER
              The actual state dimension, i.e., the order of the
                        -1      -1
              matrices S   and A  .  N >= 0.
                        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+1

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

      LDSINV  INTEGER
              The leading dimension of array SINV.  LDSINV >= MAX(1,N).

      AINV    (input) DOUBLE PRECISION array, dimension (LDAINV,N)
                                                                  -1
              The leading N-by-N part of this array must contain A  ,
              the inverse of the state transition matrix of the discrete
              system in controller Hessenberg form (e.g., as produced by
              SLICOT Library Routine TB01MD).

      LDAINV  INTEGER
              The leading dimension of array AINV.  LDAINV >= MAX(1,N).

      AINVB   (input) DOUBLE PRECISION array, dimension (LDAINB,M)
                                                                   -1
              The leading N-by-M part of this array must contain  A  B,
                              -1
              the product of A   and the input weight matrix B of the
              discrete system, in upper controller Hessenberg form
              (e.g., as produced by SLICOT Library Routine TB01MD).

      LDAINB  INTEGER
              The leading dimension of array AINVB.  LDAINB >= MAX(1,N).

      RINV    (input) DOUBLE PRECISION array, dimension (LDRINV,*)
              If MULTRC = 'N', then the leading P-by-P upper triangular
                                               -1/2
              part of this array must contain R    , the inverse of the
                                               i+1
              covariance square root (right Cholesky factor) of the
              output (measurement) noise (hence the information square
              root) at instant i+1.
              The strict lower triangular part of this array is not
              referenced.
              Otherwise, RINV is not referenced and can be supplied as a
              dummy array (i.e., set parameter LDRINV = 1 and declare
              this array to be RINV(1,1) in the calling program).

      LDRINV  INTEGER
              The leading dimension of array RINV.
              LDRINV >= MAX(1,P) if MULTRC = 'N';
              LDRINV >= 1        if MULTRC = 'P'.

      C       (input) DOUBLE PRECISION array, dimension (LDC,N)
              The leading P-by-N part of this array must contain C   ,
                                                        -1/2      i+1
              the output weight matrix (or the product R    C    if
                                                        i+1  i+1
              MULTRC = 'P') of the discrete system at instant i+1.

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

      QINV    (input/output) DOUBLE PRECISION array, dimension
              (LDQINV,M)
              On entry, the leading M-by-M upper triangular part of this
                                  -1/2
              array must contain Q    , the inverse of the covariance
                                  i
              square root (right Cholesky factor) of the input (process)
              noise (hence the information square root) at instant i.
              On exit, the leading M-by-M upper triangular part of this
                                     -1/2
              array contains (QINOV )    , the inverse of the covariance
                                   i
              square root (right Cholesky factor) of the process noise
              innovation (hence the information square root) at
              instant i.
              The strict lower triangular part of this array is not
              referenced.

      LDQINV  INTEGER
              The leading dimension of array QINV.  LDQINV >= MAX(1,M).

      X       (input/output) DOUBLE PRECISION array, dimension (N)
              On entry, this array must contain X , the estimated
                                                 i
              filtered state at instant i.
              On exit, if JOBX = 'X', and INFO = 0, then this array
              contains X   , the estimated filtered state at
                        i+1
              instant i+1.
              On exit, if JOBX = 'N', or JOBX = 'X' and INFO = 1, then
                                   -1
              this array contains S   X   .
                                   i+1 i+1

      RINVY   (input) DOUBLE PRECISION array, dimension (P)
                                       -1/2
              This array must contain R    Y   , the product of the
                                       i+1  i+1
                                       -1/2
              upper triangular matrix R     and the measured output
                                       i+1
              vector Y    at instant i+1.
                      i+1

      Z       (input) DOUBLE PRECISION array, dimension (M)
              This array must contain Z , the mean value of the state
                                       i
              process noise at instant i.

      E       (output) DOUBLE PRECISION array, dimension (P)
              This array contains E   , the estimated error at instant
                                   i+1
              i+1.

Tolerances

      TOL     DOUBLE PRECISION
              If JOBX = 'X', then TOL is used to test for near
                                         -1
              singularity of the matrix S   . If the user sets
                                         i+1
              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 = N*N*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 = N if JOBX = 'X',
              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 JOBX = 'X', DWORK(2) returns
              an estimate of the reciprocal of the condition number
                                  -1
              (in the 1-norm) of S   .
                                  i+1

      LDWORK  The length of the array DWORK.
              LDWORK >= MAX(1,N*(N+2*M)+3*M,(N+P)*(N+1)+N+MAX(N-1,M+1)),
                                  if JOBX = 'N';
              LDWORK >= MAX(2,N*(N+2*M)+3*M,(N+P)*(N+1)+N+MAX(N-1,M+1),
                            3*N), if JOBX = 'X'.
              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
              = 1:  if JOBX = 'X' and the matrix S    is singular,
                                                  i+1       -1
                    i.e., the condition number estimate of S    (in the
                                                            i+1
                                                          -1    -1/2
                    1-norm) exceeds 1/TOL.  The matrices S   , Q
                                                          i+1   i
                    and E have been computed.

Method

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

        |    -1/2             -1/2    |     |         -1/2             |
        |   Q         0      Q    Z   |     | (QINOV )     *     *     |
        |    i                i    i  |     |       i                  |
        |                             |     |                          |
        |           -1/2      -1/2    |     |             -1    -1     |
      T |    0     R    C    R    Y   |  =  |    0       S     S   X   |
        |           i+1  i+1  i+1  i+1|     |             i+1   i+1 i+1|
        |                             |     |                          |
        |  -1 -1     -1 -1    -1      |     |                          |
        | S  A  B   S  A     S  X     |     |    0         0     E     |
        |  i         i        i  i    |     |                     i+1  |

                    (Pre-array)                      (Post-array)

      where T is an orthogonal transformation triangularizing the
                         -1/2
      pre-array, (QINOV )     is the inverse of the covariance square
                       i
      root (right Cholesky factor) of the process noise innovation
                                                             -1  -1
      (hence the information square root) at instant i and (A  ,A  B) is
      in upper controller Hessenberg form.

      An example of the pre-array is given below (where N = 6, M = 2,
      and P = 3):

          |x x |             | x|
          |  x |             | x|
          _______________________
          |    | x x x x x x | x|
          |    | x x x x x x | x|
          |    | x x x x x x | x|
          _______________________
          |x x | x x x x x x | x|
          |  x | x x x x x x | x|
          |    | x x x x x x | x|
          |    |   x x x x x | x|
          |    |     x x x x | x|
          |    |       x x x | x|

      The inverse of the corresponding state covariance matrix P
                                                                i+1|i+1
      (hence the information matrix I) is then factorized as

                     -1         -1     -1
          I       = P       = (S   )' S
           i+1|i+1   i+1|i+1    i+1    i+1

      and one combined time and measurement update for the state is
      given by X   .
                i+1

      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] Van Dooren, P. and Verhaegen, M.H.G.
          Condensed Forms for Efficient Time-Invariant Kalman Filtering.
          SIAM J. Sci. Stat. Comp., 9. pp. 516-530, 1988.

      [3] 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.

      [4] 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 approximately

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

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

Further Comments

      None

Example

Program Text

    *     FB01TD EXAMPLE PROGRAM TEXT
    *
    *     .. Parameters ..
          INTEGER          NIN, NOUT
          PARAMETER        ( NIN = 5, NOUT = 6 )
          INTEGER          NMAX, MMAX, PMAX
          PARAMETER        ( NMAX = 20, MMAX = 20, PMAX = 20 )
          INTEGER          LDAINB, LDAINV, LDC, LDQINV, LDRINV, LDSINV
          PARAMETER        ( LDAINB = NMAX, LDAINV = NMAX, LDC = PMAX,
         $                   LDQINV = MMAX, LDRINV = PMAX, LDSINV = NMAX )
          INTEGER          LDWORK
          PARAMETER        ( LDWORK = MAX( NMAX*(NMAX + 2*MMAX) + 3*MMAX,
         $                                (NMAX + PMAX)*(NMAX + 1) + NMAX +
         $                                 MAX( NMAX - 1, MMAX + 1 ),
         $                                 3*NMAX ) )
    *     .. Local Scalars ..
          DOUBLE PRECISION TOL
          INTEGER          I, INFO, ISTEP, J, M, N, P
          CHARACTER*1      JOBX, MULTRC
    *     .. Local Arrays ..
          DOUBLE PRECISION AINV(LDAINV,NMAX), AINVB(LDAINB,MMAX),
         $                 C(LDC,NMAX), DIAG(MMAX), DWORK(LDWORK), E(PMAX),
         $                 QINV(LDQINV,MMAX), RINV(LDRINV,PMAX),
         $                 RINVY(PMAX), SINV(LDSINV,NMAX), X(NMAX), Z(MMAX)
          INTEGER          IWORK(NMAX)
    *     .. External Functions ..
          LOGICAL          LSAME
          EXTERNAL         LSAME
    *     .. External Subroutines ..
          EXTERNAL         DCOPY, FB01TD
    *     .. 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, JOBX, TOL, MULTRC
          IF ( N.LE.0 .OR. N.GT.NMAX ) THEN
             WRITE ( NOUT, FMT = 99993 ) N
          ELSE
             READ ( NIN, FMT = * ) ( ( AINV(I,J), J = 1,N ), I = 1,N )
             IF ( P.LE.0 .OR. P.GT.PMAX ) THEN
                WRITE ( NOUT, FMT = 99991 ) P
             ELSE
                READ ( NIN, FMT = * ) ( ( C(I,J), J = 1,N ), I = 1,P )
                IF ( LSAME( MULTRC, 'N' ) ) READ ( NIN, FMT = * )
         $                         ( ( RINV(I,J), J = 1,P ), I = 1,P )
                IF ( M.LE.0 .OR. M.GT.MMAX ) THEN
                   WRITE ( NOUT, FMT = 99992 ) M
                ELSE
                   READ ( NIN, FMT = * )
         $                              ( ( AINVB(I,J), J = 1,M ), I = 1,N )
                   READ ( NIN, FMT = * ) ( ( QINV(I,J), J = 1,M ), I = 1,M )
                   READ ( NIN, FMT = * ) ( ( SINV(I,J), J = 1,N ), I = 1,N )
                   READ ( NIN, FMT = * ) ( Z(J), J = 1,M )
                   READ ( NIN, FMT = * ) ( X(J), J = 1,N )
                   READ ( NIN, FMT = * ) ( RINVY(J), J = 1,P )
    *              Save the strict upper triangle of QINV in its strict
    *              lower triangle and the diagonal in the array DIAG.
                   DO 10 I = 2, M
                      CALL DCOPY( I, QINV(1,I), 1, QINV(I,1), LDQINV )
       10          CONTINUE
                   CALL DCOPY( M, QINV, LDQINV+1, DIAG, 1 )
    *              Perform three iterations of the (Kalman) filter
    *              recursion (in square root information form).
                   ISTEP = 1
       20          CONTINUE
                      CALL FB01TD( JOBX, MULTRC, N, M, P, SINV, LDSINV,
         $                         AINV, LDAINV, AINVB, LDAINB, RINV,
         $                         LDRINV, C, LDC, QINV, LDQINV, X, RINVY,
         $                         Z, E, TOL, IWORK, DWORK, LDWORK, INFO )
                      ISTEP = ISTEP + 1
                      IF ( INFO.EQ.0 .AND. ISTEP.LE.3 ) THEN
    *                    Restore the upper triangle of QINV.
                         DO 30 I = 2, M
                            CALL DCOPY( I, QINV(I,1), LDQINV, QINV(1,I), 1 )
       30                CONTINUE
                         CALL DCOPY( M, DIAG, 1, QINV, LDQINV+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 = 99996 ) ( SINV(I,J), J = 1,N )
       40             CONTINUE
                      IF ( LSAME( JOBX, 'X' ) ) THEN
                         WRITE ( NOUT, FMT = 99995 )
                         DO 50 I = 1, N
                            WRITE ( NOUT, FMT = 99994 ) I, X(I)
       50                CONTINUE
                      END IF
                   END IF
                END IF
             END IF
          END IF
          STOP
    *
    99999 FORMAT (' FB01TD EXAMPLE PROGRAM RESULTS',/1X)
    99998 FORMAT (' INFO on exit from FB01TD = ',I2)
    99997 FORMAT (' The inverse of the square root of the state covariance',
         $       ' matrix is ')
    99996 FORMAT (20(1X,F8.4))
    99995 FORMAT (/' The components of the estimated filtered state are ',
         $       //'   k       X(k)',/)
    99994 FORMAT (I4,3X,F8.4)
    99993 FORMAT (/' N is out of range.',/' N = ',I5)
    99992 FORMAT (/' M is out of range.',/' M = ',I5)
    99991 FORMAT (/' P is out of range.',/' P = ',I5)
          END

Program Data

     FB01TD EXAMPLE PROGRAM DATA
       4     2     2     X     0.0     N
       0.2113  0.7560  0.0002  0.3303
       0.8497  0.6857  0.8782  0.0683
       0.7263  0.1985  0.5442  0.2320
       0.0000  0.6525  0.3076  0.9329
       0.3616  0.5664  0.5015  0.2693
       0.2922  0.4826  0.4368  0.6325
       1.0000  0.0000
       0.0000  1.0000
      -0.8805  1.3257
       0.0000  0.5207
       0.0000  0.0000
       0.0000  0.0000
       1.1159  0.2305
       0.0000  0.6597
       1.0000  0.0000  0.0000  0.0000
       0.0000  1.0000  0.0000  0.0000
       0.0000  0.0000  1.0000  0.0000
       0.0000  0.0000  0.0000  1.0000
       0.0019
       0.5075
       0.4076
       0.8408
       0.5017
       0.9128
       0.2129
       0.5591

Program Results

     FB01TD EXAMPLE PROGRAM RESULTS

     The inverse of the square root of the state covariance matrix is
      -0.8731  -1.1461  -1.0260  -0.8901
       0.0000  -0.2763  -0.1929  -0.3763
       0.0000   0.0000  -0.1110  -0.1051
       0.0000   0.0000   0.0000   0.3120

     The components of the estimated filtered state are

       k       X(k)

       1    -2.0688
       2    -0.7814
       3     2.2181
       4     0.9298

Return to index