nag_kalman_sqrt_filt_cov_invar (g13ebc) (PDF version)
g13 Chapter Contents
g13 Chapter Introduction
NAG C Library Manual

NAG Library Function Document

nag_kalman_sqrt_filt_cov_invar (g13ebc)

+ Contents

    1  Purpose
    7  Accuracy

1  Purpose

nag_kalman_sqrt_filt_cov_invar (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>
#include <nagg13.h>
void  nag_kalman_sqrt_filt_cov_invar (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)

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, summarized 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:     nIntegerInput
On entry: the actual state dimension, n , i.e., the order of the matrices S i  and A .
Constraint: n1 .
2:     mIntegerInput
On entry: the actual input dimension, m , i.e., the order of the matrix Q i 1/2 .
Constraint: m1 .
3:     pIntegerInput
On entry: the actual output dimension, p , i.e., the order of the matrix R i 1/2 .
Constraint: p1 .
4:     s[n×tds]doubleInput/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:     tdsIntegerInput
On entry: the stride separating matrix column elements in the array s.
Constraint: tdsn .
6:     a[n×tda]const doubleInput
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 nag_trans_hessenberg_observer (g13ewc).
7:     tdaIntegerInput
On entry: the stride separating matrix column elements in the array a.
Constraint: tdan .
8:     b[n×tdb]const doubleInput
Note: the i,jth element of the matrix B is stored in b[i-1×tdb+j-1].
On entry: if the array argument q (below) has been defined then the leading n  by m  part of this array must contain the matrix UB , otherwise (if q is the null pointer (double *)0) 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:     tdbIntegerInput
On entry: the stride separating matrix column elements in the array b.
Constraint: tdbm .
10:   q[m×tdq]const doubleInput
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 the null pointer, i.e., (double *)0.
11:   tdqIntegerInput
On entry: the stride separating matrix column elements in the array q.
Constraint: tdqm  if q is defined.
12:   c[p×tdc]const doubleInput
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 nag_trans_hessenberg_observer (g13ewc).
13:   tdcIntegerInput
On entry: the stride separating matrix column elements in the array c.
Constraint: tdcn .
14:   r[p×tdr]const doubleInput
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:   tdrIntegerInput
On entry: the stride separating matrix column elements in the array r.
Constraint: tdrp .
16:   k[n×tdk]doubleOutput
Note: the i,jth element of the matrix K is stored in k[i-1×tdk+j-1].
On exit: if k is defined, then the leading n  by p  part of this array contains the AK i , the product of the Kalman filter gain matrix K i  with the state transition matrix A . If this is not required then the array k must be set to the null pointer, i.e., (double *)0.
17:   tdkIntegerInput
On entry: the stride separating matrix column elements in the array k.
Constraint: tdkp  if k is defined.
18:   h[p×tdh]doubleOutput
Note: the i,jth element of the matrix H is stored in h[i-1×tdh+j-1].
On exit: if k is defined, then the leading p  by p  lower triangular part of this array contains H i 1/2 . If k has not been defined then array h is not referenced and may be set to the null pointer, i.e., (double *)0.
19:   tdhIntegerInput
On entry: the stride separating matrix column elements in the array h.
Constraint: tdhp  if k and h are defined.
20:   toldoubleInput
On entry: if k is defined, 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:   failNagError *Input/Output
The NAG error argument (see Section 3.6 in the Essential Introduction).

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

9  Example

For this function two examples are presented. There is a single example program for nag_kalman_sqrt_filt_cov_invar (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 nag_kalman_sqrt_filt_cov_var (g13eac) is compared with that of the time-invariant function nag_kalman_sqrt_filt_cov_invar (g13ebc). The same original data is used by both functions but additional transformations are required before it can be supplied to nag_kalman_sqrt_filt_cov_invar (g13ebc). It can be seen that (after the appropriate back-transformations on the output of nag_kalman_sqrt_filt_cov_invar (g13ebc)) the results of both nag_kalman_sqrt_filt_cov_var (g13eac) and nag_kalman_sqrt_filt_cov_invar (g13ebc) are the same.

9.1  Program Text

Program Text (g13ebce.c)

9.2  Program Data

Program Data (g13ebce.d)

9.3  Program Results

Program Results (g13ebce.r)


nag_kalman_sqrt_filt_cov_invar (g13ebc) (PDF version)
g13 Chapter Contents
g13 Chapter Introduction
NAG C Library Manual

© The Numerical Algorithms Group Ltd, Oxford, UK. 2012