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
C PURPOSE
C
C To compute one recursion of the conventional Kalman filter
C equations. This is one update of the Riccati difference equation
C and the Kalman filter gain.
C
C ARGUMENTS
C
C Input/Output Parameters
C
C N (input) INTEGER
C The actual state dimension, i.e., the order of the
C matrices P and A . N >= 0.
C i|i-1 i
C
C M (input) INTEGER
C The actual input dimension, i.e., the order of the matrix
C Q . M >= 0.
C i
C
C L (input) INTEGER
C The actual output dimension, i.e., the order of the matrix
C R . L >= 0.
C i
C
C P (input/output) DOUBLE PRECISION array, dimension (LDP,N)
C On entry, the leading N-by-N part of this array must
C contain P , the state covariance matrix at instant
C i|i-1
C (i-1). The upper triangular part only is needed.
C On exit, if INFO = 0, the leading N-by-N part of this
C array contains P , the state covariance matrix at
C i+1|i
C instant i. The strictly lower triangular part is not set.
C Otherwise, the leading N-by-N part of this array contains
C P , its input value.
C i|i-1
C
C LDP INTEGER
C The leading dimension of array P. LDP >= MAX(1,N).
C
C A (input) DOUBLE PRECISION array, dimension (LDA,N)
C The leading N-by-N part of this array must contain A ,
C i
C the state transition matrix of the discrete system at
C instant i.
C
C LDA INTEGER
C The leading dimension of array A. LDA >= MAX(1,N).
C
C B (input) DOUBLE PRECISION array, dimension (LDB,M)
C The leading N-by-M part of this array must contain B ,
C i
C the input weight matrix of the discrete system at
C instant i.
C
C LDB INTEGER
C The leading dimension of array B. LDB >= MAX(1,N).
C
C C (input) DOUBLE PRECISION array, dimension (LDC,N)
C The leading L-by-N part of this array must contain C ,
C i
C the output weight matrix of the discrete system at
C instant i.
C
C LDC INTEGER
C The leading dimension of array C. LDC >= MAX(1,L).
C
C Q (input) DOUBLE PRECISION array, dimension (LDQ,M)
C The leading M-by-M part of this array must contain Q ,
C i
C the input (process) noise covariance matrix at instant i.
C The diagonal elements of this array are modified by the
C routine, but are restored on exit.
C
C LDQ INTEGER
C The leading dimension of array Q. LDQ >= MAX(1,M).
C
C R (input/output) DOUBLE PRECISION array, dimension (LDR,L)
C On entry, the leading L-by-L part of this array must
C contain R , the output (measurement) noise covariance
C i
C matrix at instant i.
C On exit, if INFO = 0, or INFO = L+1, the leading L-by-L
C 1/2
C upper triangular part of this array contains (RINOV ) ,
C i
C the square root (left Cholesky factor) of the covariance
C matrix of the innovations at instant i.
C
C LDR INTEGER
C The leading dimension of array R. LDR >= MAX(1,L).
C
C K (output) DOUBLE PRECISION array, dimension (LDK,L)
C If INFO = 0, the leading N-by-L part of this array
C contains K , the Kalman filter gain matrix at instant i.
C i
C If INFO > 0, the leading N-by-L part of this array
C contains the matrix product P C'.
C i|i-1 i
C
C LDK INTEGER
C The leading dimension of array K. LDK >= MAX(1,N).
C
C Tolerances
C
C TOL DOUBLE PRECISION
C The tolerance to be used to test for near singularity of
C the matrix RINOV . If the user sets TOL > 0, then the
C i
C given value of TOL is used as a lower bound for the
C reciprocal condition number of that matrix; a matrix whose
C estimated condition number is less than 1/TOL is
C considered to be nonsingular. If the user sets TOL <= 0,
C then an implicitly computed, default tolerance, defined by
C TOLDEF = L*L*EPS, is used instead, where EPS is the
C machine precision (see LAPACK Library routine DLAMCH).
C
C Workspace
C
C IWORK INTEGER array, dimension (L)
C
C DWORK DOUBLE PRECISION array, dimension (LDWORK)
C On exit, if INFO = 0, or INFO = L+1, DWORK(1) returns an
C estimate of the reciprocal of the condition number (in the
C 1-norm) of the matrix RINOV .
C i
C
C LDWORK The length of the array DWORK.
C LDWORK >= MAX(1,L*N+3*L,N*N,N*M).
C
C Error Indicator
C
C INFO INTEGER
C = 0: successful exit;
C < 0: if INFO = -k, the k-th argument had an illegal
C value;
C = k: if INFO = k, 1 <= k <= L, the leading minor of order
C k of the matrix RINOV is not positive-definite, and
C i
C its Cholesky factorization could not be completed;
C = L+1: the matrix RINOV is singular, i.e., the condition
C i
C number estimate of RINOV (in the 1-norm) exceeds
C i
C 1/TOL.
C
C METHOD
C
C The conventional Kalman filter gain used at the i-th recursion
C step is of the form
C
C -1
C K = P C' RINOV ,
C i i|i-1 i i
C
C where RINOV = C P C' + R , and the state covariance matrix
C i i i|i-1 i i
C
C P is updated by the discrete-time difference Riccati equation
C i|i-1
C
C P = A (P - K C P ) A' + B Q B'.
C i+1|i i i|i-1 i i i|i-1 i i i i
C
C Using these two updates, the combined time and measurement update
C of the state X is given by
C i|i-1
C
C X = A X + A K (Y - C X ),
C i+1|i i i|i-1 i i i i i|i-1
C
C where Y is the new observation at step i.
C i
C
C REFERENCES
C
C [1] Anderson, B.D.O. and Moore, J.B.
C Optimal Filtering,
C Prentice Hall, Englewood Cliffs, New Jersey, 1979.
C
C [2] Verhaegen, M.H.G. and Van Dooren, P.
C Numerical Aspects of Different Kalman Filter Implementations.
C IEEE Trans. Auto. Contr., AC-31, pp. 907-917, 1986.
C
C NUMERICAL ASPECTS
C
C The algorithm requires approximately
C
C 3 2
C 3/2 x N + N x (3 x L + M/2)
C
C operations.
C
C CONTRIBUTORS
C
C Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Feb. 1997.
C Supersedes Release 2.0 routine FB01JD by M.H.G. Verhaegen,
C M. Vanbegin, and P. Van Dooren.
C
C REVISIONS
C
C February 20, 1998, November 20, 2003, April 20, 2004.
C
C KEYWORDS
C
C Kalman filtering, optimal filtering, recursive estimation.
C
C ******************************************************************
C
C .. Parameters ..
DOUBLE PRECISION ZERO, ONE, TWO
PARAMETER ( ZERO = 0.0D0, ONE = 1.0D0, TWO = 2.0D0 )
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,*)
C .. Local Scalars ..
INTEGER J, JWORK, LDW, N1
DOUBLE PRECISION RCOND, RNORM, TOLDEF
C .. External Functions ..
DOUBLE PRECISION DLAMCH, DLANSY
EXTERNAL DLAMCH, DLANSY
C .. External Subroutines ..
EXTERNAL DAXPY, DCOPY, DGEMV, DLACPY, DLASET, DPOCON,
$ DPOTRF, DSCAL, DTRMM, DTRSM, MB01RD, XERBLA
C .. Intrinsic Functions ..
INTRINSIC DBLE, MAX
C .. Executable Statements ..
C
C Test the input scalar arguments.
C
INFO = 0
N1 = MAX( 1, N )
IF( N.LT.0 ) THEN
INFO = -1
ELSE IF( M.LT.0 ) THEN
INFO = -2
ELSE IF( L.LT.0 ) THEN
INFO = -3
ELSE IF( LDP.LT.N1 ) THEN
INFO = -5
ELSE IF( LDA.LT.N1 ) THEN
INFO = -7
ELSE IF( LDB.LT.N1 ) THEN
INFO = -9
ELSE IF( LDC.LT.MAX( 1, L ) ) THEN
INFO = -11
ELSE IF( LDQ.LT.MAX( 1, M ) ) THEN
INFO = -13
ELSE IF( LDR.LT.MAX( 1, L ) ) THEN
INFO = -15
ELSE IF( LDK.LT.N1 ) THEN
INFO = -17
ELSE IF( LDWORK.LT.MAX( 1, L*N + 3*L, N*N, N*M ) ) THEN
INFO = -21
END IF
C
IF ( INFO.NE.0 ) THEN
C
C Error return.
C
CALL XERBLA( 'FB01VD', -INFO )
RETURN
END IF
C
C Quick return if possible.
C
IF ( MAX( N, L ).EQ.0 ) THEN
DWORK(1) = ONE
RETURN
END IF
C
C Efficiently compute RINOV = CPC' + R in R and put CP in DWORK and
C PC' in K. (The content of DWORK on exit from MB01RD is used.)
C Workspace: need L*N.
C
C (Note: Comments in the code beginning "Workspace:" describe the
C minimal amount of real workspace needed at that point in the
C code.)
C
CALL MB01RD( 'Upper', 'No transpose', L, N, ONE, ONE, R, LDR, C,
$ LDC, P, LDP, DWORK, LDWORK, INFO )
LDW = MAX( 1, L )
C
DO 10 J = 1, L
CALL DCOPY( N, DWORK(J), LDW, K(1,J), 1 )
10 CONTINUE
C
CALL DLACPY( 'Full', L, N, C, LDC, DWORK, LDW )
CALL DTRMM( 'Right', 'Upper', 'Transpose', 'Non-unit', L, N, ONE,
$ P, LDP, DWORK, LDW )
CALL DSCAL( N, TWO, P, LDP+1 )
C
DO 20 J = 1, L
CALL DAXPY( N, ONE, K(1,J), 1, DWORK(J), LDW )
CALL DCOPY( N, DWORK(J), LDW, K(1,J), 1 )
20 CONTINUE
C
C Calculate the Cholesky decomposition U'U of the innovation
C covariance matrix RINOV, and its reciprocal condition number.
C Workspace: need L*N + 3*L.
C
JWORK = L*N + 1
RNORM = DLANSY( '1-norm', 'Upper', L, R, LDR, DWORK(JWORK) )
C
TOLDEF = TOL
IF ( TOLDEF.LE.ZERO )
$ TOLDEF = DBLE( L*L )*DLAMCH( 'Epsilon' )
CALL DPOTRF( 'Upper', L, R, LDR, INFO )
IF ( INFO.NE.0 )
$ RETURN
C
CALL DPOCON( 'Upper', L, R, LDR, RNORM, RCOND, DWORK(JWORK),
$ IWORK, INFO )
C
IF ( RCOND.LT.TOLDEF ) THEN
C
C Error return: RINOV is numerically singular.
C
INFO = L+1
DWORK(1) = RCOND
RETURN
END IF
C
IF ( L.GT.1 )
$ CALL DLASET( 'Lower', L-1, L-1, ZERO, ZERO, R(2,1),LDR )
C -1
C Calculate the Kalman filter gain matrix K = PC'RINOV .
C Workspace: need L*N.
C
CALL DTRSM( 'Right', 'Upper', 'No transpose', 'Non-unit', N, L,
$ ONE, R, LDR, K, LDK )
CALL DTRSM( 'Right', 'Upper', 'Transpose', 'Non-unit', N, L,
$ ONE, R, LDR, K, LDK )
C
C First part of the Riccati equation update: compute A(P-KCP)A'.
C The upper triangular part of the symmetric matrix P-KCP is formed.
C Workspace: need max(L*N,N*N).
C
JWORK = 1
C
DO 30 J = 1, N
CALL DGEMV( 'No transpose', J, L, -ONE, K, LDK, DWORK(JWORK),
$ 1, ONE, P(1,J), 1 )
JWORK = JWORK + L
30 CONTINUE
C
CALL MB01RD( 'Upper', 'No transpose', N, N, ZERO, ONE, P, LDP, A,
$ LDA, P, LDP, DWORK, LDWORK, INFO )
C
C Second part of the Riccati equation update: add BQB'.
C Workspace: need N*M.
C
CALL MB01RD( 'Upper', 'No transpose', N, M, ONE, ONE, P, LDP, B,
$ LDB, Q, LDQ, DWORK, LDWORK, INFO )
CALL DSCAL( M, TWO, Q, LDQ+1 )
C
C Set the reciprocal of the condition number estimate.
C
DWORK(1) = RCOND
C
RETURN
C *** Last line of FB01VD ***
END