## 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
```   Anderson, B.D.O. and Moore, J.B.
Optimal Filtering.
Prentice Hall, Englewood Cliffs, New Jersey, 1979.

 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.

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

```
```  None
```
Example

Program Text

```*     FB01QD 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          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
```