NAG Library Function Document

nag_kalman_sqrt_filt_info_invar (g13edc)

+− 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_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

\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 information filter algorithm, summarised as follows:

\begin{matrix} U_{1} & (\begin{matrix} Q_{i}^{- 1 / 2} & 0 & Q_{i}^{- 1 / 2} \bar{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_{i ∣ i} \end{matrix}) & = & (\begin{matrix} F_{i + 1}^{- 1 / 2} & * & * \\ 0 & S_{i + 1}^{- 1} & ξ_{i + 1 ∣ i + 1} \\ 0 & 0 & E_{i + 1} \end{matrix}) \\ (Pre-array) & (Post-array) \end{matrix}

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

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

The term

\bar{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_{i ∣ i}

is factored as follows

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

where

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

(

S_{i}

is lower). The new state filtered state estimate is computed via

{\hat{X}}_{i + 1 ∣ i + 1} = S_{i + 1}^{- 1} ξ_{i + 1 ∣ i + 1}

The function returns

S_{i + 1}^{- 1}

and, optionally,

{\hat{X}}_{i + 1 ∣ i + 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: n – IntegerInput: On entry: the actual state dimension, $n$ , i.e., the order of the matrices $S_{i}^{- 1}$ and $A^{- 1}$ .
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: t[ $n \times tdt$ ] – doubleInput/Output: Note: the $(i, j)$ th element of the matrix $T$ is stored in $t [(i - 1) \times 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_{i ∣ i}$ .

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 + 1 ∣ i + 1}$ .
5: tdt – IntegerInput: On entry: the stride separating matrix column elements in the array t.

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

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

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

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

Constraint: $tdr \geq p$ if rinv 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: if the array argument rinv (above) has been defined then the leading $p$ by $n$ part of this array must contain the matrix $C U^{T}$ , otherwise (if rinv is NULL then the leading $p$ by $n$ part of the array must contain the matrix $R_{i}^{- 1 / 2} C U^{T}$ . $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: tdc – IntegerInput: On entry: the stride separating matrix column elements in the array c.

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

Constraint: $tdq \geq m$ .
16: x[n] – doubleInput/Output: On entry: this array must contain the estimated state ${\hat{X}}_{i ∣ i}$

On exit: this array contains the estimated state ${\hat{X}}_{i + 1 ∣ i + 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 $\bar{w}$ , the mean value of the state process noise.
19: tol – doubleInput: 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} \times ε$ then the tolerance is taken as $n^{2} \times ε$ , where $ε$ is the machine precision.
20: 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, $tdt = ⟨value⟩$ while $n = ⟨value⟩$ . These arguments must satisfy $tdt \geq n$ .
On entry $tda = ⟨value⟩$ while $n = ⟨value⟩$ . These arguments must satisfy $tda \geq n$ .
On entry $tdai = ⟨value⟩$ while $m = ⟨value⟩$ . These arguments must satisfy $tdai \geq m$ .
On entry $tdc = ⟨value⟩$ while $n = ⟨value⟩$ . These arguments must satisfy $tdc \geq n$ .
On entry $tdq = ⟨value⟩$ while $m = ⟨value⟩$ . These arguments must satisfy $tdq \geq m$ .
On entry $tdr = ⟨value⟩$ while $p = ⟨value⟩$ . These arguments must satisfy $tdr \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 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

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

NAG Library Function Documentnag_kalman_sqrt_filt_info_invar (g13edc)