g13ecc (kalman_sqrt_filt_info_var) : NAG Library CL Interface, Mark 30

2 Specification

#include <nag.h>

void

g13ecc (Integer n, Integer m, Integer p, Nag_ab_input inp_ab, double t[], Integer tdt, const double ainv[], Integer tda, const double b[], Integer tdb, 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: g13ecc, nag_tsa_kalman_sqrt_filt_info_var or nag_kalman_sqrt_filt_info_var.

3 Description

For the state space system defined by

\begin{array}{l} X_{i + 1} & = & A_{i} X_{i} + B_{i} W_{i} & var (W_{i}) = Q_{i} \\ Y_{i} & = & C_{i} 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}

. 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}}_{i} \\ S_{i}^{−1} A_{i}^{−1} B_{i} & S_{i}^{−1} A_{i}^{−1} & S_{i}^{−1} {\hat{X}}_{i ∣ i} \\ 0 & R_{i + 1}^{- 1 / 2} C_{i + 1} & R_{i + 1}^{- 1 / 2} Y_{i + 1} \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. The triangularization is done entirely via Householder transformations exploiting the zero pattern of the pre-array. The term

{\bar{w}}_{i}

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 triangular).

The new state filtered state estimate is computed via

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

The function returns

S_{i + 1}^{−1}

and

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

(see the G13 Chapter Introduction 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

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

n

– Integer Input

On entry: the actual state dimension,

n

, i.e., the order of the matrices

S_{i}

and

A_{i}^{−1}

Constraint:

n \geq 1

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

p

– Integer Input

On entry: the actual output dimension,

p

, i.e., the order of the matrix

R_{i + 1}^{- 1 / 2}

Constraint:

p \geq 1

inp_ab

– Nag_ab_input Input

On entry: indicates how the matrix

B_{i}

is to be passed to the function.

$inp_ab = Nag_ab_prod$: Array b must contain the product $A_{i}^{−1} B_{i}$ .
$inp_ab = Nag_ab_sep$: Then array b must contain $B_{i}$ .

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}

tdt

– Integer Input

On entry: the stride separating matrix column elements in the array t.

Constraint:

tdt \geq n

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

A_{i}^{−1}

the inverse of the state transition matrix.

tda

– Integer Input

On entry: the stride separating matrix column elements in the array ainv.

Constraint:

tda \geq n

b [n \times tdb]

– const double Input

Note: the

(i, j)

th element of the matrix

B

is stored in

b [(i - 1) \times tdb + j - 1]

On entry: the leading

n \times m

part of this array must contain

B_{i}

(if

inp_ab = Nag_ab_sep

) or its product with

A_{i}^{−1}

(if

inp_ab = Nag_ab_prod

10:

tdb

– Integer Input

On entry: the stride separating matrix column elements in the array b.

Constraint:

tdb \geq m

11:

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 measurement 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_{i + 1}^{- 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 (see below) then the array rinv must be set to NULL.

12:

tdr

– Integer Input

On entry: the stride separating matrix column elements in the array rinv.

Constraint:

tdr \geq p

if rinv is defined.

13:

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 rinv has been set to NULL then the leading

p \times n

part of this array must contain

C_{i + 1}

, the output weight matrix (or its product with

R_{i + 1}^{- 1 / 2}

) of the discrete system at instant

i + 1

14:

tdc

– Integer Input

On entry: the stride separating matrix column elements in the array c.

Constraint:

tdc \geq n

15:

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.

16:

tdq

– Integer Input

On entry: the stride separating matrix column elements in the array qinv.

Constraint:

tdq \geq m

17:

x [n]

– double Input/Output

On entry: this array must contain the estimated state

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

On exit: the estimated state

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

18:

rinvy [p]

– const double Input

On entry: this array must contain

R_{i + 1}^{- 1 / 2} Y_{i + 1}

, the product of the upper triangular matrix

R_{i + 1}^{- 1 / 2}

and the measured output

Y_{i + 1}

19:

z [m]

– const double Input

On entry: this array must contain

{\bar{w}}_{i}

, the mean value of the state process noise.

20:

tol

– double Input

On entry: tol is used to test for near singularity of the matrix

S_{i + 1}^{−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.

21:

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,

tda = ⟨ value ⟩

while

n = ⟨ value ⟩