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
NoneExample
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.7340Program 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