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

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,*)

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

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

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.

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.

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

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.

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

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

None

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

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

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