g13 Chapter Contents
g13 Chapter Introduction
NAG Library Manual

# NAG Library Function Documentnag_kalman_sqrt_filt_cov_invar (g13ebc)

## 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 #include
 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
 $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 covariance filter algorithm, summarised as follows:
 $R i 1/2 0 C S i 0 B Q i 1/2 A S i U 1 = H i 1/2 0 0 G i S i+1 0 Pre-array Post-array$
where ${U}_{1}$ is an orthogonal transformation triangularizing the pre-array, and the matrix pair $\left(A,C\right)$ 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$):
 $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$
The measurement-update for the estimated state vector $X$ is
 $X ^ i∣i = X ^ i∣i - 1 - K i C X ^ i∣i - 1 - Y i$
whilst the time-update for $X$ is
 $X ^ i + 1∣i = A 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}$ is
 $A K i = G i H i 1/2$
The function returns the product of the matrices $A$ and ${K}_{i}$, represented as ${AK}_{i}$, and the state covariance matrix ${P}_{i\mid i-1}$ factorized as ${P}_{i\mid i-1}={S}_{i}{S}_{i}^{\mathrm{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:     nIntegerInput
On entry: the actual state dimension, $n$, i.e., the order of the matrices ${S}_{i}$ and $A$.
Constraint: ${\mathbf{n}}\ge 1$.
2:     mIntegerInput
On entry: the actual input dimension, $m$, i.e., the order of the matrix ${Q}_{i}^{1/2}$.
Constraint: ${\mathbf{m}}\ge 1$.
3:     pIntegerInput
On entry: the actual output dimension, $p$, i.e., the order of the matrix ${R}_{i}^{1/2}$.
Constraint: ${\mathbf{p}}\ge 1$.
4:     s[${\mathbf{n}}×{\mathbf{tds}}$]doubleInput/Output
Note: the $\left(i,j\right)$th element of the matrix $S$ is stored in ${\mathbf{s}}\left[\left(i-1\right)×{\mathbf{tds}}+j-1\right]$.
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\mid 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\mid i}$.
5:     tdsIntegerInput
On entry: the stride separating matrix column elements in the array s.
Constraint: ${\mathbf{tds}}\ge {\mathbf{n}}$.
6:     a[${\mathbf{n}}×{\mathbf{tda}}$]const doubleInput
Note: the $\left(i,j\right)$th element of the matrix $A$ is stored in ${\mathbf{a}}\left[\left(i-1\right)×{\mathbf{tda}}+j-1\right]$.
On entry: the leading $n$ by $n$ part of this array must contain the lower observer Hessenberg matrix $UA{U}^{\mathrm{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:     tdaIntegerInput
On entry: the stride separating matrix column elements in the array a.
Constraint: ${\mathbf{tda}}\ge {\mathbf{n}}$.
8:     b[${\mathbf{n}}×{\mathbf{tdb}}$]const doubleInput
Note: the $\left(i,j\right)$th element of the matrix $B$ is stored in ${\mathbf{b}}\left[\left(i-1\right)×{\mathbf{tdb}}+j-1\right]$.
On entry: if q is not NULL then the leading $n$ by $m$ part of this array must contain the matrix $UB$, otherwise (if q is NULL then the leading $n$ by $m$ part of the array must contain the matrix ${UBQ}_{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:     tdbIntegerInput
On entry: the stride separating matrix column elements in the array b.
Constraint: ${\mathbf{tdb}}\ge {\mathbf{m}}$.
10:   q[${\mathbf{m}}×{\mathbf{tdq}}$]const doubleInput
Note: the $\left(i,j\right)$th element of the matrix $Q$ is stored in ${\mathbf{q}}\left[\left(i-1\right)×{\mathbf{tdq}}+j-1\right]$.
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:   tdqIntegerInput
On entry: the stride separating matrix column elements in the array q.
Constraint: ${\mathbf{tdq}}\ge {\mathbf{m}}$ if q is defined.
12:   c[${\mathbf{p}}×{\mathbf{tdc}}$]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: the leading $p$ by $n$ part of this array must contain the lower observer Hessenberg matrix $C{U}^{\mathrm{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:   tdcIntegerInput
On entry: the stride separating matrix column elements in the array c.
Constraint: ${\mathbf{tdc}}\ge {\mathbf{n}}$.
14:   r[${\mathbf{p}}×{\mathbf{tdr}}$]const doubleInput
Note: the $\left(i,j\right)$th element of the matrix $R$ is stored in ${\mathbf{r}}\left[\left(i-1\right)×{\mathbf{tdr}}+j-1\right]$.
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:   tdrIntegerInput
On entry: the stride separating matrix column elements in the array r.
Constraint: ${\mathbf{tdr}}\ge {\mathbf{p}}$.
16:   k[${\mathbf{n}}×{\mathbf{tdk}}$]doubleOutput
Note: the $\left(i,j\right)$th element of the matrix $K$ is stored in ${\mathbf{k}}\left[\left(i-1\right)×{\mathbf{tdk}}+j-1\right]$.
On exit: if k is not NULL then the leading $n$ by $p$ part of k contains ${AK}_{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:   tdkIntegerInput
On entry: the stride separating matrix column elements in the array k.
Constraint: ${\mathbf{tdk}}\ge {\mathbf{p}}$ if k is defined.
18:   h[${\mathbf{p}}×{\mathbf{tdh}}$]doubleOutput
Note: the $\left(i,j\right)$th element of the matrix $H$ is stored in ${\mathbf{h}}\left[\left(i-1\right)×{\mathbf{tdh}}+j-1\right]$.
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:   tdhIntegerInput
On entry: the stride separating matrix column elements in the array h.
Constraint: ${\mathbf{tdh}}\ge {\mathbf{p}}$ if k and h are defined.
20:   toldoubleInput
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}\epsilon$ then the tolerance is taken as ${p}^{2}\epsilon$, where $\epsilon$ is the machine precision. Otherwise, tol need not be set by you.
21:   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, ${\mathbf{tds}}=⟨\mathit{\text{value}}⟩$ while ${\mathbf{n}}=⟨\mathit{\text{value}}⟩$. These arguments must satisfy ${\mathbf{tds}}\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{tdb}}=⟨\mathit{\text{value}}⟩$ while ${\mathbf{m}}=⟨\mathit{\text{value}}⟩$. These arguments must satisfy ${\mathbf{tdb}}\ge {\mathbf{n}}$. 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{tdr}}=⟨\mathit{\text{value}}⟩$ while ${\mathbf{p}}=⟨\mathit{\text{value}}⟩$. These arguments must satisfy ${\mathbf{tdr}}\ge {\mathbf{p}}$. 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{tdk}}=⟨\mathit{\text{value}}⟩$ while ${\mathbf{p}}=⟨\mathit{\text{value}}⟩$. These arguments must satisfy ${\mathbf{tdk}}\ge {\mathbf{p}}$. On entry ${\mathbf{tdh}}=⟨\mathit{\text{value}}⟩$ while ${\mathbf{p}}=⟨\mathit{\text{value}}⟩$. These arguments must satisfy ${\mathbf{tdh}}\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 sqrt(H) is singular.
NE_NULL_ARRAY

## 7  Accuracy

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

## 8  Parallelism and Performance

Not applicable.

The algorithm requires $\frac{1}{6}{n}^{3}+{n}^{2}\left(\frac{3}{2}p+m\right)+2n{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 $\left(A,B,C\right)$ 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 $\left(A,B,C\right)$. 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.

### 10.1  Program Text

Program Text (g13ebce.c)

### 10.2  Program Data

Program Data (g13ebce.d)

### 10.3  Program Results

Program Results (g13ebce.r)