NAG Library Function Document

nag_kalman_sqrt_filt_cov_invar (g13ebc)

+− Contents

1 Purpose

2 Specification

3 Description

4 References

5 Arguments

6 Error Indicators and Warnings

7 Accuracy

8 Parallelism and Performance

9 Further Comments

+− 10 Example

10.1 Program Text

10.2 Program Data

10.3 Program Results

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

\begin{array}{l} 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} \end{array}

the estimate of

X_{i}

given observations

Y_{1}

Y_{i - 1}

is denoted by

{\hat{X}}_{i ∣ i - 1}

, with

var ({\hat{X}}_{i ∣ i - 1}) = P_{i ∣ i - 1} = S_{i} S_{i}^{T}

(where

A

B

and

C

are time invariant). The function performs one recursion of the square root covariance filter algorithm, summarised as follows:

\begin{matrix} (\begin{matrix} R_{i}^{1 / 2} & 0 & C S_{i} \\ 0 & B Q_{i}^{1 / 2} & A S_{i} \end{matrix}) & U_{1} = & (\begin{matrix} H_{i}^{1 / 2} & 0 & 0 \\ G_{i} & S_{i + 1} & 0 \end{matrix}) \\ (Pre-array) & (Post-array) \end{matrix}

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

(\begin{matrix} 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 \end{matrix})

The measurement-update for the estimated state vector

X

{\hat{X}}_{i ∣ i} = {\hat{X}}_{i ∣ i - 1} - K_{i} [C {\hat{X}}_{i ∣ i - 1} - Y_{i}]

whilst the time-update for

X

{\hat{X}}_{i + 1 ∣ i} = A {\hat{X}}_{i ∣ i} + 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}

A K_{i} = G_{i} (H_{i}^{1 / 2})

The function returns the product of the matrices

A

and

K_{i}

, represented as

{A K}_{i}

, and the state covariance matrix

P_{i ∣ i - 1}

factorized as

P_{i ∣ i - 1} = S_{i} S_{i}^{T}

(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 – IntegerInput: On entry: the actual state dimension, $n$ , i.e., the order of the matrices $S_{i}$ and $A$ .
Constraint: $n \geq 1$ .
2: m – IntegerInput: On entry: the actual input dimension, $m$ , i.e., the order of the matrix $Q_{i}^{1 / 2}$ .
Constraint: $m \geq 1$ .
3: p – IntegerInput: On entry: the actual output dimension, $p$ , i.e., the order of the matrix $R_{i}^{1 / 2}$ .
Constraint: $p \geq 1$ .
4: s[ $n \times tds$ ] – doubleInput/Output: Note: the $(i, j)$ th element of the matrix $S$ is stored in $s [(i - 1) \times 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_{i ∣ i - 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 + 1 ∣ i}$ .
5: tds – IntegerInput: On entry: the stride separating matrix column elements in the array s.

Constraint: $tds \geq n$ .
6: a[ $n \times tda$ ] – const doubleInput: Note: the $(i, j)$ th element of the matrix $A$ is stored in $a [(i - 1) \times tda + j - 1]$ .

On entry: the leading $n$ by $n$ part of this array must contain the lower observer Hessenberg matrix $U A U^{T}$ . 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: tda – IntegerInput: On entry: the stride separating matrix column elements in the array a.

Constraint: $tda \geq n$ .
8: b[ $n \times tdb$ ] – const doubleInput: Note: the $(i, j)$ th element of the matrix $B$ is stored in $b [(i - 1) \times tdb + j - 1]$ .

On entry: if q is not NULL then the leading $n$ by $m$ part of this array must contain the matrix $U B$ , otherwise (if q is NULL then the leading $n$ by $m$ part of the array must contain the matrix ${U B Q}_{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 – IntegerInput: On entry: the stride separating matrix column elements in the array b.

Constraint: $tdb \geq m$ .
10: q[ $m \times tdq$ ] – const doubleInput: Note: the $(i, j)$ th element of the matrix $Q$ is stored in $q [(i - 1) \times 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 – IntegerInput: On entry: the stride separating matrix column elements in the array q.

Constraint: $tdq \geq m$ if q is defined.
12: c[ $p \times tdc$ ] – const doubleInput: Note: the $(i, j)$ th element of the matrix $C$ is stored in $c [(i - 1) \times tdc + j - 1]$ .

On entry: the leading $p$ by $n$ part of this array must contain the lower observer Hessenberg matrix $C U^{T}$ . 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: tdc – IntegerInput: On entry: the stride separating matrix column elements in the array c.

Constraint: $tdc \geq n$ .
14: r[ $p \times tdr$ ] – const doubleInput: Note: the $(i, j)$ th element of the matrix $R$ is stored in $r [(i - 1) \times 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 – IntegerInput: On entry: the stride separating matrix column elements in the array r.

Constraint: $tdr \geq p$ .
16: k[ $n \times tdk$ ] – doubleOutput: Note: the $(i, j)$ th element of the matrix $K$ is stored in $k [(i - 1) \times tdk + j - 1]$ .

On exit: if k is not NULL then the leading $n$ by $p$ part of k contains ${A K}_{i}$ , the product of the Kalman filter gain matrix $K_{i}$ with the state transition matrix $A_{i}$ . If $A K_{i}$ is not required then k must be set to NULL.
17: tdk – IntegerInput: On entry: the stride separating matrix column elements in the array k.

Constraint: $tdk \geq p$ if k is defined.
18: h[ $p \times tdh$ ] – doubleOutput: Note: the $(i, j)$ th element of the matrix $H$ is stored in $h [(i - 1) \times 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 – IntegerInput: On entry: the stride separating matrix column elements in the array h.

Constraint: $tdh \geq p$ if k and h are defined.
20: tol – doubleInput: 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 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 $tds \geq n$ . On entry $tda = ⟨value⟩$ while $n = ⟨value⟩$ . These arguments must satisfy $tda \geq n$ . On entry $tdb = ⟨value⟩$ while $m = ⟨value⟩$ . These arguments must satisfy $tdb \geq n$ . On entry $tdc = ⟨value⟩$ while $n = ⟨value⟩$ . These arguments must satisfy $tdc \geq n$ . On entry $tdr = ⟨value⟩$ while $p = ⟨value⟩$ . These arguments must satisfy $tdr \geq p$ . On entry $tdq = ⟨value⟩$ while $m = ⟨value⟩$ . These arguments must satisfy $tdq \geq m$ . On entry $tdk = ⟨value⟩$ while $p = ⟨value⟩$ . These arguments must satisfy $tdk \geq p$ . On entry $tdh = ⟨value⟩$ while $p = ⟨value⟩$ . These arguments must satisfy $tdh \geq p$ .
NE_ALLOC_FAIL: Dynamic memory allocation failed.
NE_INT_ARG_LT: On entry, $m = ⟨value⟩$ .
Constraint: $m \geq 1$ .
On entry, $n = ⟨value⟩$ .
Constraint: $n \geq 1$ .
On entry, $p = ⟨value⟩$ .
Constraint: $p \geq 1$ .
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

Not applicable.

9 Further Comments

The algorithm requires

\frac{1}{6} n^{3} + n^{2} (\frac{3}{2} p + m) + 2 n p^{2} + \frac{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 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.

NAG Library Function Documentnag_kalman_sqrt_filt_cov_invar (g13ebc)