## 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]).

```
```  None
```
Example

Program Text

```*     FB01TD EXAMPLE PROGRAM TEXT
*     Copyright (c) 2002-2017 NICONET e.V.
*
*     .. 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
```