NAG CL Interface
g13ebc (multi_​kalman_​sqrt_​invar)

1 Purpose

g13ebc performs a combined measurement and time update of one iteration of the time-invariant Kalman filter. The method employed for this update is the square root covariance filter with the system matrices transformed into condensed observer Hessenberg form.

2 Specification

#include <nag.h>
void  g13ebc (Integer n, Integer m, Integer p, double s[], Integer tds, const double a[], Integer tda, const double b[], Integer tdb, const double q[], Integer tdq, const double c[], Integer tdc, const double r[], Integer tdr, double k[], Integer tdk, double h[], Integer tdh, double tol, NagError *fail)
The function may be called by the names: g13ebc, nag_tsa_multi_kalman_sqrt_invar or nag_kalman_sqrt_filt_cov_invar.

3 Description

For the state space system defined by
X i+1 = A X i + B W i var W i = Q i Y i = C X i + V i var V i = R i  
the estimate of X i given observations Y 1 to Y i-1 is denoted by X ^ ii - 1 , with var X ^ ii - 1 = P ii - 1 = S i SiT (where A , B and C are time invariant). The function performs one recursion of the square root covariance filter algorithm, summarised as follows:
R i 1/2 0 C S i 0 B Q i 1/2 A S i U 1 = H i 1/2 0 0 G i S i+1 0 Pre-array Post-array  
where U 1 is an orthogonal transformation triangularizing the pre-array, and the matrix pair A,C is in lower observer Hessenberg form. The triangularization is carried out via Householder transformations exploiting the zero pattern of the pre-array. An example of the pre-array is given below (where n = 6 , p = 2 and m=3 ):
x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x  
The measurement-update for the estimated state vector X is
X ^ ii = X ^ ii - 1 - K i C X ^ ii - 1 - Y i  
whilst the time-update for X is
X ^ i + 1i = A X ^ ii + D i U i  
where D i U i represents any deterministic control used. The relationship between the Kalman gain matrix K i and G i is
A K i = G i H i 1/2  
The function returns the product of the matrices A and K i , represented as AK i , and the state covariance matrix P ii - 1 factorized as P ii - 1 = S i SiT (see the Introduction to Chapter G13 for more information concerning the covariance filter).

4 References

Anderson B D O and Moore J B (1979) Optimal Filtering Prentice–Hall
Vanbegin M, van Dooren P and Verhaegen M H G (1989) 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 243–256
van Dooren P and Verhaegen M H G (1988) Condensed forms for efficient time-invariant Kalman filtering SIAM J. Sci. Stat. Comput. 9 516–530
Verhaegen M H G and van Dooren P (1986) Numerical aspects of different Kalman filter implementations IEEE Trans. Auto. Contr. AC-31 907–917

5 Arguments

