nag_kalman_sqrt_filt_info_invar (g13edc) (PDF version)
g13 Chapter Contents
g13 Chapter Introduction
NAG Library Manual

NAG Library Function Document

nag_kalman_sqrt_filt_info_invar (g13edc)

+ Contents

    1  Purpose
    7  Accuracy

1  Purpose

nag_kalman_sqrt_filt_info_invar (g13edc) 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 information filter with the system matrices in condensed controller Hessenberg form.

2  Specification

#include <nag.h>
#include <nagg13.h>
void  nag_kalman_sqrt_filt_info_invar (Integer n, Integer m, Integer p, double t[], Integer tdt, const double ainv[], Integer tda, const double ainvb[], Integer tdai, const double rinv[], Integer tdr, const double c[], Integer tdc, const double qinv[], Integer tdq, double x[], const double rinvy[], const double z[], 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 information filter algorithm, summarised as follows:
U 1 Q i - 1 / 2 0 Q i - 1 / 2 w - 0 R i - 1 / 2 C R - 1 / 2 Y i+1 S i -1 A -1 B S i -1 A -1 S i -1 X ii = F i+1 - 1 / 2 * * 0 S i+1 -1 ξ i + 1i + 1 0 0 E i+1 Pre-array Post-array
where U 1  is an orthogonal transformation triangularizing the pre-array, and the matrix pair A -1 , A -1 B  is in upper controller Hessenberg form. The triangularization is done entirely via Householder transformations exploiting the zero pattern of the pre-array. An example of the pre-array is given below (where n=6 , m=2  and p=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
The term w -  is the mean process noise, and E i+1  is the estimated error at instant i+1 . The inverse of the state covariance matrix P ii  is factored as follows
P ii -1 = S i -1 T S i -1
where P ii = S i SiT  ( S i  is lower). The new state filtered state estimate is computed via
X ^ i + 1i + 1 = S i+1 -1 ξ i + 1i + 1
The function returns S i+1 -1  and, optionally, X ^ i + 1i + 1  (see the Introduction to Chapter g13 for more information concerning the information 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 -1  and A -1 .
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:     t[n×tdt]doubleInput/Output
Note: the i,jth element of the matrix T is stored in t[i-1×tdt+j-1].
On entry: the leading n  by n  upper triangular part of this array must contain S i -1  the square root of the inverse of the state covariance matrix P ii .
On exit: the leading n  by n  upper triangular part of this array contains S i+1 -1 , the square root of the inverse of the of the state covariance matrix P i + 1i + 1 .
5:     tdtIntegerInput
On entry: the stride separating matrix column elements in the array t.
Constraint: tdtn .
6:     ainv[n×tda]const doubleInput
Note: the i,jth element of the matrix is stored in ainv[i-1×tda+j-1].
On entry: the leading n  by n  part of this array must contain the upper controller Hessenberg matrix U A -1 UT . Where A -1  is the inverse of the state transition matrix, and U  is the unitary matrix generated by the function nag_trans_hessenberg_controller (g13exc).
7:     tdaIntegerInput
On entry: the stride separating matrix column elements in the array ainv.
Constraint: tdan .
8:     ainvb[n×tdai]const doubleInput
Note: the i,jth element of the matrix is stored in ainvb[i-1×tdai+j-1].
On entry: the leading n  by m  part of this array must contain the upper controller Hessenberg matrix U A -1 B . Where A -1  is the inverse of the transition matrix, B  is the input weight matrix B , and U  is the unitary transformation generated by the function nag_trans_hessenberg_controller (g13exc).
9:     tdaiIntegerInput
On entry: the stride separating matrix column elements in the array ainvb.
Constraint: tdaim .
10:   rinv[p×tdr]const doubleInput
Note: the i,jth element of the matrix is stored in rinv[i-1×tdr+j-1].
On entry: if the noise covariance matrix is to be supplied separately from the output weight matrix, then the leading p  by p  upper triangular part of this array must contain R - 1 / 2  the right Cholesky factor of the inverse of the measurement noise covariance matrix. If this information is not to be input separately from the output weight matrix c then the array rinv must be set to NULL.
11:   tdrIntegerInput
On entry: the stride separating matrix column elements in the array rinv.
Constraint: tdrp  if rinv 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: if the array argument rinv (above) has been defined then the leading p  by n  part of this array must contain the matrix C UT , otherwise (if rinv is NULL then the leading p  by n  part of the array must contain the matrix R i - 1 / 2 C UT . C  is the output weight matrix, R i  is the noise covariance matrix and U  is the same unitary transformation used for defining array arguments ainv and ainvb.
13:   tdcIntegerInput
On entry: the stride separating matrix column elements in the array c.
Constraint: tdcn .
14:   qinv[m×tdq]const doubleInput
Note: the i,jth element of the matrix is stored in qinv[i-1×tdq+j-1].
On entry: the leading m  by m  upper triangular part of this array must contain Q i - 1 / 2  the right Cholesky factor of the inverse of the process noise covariance matrix.
15:   tdqIntegerInput
On entry: the stride separating matrix column elements in the array qinv.
Constraint: tdqm .
16:   x[n]doubleInput/Output
On entry: this array must contain the estimated state X ^ ii
On exit: this array contains the estimated state X ^ i + 1i + 1 .
17:   rinvy[p]const doubleInput
On entry: this array must contain R i - 1 / 2 Y i+1 , the product of the upper triangular matrix R i - 1 / 2  and the measured output vector Y i+1 .
18:   z[m]const doubleInput
On entry: this array must contain w - , the mean value of the state process noise.
19:   toldoubleInput
On entry: tol is used to test for near singularity of the matrix S i+1 . If you set tol to be less than n 2 × ε  then the tolerance is taken as n 2 × ε , where ε  is the machine precision.
20:   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, tdt=value  while n=value . These arguments must satisfy tdtn .
On entry tda=value  while n=value . These arguments must satisfy tdan .
On entry tdai=value  while m=value . These arguments must satisfy tdaim .
On entry tdc=value  while n=value . These arguments must satisfy tdcn .
On entry tdq=value  while m=value . These arguments must satisfy tdqm .
On entry tdr=value  while p=value . These arguments must satisfy tdrp .
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 inverse(S) is singular.

7  Accuracy

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

8  Parallelism and Performance

Not applicable.

9  Further Comments

The algorithm requires approximately 1 6 n 3 + n 2 3 2 m + p + 2 nm2 + 2 3 m 3  operations and is backward stable (see Verhaegen and van Dooren (1986)).

10  Example

For this function two examples are presented. There is a single example program for nag_kalman_sqrt_filt_info_invar (g13edc), 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 information form) to the time-invariant system A -1 , A -1 B , C  supplied in upper controller Hessenberg form.
Example 2 ex2)
To apply three iterations of the Kalman filter (in square root information form) to the general time-invariant system A -1 , A -1 B , C . The use of the time-varying Kalman function nag_kalman_sqrt_filt_info_var (g13ecc) is compared with that of the time-invariant function nag_kalman_sqrt_filt_info_invar (g13edc). The same original data is used by both functions but additional transformations are required before it can be supplied to nag_kalman_sqrt_filt_info_invar (g13edc). It can be seen that (after the appropriate back-transformations on the output of nag_kalman_sqrt_filt_info_invar (g13edc)) the results of both nag_kalman_sqrt_filt_info_var (g13ecc) and nag_kalman_sqrt_filt_info_invar (g13edc) are in agreeement.

10.1  Program Text

Program Text (g13edce.c)

10.2  Program Data

Program Data (g13edce.d)

10.3  Program Results

Program Results (g13edce.r)


nag_kalman_sqrt_filt_info_invar (g13edc) (PDF version)
g13 Chapter Contents
g13 Chapter Introduction
NAG Library Manual

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