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>

void

g13edc (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)

The function may be called by the names: g13edc, nag_tsa_kalman_sqrt_filt_info_invar or nag_kalman_sqrt_filt_info_invar.

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$ – Integer Input: 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$ – Integer Input: On entry: the actual input dimension, $m$ , i.e., the order of the matrix $Q_{i}^{- 1 / 2}$ .

Constraint: $m \geq 1$ .
3: $p$ – Integer Input: 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]$ – double Input/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 \times 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 \times 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$ – Integer Input: On entry: the stride separating matrix column elements in the array t.

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

On entry: the leading $n \times 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 g13exc.
7: $tda$ – Integer Input: On entry: the stride separating matrix column elements in the array ainv.

Constraint: $tda \geq n$ .
8: $ainvb [n \times tdai]$ – const double Input: Note: the $(i, j)$ th element of the matrix is stored in $ainvb [(i - 1) \times tdai + j - 1]$ .

On entry: the leading $n \times 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 g13exc.
9: $tdai$ – Integer Input: On entry: the stride separating matrix column elements in the array ainvb.

Constraint: $tdai \geq m$ .
10: $rinv [p \times tdr]$ – const double Input: 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 \times 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$ – Integer Input: 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 double Input: 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 \times n$ part of this array must contain the matrix $C U^{T}$ , otherwise (if rinv is NULL then the leading $p \times 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$ – Integer Input: On entry: the stride separating matrix column elements in the array c.

Constraint: $tdc \geq n$ .
14: $qinv [m \times tdq]$ – const double Input: Note: the $(i, j)$ th element of the matrix is stored in $qinv [(i - 1) \times tdq + j - 1]$ .

On entry: the leading $m \times 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$ – Integer Input: On entry: the stride separating matrix column elements in the array qinv.

Constraint: $tdq \geq m$ .
16: $x [n]$ – double Input/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 double Input: 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 double Input: On entry: this array must contain $\bar{w}$ , the mean value of the state process noise.
19: $tol$ – double Input: 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 7 in the Introduction to the NAG Library CL Interface).

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

Background information to multithreading can be found in the Multithreading documentation.

g13edc 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 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 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 g13ecc is compared with that of the time-invariant function g13edc. The same original data is used by both functions but additional transformations are required before it can be supplied to g13edc. It can be seen that (after the appropriate back-transformations on the output of g13edc) the results of both g13ecc and g13edc are in agreeement.

g13ed: FL CL CPP AD PY MB

NAG CL Interfaceg13edc (kalman_​sqrt_​filt_​info_​invar)

▸▿ 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

NAG CL Interface
g13edc (kalman_sqrt_filt_info_invar)