1: n Integer Input
On entry: the actual state dimension, n , i.e., the order of the matrices S i and A .
Constraint: n1 .
2: m Integer Input
On entry: the actual input dimension, m , i.e., the order of the matrix Q i 1/2 .
Constraint: m1 .
3: p Integer Input
On entry: the actual output dimension, p , i.e., the order of the matrix R i 1/2 .
Constraint: p1 .
4: s[n×tds] double Input/Output
Note: the i,jth element of the matrix S is stored in s[i-1×tds+j-1].
On entry: the leading n by n lower triangular part of this array must contain S i , the left Cholesky factor of the state covariance matrix P ii - 1 .
On exit: the leading n by n lower triangular part of this array contains S i+1 , the left Cholesky factor of the state covariance matrix P i + 1i .
5: tds Integer Input
On entry: the stride separating matrix column elements in the array s.
Constraint: tdsn .
6: a[n×tda] const double Input
Note: the i,jth element of the matrix A is stored in a[i-1×tda+j-1].
On entry: the leading n by n part of this array must contain the lower observer Hessenberg matrix U A UT . Where A is the state transition matrix of the discrete system and U is the unitary transformation generated by the function g13ewc.
7: tda Integer Input
On entry: the stride separating matrix column elements in the array a.
Constraint: tdan .
8: b[n×tdb] const double Input
Note: the i,jth element of the matrix B is stored in b[i-1×tdb+j-1].
On entry: if q is not NULL then the leading n by m part of this array must contain the matrix UB , otherwise (if q is NULL then the leading n by m part of the array must contain the matrix UBQ i 1/2 . B is the input weight matrix, Q i is the noise covariance matrix and U is the same unitary transformation used for defining array arguments a and c.
9: tdb Integer Input
On entry: the stride separating matrix column elements in the array b.
Constraint: tdbm .
10: q[m×tdq] const double Input
Note: the i,jth element of the matrix Q is stored in q[i-1×tdq+j-1].
On entry: if the noise covariance matrix is to be supplied separately from the input weight matrix then the leading m by m lower triangular part of this array must contain Q i 1/2 , the left Cholesky factor process noise covariance matrix. If the noise covariance matrix is to be input with the weight matrix as B Q i 1/2 then the array q must be set to NULL.
11: tdq Integer Input
On entry: the stride separating matrix column elements in the array q.
Constraint: tdqm if q is defined.
12: c[p×tdc] const double Input
Note: the i,jth element of the matrix C is stored in c[i-1×tdc+j-1].
On entry: the leading p by n part of this array must contain the lower observer Hessenberg matrix C UT . Where C is the output weight matrix of the discrete system and U is the unitary transformation matrix generated by the function g13ewc.
13: tdc Integer Input
On entry: the stride separating matrix column elements in the array c.
Constraint: tdcn .
14: r[p×tdr] const double Input
Note: the i,jth element of the matrix R is stored in r[i-1×tdr+j-1].
On entry: the leading p by p lower triangular part of this array must contain R i 1/2 , the left Cholesky factor of the measurement noise covariance matrix.
15: tdr Integer Input
On entry: the stride separating matrix column elements in the array r.
Constraint: tdrp .
16: k[n×tdk] double Output
Note: the i,jth element of the matrix K is stored in k[i-1×tdk+j-1].
On exit: if k is not NULL then the leading n by p part of k contains AK i , the product of the Kalman filter gain matrix K i with the state transition matrix A i . If AKi is not required then k must be set to NULL.
17: tdk Integer Input
On entry: the stride separating matrix column elements in the array k.
Constraint: tdkp if k is defined.
18: h[p×tdh] double Output
Note: the i,jth element of the matrix H is stored in h[i-1×tdh+j-1].
On exit: if k is not NULL then the leading p by p lower triangular part of this array contains H i 1/2 . If k is NULL then h is not referenced and may be set to NULL.
19: tdh Integer Input
On entry: the stride separating matrix column elements in the array h.
Constraint: tdhp if k and h are defined.
20: tol double Input
On entry: if both k and h are not NULL then tol is used to test for near singularity of the matrix H i 1/2 . If you set tol to be less than p 2 ε then the tolerance is taken as p 2 ε , where ε is the machine precision. Otherwise, tol need not be set by you.
21: fail NagError * Input/Output
The NAG error argument (see Section 7 in the Introduction to the NAG Library CL Interface).

6 Error Indicators and Warnings

NE_2_INT_ARG_LT
On entry, tds=value while n=value . These arguments must satisfy tdsn . On entry tda=value while n=value . These arguments must satisfy tdan . On entry tdb=value while m=value . These arguments must satisfy tdbn . On entry tdc=value while n=value . These arguments must satisfy tdcn . On entry tdr=value while p=value . These arguments must satisfy tdrp . On entry tdq=value while m=value . These arguments must satisfy tdqm . On entry tdk=value while p=value . These arguments must satisfy tdkp . On entry tdh=value while p=value . These arguments must satisfy tdhp .
NE_ALLOC_FAIL
Dynamic memory allocation failed.
NE_INT_ARG_LT
On entry, m=value.
Constraint: m1.
On entry, n=value.
Constraint: n1.
On entry, p=value.
Constraint: p1.
NE_MAT_SINGULAR
The matrix sqrt(H) is singular.
NE_NULL_ARRAY
Array h has null address.

7 Accuracy

The use of the square root algorithm improves the stability of the computations.

8 Parallelism and Performance

g13ebc makes calls to BLAS and/or LAPACK routines, which may be threaded within the vendor library used by this implementation. Consult the documentation for the vendor library for further information.
Please consult the X06 Chapter Introduction for information on how to control and interrogate the OpenMP environment used within this function. Please also consult the Users' Note for your implementation for any additional implementation-specific information.

9 Further Comments

The algorithm requires 1 6 n 3 + n 2 3 2 p + m + 2 np2 + 2 3 p 3 operations and is backward stable (see Verhaegen et al).

10 Example

For this function two examples are presented. There is a single example program for g13ebc, with a main program and the code to solve the two example problems is given in the functions ex1 and ex2.
Example 1 (ex1)
To apply three iterations of the Kalman filter (in square root covariance form) to the time-invariant system A,B,C supplied in lower observer Hessenberg form.
Example 2 (ex2)
To apply three iterations of the Kalman filter (in square root covariance form) to the general time-invariant system A,B,C . The use of the time-varying Kalman function g13eac is compared with that of the time-invariant function g13ebc. The same original data is used by both functions but additional transformations are required before it can be supplied to g13ebc. It can be seen that (after the appropriate back-transformations on the output of g13ebc) the results of both g13eac and g13ebc are the same.

10.1 Program Text

Program Text (g13ebce.c)

10.2 Program Data

Program Data (g13ebce.d)

10.3 Program Results

Program Results (g13ebce.r)