## FB01VD

### One recursion of the conventional Kalman filter

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

Purpose

```  To compute one recursion of the conventional Kalman filter
equations. This is one update of the Riccati difference equation
and the Kalman filter gain.

```
Specification
```      SUBROUTINE FB01VD( N, M, L, P, LDP, A, LDA, B, LDB, C, LDC, Q,
\$                   LDQ, R, LDR, K, LDK, TOL, IWORK, DWORK, LDWORK,
\$                   INFO )
C     .. Scalar Arguments ..
INTEGER           INFO, L, LDA, LDB, LDC, LDK, LDP, LDQ, LDR,
\$                  LDWORK, M, N
DOUBLE PRECISION  TOL
C     .. Array Arguments ..
INTEGER           IWORK(*)
DOUBLE PRECISION  A(LDA,*), B(LDB,*), C(LDC,*), DWORK(*),
\$                  K(LDK,*), P(LDP,*), Q(LDQ,*), R(LDR,*)

```
Arguments

Input/Output Parameters

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

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

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

P       (input/output) DOUBLE PRECISION array, dimension (LDP,N)
On entry, the leading N-by-N part of this array must
contain P     , the state covariance matrix at instant
i|i-1
(i-1). The upper triangular part only is needed.
On exit, if INFO = 0, the leading N-by-N part of this
array contains P     , the state covariance matrix at
i+1|i
instant i. The strictly lower triangular part is not set.
Otherwise, the leading N-by-N part of this array contains
P     , its input value.
i|i-1

LDP     INTEGER
The leading dimension of array P.  LDP >= 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 ,
i
the input weight matrix of the discrete system at
instant i.

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

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

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

Q       (input) DOUBLE PRECISION array, dimension (LDQ,M)
The leading M-by-M part of this array must contain Q ,
i
the input (process) noise covariance matrix at instant i.
The diagonal elements of this array are modified by the
routine, but are restored on exit.

LDQ     INTEGER
The leading dimension of array Q.  LDQ >= MAX(1,M).

R       (input/output) DOUBLE PRECISION array, dimension (LDR,L)
On entry, the leading L-by-L part of this array must
contain R , the output (measurement) noise covariance
i
matrix at instant i.
On exit, if INFO = 0, or INFO = L+1, the leading L-by-L
1/2
upper triangular part of this array contains (RINOV )   ,
i
the square root (left Cholesky factor) of the covariance
matrix of the innovations at instant i.

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

K       (output) DOUBLE PRECISION array, dimension (LDK,L)
If INFO = 0, the leading N-by-L part of this array
contains K , the Kalman filter gain matrix at instant i.
i
If INFO > 0, the leading N-by-L part of this array
contains the matrix product P     C'.
i|i-1 i

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

```
Tolerances
```  TOL     DOUBLE PRECISION
The tolerance to be used to test for near singularity of
the matrix RINOV . If the user sets TOL > 0, then the
i
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 = L*L*EPS, is used instead, where EPS is the
machine precision (see LAPACK Library routine DLAMCH).

```
Workspace
```  IWORK   INTEGER array, dimension (L)

DWORK   DOUBLE PRECISION array, dimension (LDWORK)
On exit, if INFO = 0, or INFO = L+1, DWORK(1) returns an
estimate of the reciprocal of the condition number (in the
1-norm) of the matrix RINOV .
i

LDWORK  The length of the array DWORK.
LDWORK >= MAX(1,L*N+3*L,N*N,N*M).

```
Error Indicator
```  INFO    INTEGER
= 0:  successful exit;
< 0:  if INFO = -k, the k-th argument had an illegal
value;
= k:  if INFO = k, 1 <= k <= L, the leading minor of order
k of the matrix RINOV  is not positive-definite, and
i
its Cholesky factorization could not be completed;
= L+1: the matrix RINOV  is singular, i.e., the condition
i
number estimate of RINOV  (in the 1-norm) exceeds
i
1/TOL.

```
Method
```  The conventional Kalman filter gain used at the i-th recursion
step is of the form

-1
K  = P     C'  RINOV  ,
i    i|i-1 i       i

where RINOV  = C P     C' + R , and the state covariance matrix
i    i i|i-1 i    i

P      is updated by the discrete-time difference Riccati equation
i|i-1

P      = A  (P      - K C P     ) A'  + B Q B'.
i+1|i    i   i|i-1    i i i|i-1   i     i i i

Using these two updates, the combined time and measurement update
of the state X      is given by
i|i-1

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

where Y  is the new observation at step i.
i

```
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, 1986.

```
Numerical Aspects
```  The algorithm requires approximately

3   2
3/2 x N + N  x (3 x L + M/2)

operations.

```
```  None
```
Example

Program Text

