# NAG Library Function Document

## 1Purpose

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.

## 2Specification

 #include #include
 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)

## 3Description

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 ${\stackrel{^}{X}}_{i\mid i-1}$ with $\mathrm{var}\left({\stackrel{^}{X}}_{i\mid i-1}\right)={P}_{i\mid i-1}={S}_{i}{S}_{i}^{\mathrm{T}}$ (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 i∣i = F i+1 - 1 / 2 * * 0 S i+1 -1 ξ i + 1∣i + 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 $\left({A}^{-1},{A}^{-1}B\right)$ 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 $\stackrel{-}{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\mid i}$ is factored as follows
 $P i∣i -1 = S i -1 T S i -1$
where ${P}_{i\mid i}={S}_{i}{S}_{i}^{\mathrm{T}}$ (${S}_{i}$ is lower). The new state filtered state estimate is computed via
 $X ^ i + 1∣i + 1 = S i+1 -1 ξ i + 1∣i + 1$
The function returns ${S}_{i+1}^{-1}$ and, optionally, ${\stackrel{^}{X}}_{i+1\mid i+1}$ (see the Introduction to Chapter g13 for more information concerning the information filter).
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

## 5Arguments

1:    $\mathbf{n}$IntegerInput
On entry: the actual state dimension, $n$, i.e., the order of the matrices ${S}_{i}^{-1}$ and ${A}^{-1}$.
Constraint: ${\mathbf{n}}\ge 1$.
2:    $\mathbf{m}$IntegerInput
On entry: the actual input dimension, $m$, i.e., the order of the matrix ${Q}_{i}^{-1/2}$.
Constraint: ${\mathbf{m}}\ge 1$.
3:    $\mathbf{p}$IntegerInput
On entry: the actual output dimension, $p$, i.e., the order of the matrix ${R}_{i}^{-1/2}$.
Constraint: ${\mathbf{p}}\ge 1$.
4:    $\mathbf{t}\left[{\mathbf{n}}×{\mathbf{tdt}}\right]$doubleInput/Output
Note: the $\left(i,j\right)$th element of the matrix $T$ is stored in ${\mathbf{t}}\left[\left(i-1\right)×{\mathbf{tdt}}+j-1\right]$.
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\mid 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\mid i+1}$.
5:    $\mathbf{tdt}$IntegerInput
On entry: the stride separating matrix column elements in the array t.
Constraint: ${\mathbf{tdt}}\ge {\mathbf{n}}$.
6:    $\mathbf{ainv}\left[{\mathbf{n}}×{\mathbf{tda}}\right]$const doubleInput
Note: the $\left(i,j\right)$th element of the matrix is stored in ${\mathbf{ainv}}\left[\left(i-1\right)×{\mathbf{tda}}+j-1\right]$.
On entry: the leading $n$ by $n$ part of this array must contain the upper controller Hessenberg matrix $U{A}^{-1}{U}^{\mathrm{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:    $\mathbf{tda}$IntegerInput
On entry: the stride separating matrix column elements in the array ainv.
Constraint: ${\mathbf{tda}}\ge {\mathbf{n}}$.
8:    $\mathbf{ainvb}\left[{\mathbf{n}}×{\mathbf{tdai}}\right]$const doubleInput
Note: the $\left(i,j\right)$th element of the matrix is stored in ${\mathbf{ainvb}}\left[\left(i-1\right)×{\mathbf{tdai}}+j-1\right]$.
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:    $\mathbf{tdai}$IntegerInput
On entry: the stride separating matrix column elements in the array ainvb.
Constraint: ${\mathbf{tdai}}\ge {\mathbf{m}}$.
10:  $\mathbf{rinv}\left[{\mathbf{p}}×{\mathbf{tdr}}\right]$const doubleInput
Note: the $\left(i,j\right)$th element of the matrix is stored in ${\mathbf{rinv}}\left[\left(i-1\right)×{\mathbf{tdr}}+j-1\right]$.
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:  $\mathbf{tdr}$IntegerInput
On entry: the stride separating matrix column elements in the array rinv.
Constraint: ${\mathbf{tdr}}\ge {\mathbf{p}}$ if rinv is defined.
12:  $\mathbf{c}\left[{\mathbf{p}}×{\mathbf{tdc}}\right]$const doubleInput
Note: the $\left(i,j\right)$th element of the matrix $C$ is stored in ${\mathbf{c}}\left[\left(i-1\right)×{\mathbf{tdc}}+j-1\right]$.
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}^{\mathrm{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}^{\mathrm{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:  $\mathbf{tdc}$IntegerInput
On entry: the stride separating matrix column elements in the array c.
Constraint: ${\mathbf{tdc}}\ge {\mathbf{n}}$.
14:  $\mathbf{qinv}\left[{\mathbf{m}}×{\mathbf{tdq}}\right]$const doubleInput
Note: the $\left(i,j\right)$th element of the matrix is stored in ${\mathbf{qinv}}\left[\left(i-1\right)×{\mathbf{tdq}}+j-1\right]$.
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:  $\mathbf{tdq}$IntegerInput
On entry: the stride separating matrix column elements in the array qinv.
Constraint: ${\mathbf{tdq}}\ge {\mathbf{m}}$.
16:  $\mathbf{x}\left[{\mathbf{n}}\right]$doubleInput/Output
On entry: this array must contain the estimated state ${\stackrel{^}{X}}_{i\mid i}$
On exit: this array contains the estimated state ${\stackrel{^}{X}}_{i+1\mid i+1}$.
17:  $\mathbf{rinvy}\left[{\mathbf{p}}\right]$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:  $\mathbf{z}\left[{\mathbf{m}}\right]$const doubleInput
On entry: this array must contain $\stackrel{-}{w}$, the mean value of the state process noise.
19:  $\mathbf{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}×\epsilon$ then the tolerance is taken as ${n}^{2}×\epsilon$, where $\epsilon$ is the machine precision.
20:  $\mathbf{fail}$NagError *Input/Output
The NAG error argument (see Section 3.7 in How to Use the NAG Library and its Documentation).

## 6Error Indicators and Warnings

NE_2_INT_ARG_LT
On entry, ${\mathbf{tdt}}=〈\mathit{\text{value}}〉$ while ${\mathbf{n}}=〈\mathit{\text{value}}〉$. These arguments must satisfy ${\mathbf{tdt}}\ge {\mathbf{n}}$.
On entry ${\mathbf{tda}}=〈\mathit{\text{value}}〉$ while ${\mathbf{n}}=〈\mathit{\text{value}}〉$. These arguments must satisfy ${\mathbf{tda}}\ge {\mathbf{n}}$.
On entry ${\mathbf{tdai}}=〈\mathit{\text{value}}〉$ while ${\mathbf{m}}=〈\mathit{\text{value}}〉$. These arguments must satisfy ${\mathbf{tdai}}\ge {\mathbf{m}}$.
On entry ${\mathbf{tdc}}=〈\mathit{\text{value}}〉$ while ${\mathbf{n}}=〈\mathit{\text{value}}〉$. These arguments must satisfy ${\mathbf{tdc}}\ge {\mathbf{n}}$.
On entry ${\mathbf{tdq}}=〈\mathit{\text{value}}〉$ while ${\mathbf{m}}=〈\mathit{\text{value}}〉$. These arguments must satisfy ${\mathbf{tdq}}\ge {\mathbf{m}}$.
On entry ${\mathbf{tdr}}=〈\mathit{\text{value}}〉$ while ${\mathbf{p}}=〈\mathit{\text{value}}〉$. These arguments must satisfy ${\mathbf{tdr}}\ge {\mathbf{p}}$.
NE_ALLOC_FAIL
Dynamic memory allocation failed.
NE_INT_ARG_LT
On entry, ${\mathbf{m}}=〈\mathit{\text{value}}〉$.
Constraint: ${\mathbf{m}}\ge 1$.
On entry, ${\mathbf{n}}=〈\mathit{\text{value}}〉$.
Constraint: ${\mathbf{n}}\ge 1$.
On entry, ${\mathbf{p}}=〈\mathit{\text{value}}〉$.
Constraint: ${\mathbf{p}}\ge 1$.
NE_MAT_SINGULAR
The matrix inverse(S) is singular.

## 7Accuracy

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

## 8Parallelism and Performance

nag_kalman_sqrt_filt_info_invar (g13edc) is not threaded in any implementation.

The algorithm requires approximately $\frac{1}{6}{n}^{3}+{n}^{2}\left(\frac{3}{2}m+p\right)+2n{m}^{2}+\frac{2}{3}{m}^{3}$ operations and is backward stable (see Verhaegen and van Dooren (1986)).

## 10Example

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 $\left({A}^{-1},{A}^{-1}B,C\right)$ 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 $\left({A}^{-1},{A}^{-1}B,C\right)$. 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.1Program Text

Program Text (g13edce.c)

### 10.2Program Data

Program Data (g13edce.d)

### 10.3Program Results

Program Results (g13edce.r)

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