```*     FB01VD EXAMPLE PROGRAM TEXT
*     Copyright (c) 2002-2017 NICONET e.V.
*
*     .. Parameters ..
INTEGER          NIN, NOUT
PARAMETER        ( NIN = 5, NOUT = 6 )
INTEGER          NMAX, MMAX, LMAX
PARAMETER        ( NMAX = 20, MMAX = 20, LMAX = 20 )
INTEGER          LDA, LDB, LDC, LDK, LDP, LDQ, LDR
PARAMETER        ( LDA = NMAX, LDB = NMAX, LDC = LMAX, LDK = NMAX,
\$                   LDP = NMAX, LDQ = MMAX, LDR = LMAX )
INTEGER          LDWORK
PARAMETER        ( LDWORK = MAX( LMAX*NMAX + 3*LMAX, NMAX*NMAX,
\$                                 MMAX*NMAX ) )
*     .. Local Scalars ..
DOUBLE PRECISION TOL
INTEGER          I, INFO, J, L, M, N
*     .. Local Arrays ..
DOUBLE PRECISION A(LDA,NMAX), B(LDB,MMAX), C(LDC,NMAX),
\$                 DWORK(LDWORK), K(LDK,LMAX), P(LDP,NMAX),
\$                 Q(LDQ,MMAX), R(LDR,LMAX)
INTEGER          IWORK(LMAX)
*     .. External Subroutines ..
EXTERNAL         DCOPY, FB01VD
*     .. 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, L, TOL
IF ( N.LE.0 .OR. N.GT.NMAX ) THEN
WRITE ( NOUT, FMT = 99993 ) N
ELSE
READ ( NIN, FMT = * ) ( ( P(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 = 99992 ) M
ELSE
READ ( NIN, FMT = * ) ( ( B(I,J), J = 1,M ), I = 1,N )
READ ( NIN, FMT = * ) ( ( Q(I,J), J = 1,M ), I = 1,M )
IF ( L.LE.0 .OR. L.GT.LMAX ) THEN
WRITE ( NOUT, FMT = 99991 ) L
ELSE
READ ( NIN, FMT = * ) ( ( C(I,J), J = 1,N ), I = 1,L )
READ ( NIN, FMT = * ) ( ( R(I,J), J = 1,L ), I = 1,L )
*              Perform one iteration of the (Kalman) filter recursion.
CALL FB01VD( N, M, L, P, LDP, A, LDA, B, LDB, C, LDC,
\$                      Q, LDQ, R, LDR, K, LDK, TOL, IWORK, DWORK,
\$                      LDWORK, INFO )
*
IF ( INFO.NE.0 ) THEN
WRITE ( NOUT, FMT = 99998 ) INFO
ELSE
WRITE ( NOUT, FMT = 99997 )
DO 20 I = 1, N
CALL DCOPY( I-1, P(1,I), 1, P(I,1), LDP )
WRITE ( NOUT, FMT = 99994 ) ( P(I,J), J = 1,N )
20             CONTINUE
WRITE ( NOUT, FMT = 99996 )
DO 40 I = 1, N
WRITE ( NOUT, FMT = 99994 ) ( K(I,J), J = 1,L )
40             CONTINUE
WRITE ( NOUT, FMT = 99995 )
DO 60 I = 1, L
WRITE ( NOUT, FMT = 99994 ) ( R(I,J), J = 1,L )
60             CONTINUE
END IF
END IF
END IF
END IF
STOP
*
99999 FORMAT (' FB01VD EXAMPLE PROGRAM RESULTS',/1X)
99998 FORMAT (' INFO on exit from FB01VD = ',I3)
99997 FORMAT (' The state covariance matrix is ')
99996 FORMAT (/' The Kalman filter gain matrix is ')
99995 FORMAT (/' The square root of the covariance matrix of the innov',
\$          'ations is ')
99994 FORMAT (20(1X,F8.4))
99993 FORMAT (/' N is out of range.',/' N = ',I5)
99992 FORMAT (/' M is out of range.',/' M = ',I5)
99991 FORMAT (/' L is out of range.',/' P = ',I5)
END
```
Program Data
``` FB01VD EXAMPLE PROGRAM DATA
4     3     2     0.0
0.5015  0.4368  0.2693  0.6325
0.4368  0.4818  0.2639  0.4148
0.2693  0.2639  0.1121  0.6856
0.6325  0.4148  0.6856  0.8906
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.0437  0.7783  0.5618
0.4818  0.2119  0.5896
0.2639  0.1121  0.6853
0.4148  0.6856  0.8906
0.9329  0.2146  0.3126
0.2146  0.2922  0.5664
0.3126  0.5664  0.5935
0.3873  0.9488  0.3760  0.0881
0.9222  0.3435  0.7340  0.4498
1.0000  0.0000
0.0000  1.0000
```
Program Results
``` FB01VD EXAMPLE PROGRAM RESULTS

The state covariance matrix is
1.6007   1.3283   1.1153   1.7177
1.3283   1.2763   1.0132   1.5137
1.1153   1.0132   0.8222   1.2722
1.7177   1.5137   1.2722   2.1562

The Kalman filter gain matrix is
0.1648   0.2241
0.2115   0.1610
0.0728   0.1673
0.1304   0.3892

The square root of the covariance matrix of the innovations is
1.5091   1.1543
0.0000   1.5072
```