NAG Library Function Document
nag_kalman_sqrt_filt_info_invar (g13edc)
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
the estimate of
given observations
to
is denoted by
with
(where
,
and
are time invariant). The function performs one recursion of the square root information filter algorithm, summarised as follows:
where
is an orthogonal transformation triangularizing the pre-array, and the matrix pair
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
,
and
):
The term
is the mean process noise, and
is the estimated error at instant
. The inverse of the state covariance matrix
is factored as follows
where
(
is lower). The new state filtered state estimate is computed via
The function returns
and, optionally,
(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:
– IntegerInput
-
On entry: the actual state dimension, , i.e., the order of the matrices and .
Constraint:
.
- 2:
– IntegerInput
-
On entry: the actual input dimension, , i.e., the order of the matrix .
Constraint:
.
- 3:
– IntegerInput
-
On entry: the actual output dimension, , i.e., the order of the matrix .
Constraint:
.
- 4:
– doubleInput/Output
-
Note: the th element of the matrix is stored in .
On entry: the leading by upper triangular part of this array must contain the square root of the inverse of the state covariance matrix .
On exit: the leading by upper triangular part of this array contains , the square root of the inverse of the of the state covariance matrix .
- 5:
– IntegerInput
-
On entry: the stride separating matrix column elements in the array
t.
Constraint:
.
- 6:
– const doubleInput
-
Note: the th element of the matrix is stored in .
On entry: the leading
by
part of this array must contain the upper controller Hessenberg matrix
. Where
is the inverse of the state transition matrix, and
is the unitary matrix generated by the function
nag_trans_hessenberg_controller (g13exc).
- 7:
– IntegerInput
-
On entry: the stride separating matrix column elements in the array
ainv.
Constraint:
.
- 8:
– const doubleInput
-
Note: the th element of the matrix is stored in .
On entry: the leading
by
part of this array must contain the upper controller Hessenberg matrix
. Where
is the inverse of the transition matrix,
is the input weight matrix
, and
is the unitary transformation generated by the function
nag_trans_hessenberg_controller (g13exc).
- 9:
– IntegerInput
-
On entry: the stride separating matrix column elements in the array
ainvb.
Constraint:
.
- 10:
– const doubleInput
-
Note: the th element of the matrix is stored in .
On entry: if the noise covariance matrix is to be supplied separately from the output weight matrix, then the leading
by
upper triangular part of this array must contain
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:
– IntegerInput
-
On entry: the stride separating matrix column elements in the array
rinv.
Constraint:
if
rinv is defined.
- 12:
– const doubleInput
-
Note: the th element of the matrix is stored in .
On entry: if the array argument
rinv (above) has been defined then the leading
by
part of this array must contain the matrix
, otherwise (if
rinv is
NULL then the leading
by
part of the array must contain the matrix
.
is the output weight matrix,
is the noise covariance matrix and
is the same unitary transformation used for defining array arguments
ainv and
ainvb.
- 13:
– IntegerInput
-
On entry: the stride separating matrix column elements in the array
c.
Constraint:
.
- 14:
– const doubleInput
-
Note: the th element of the matrix is stored in .
On entry: the leading by upper triangular part of this array must contain the right Cholesky factor of the inverse of the process noise covariance matrix.
- 15:
– IntegerInput
-
On entry: the stride separating matrix column elements in the array
qinv.
Constraint:
.
- 16:
– doubleInput/Output
-
On entry: this array must contain the estimated state
On exit: this array contains the estimated state .
- 17:
– const doubleInput
-
On entry: this array must contain , the product of the upper triangular matrix and the measured output vector .
- 18:
– const doubleInput
-
On entry: this array must contain , the mean value of the state process noise.
- 19:
– doubleInput
-
On entry:
tol is used to test for near singularity of the matrix
. If you set
tol to be less than
then the tolerance is taken as
, where
is the
machine precision.
- 20:
– NagError *Input/Output
-
The NAG error argument (see
Section 3.7 in How to Use the NAG Library and its Documentation).
6
Error Indicators and Warnings
- NE_2_INT_ARG_LT
-
On entry, while . These arguments must satisfy .
On entry while . These arguments must satisfy .
On entry while . These arguments must satisfy .
On entry while . These arguments must satisfy .
On entry while . These arguments must satisfy .
On entry while . These arguments must satisfy .
- NE_ALLOC_FAIL
-
Dynamic memory allocation failed.
- NE_INT_ARG_LT
-
On entry, .
Constraint: .
On entry, .
Constraint: .
On entry, .
Constraint: .
- 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
nag_kalman_sqrt_filt_info_invar (g13edc) is not threaded in any implementation.
The algorithm requires approximately
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 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
. 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.1
Program Text
Program Text (g13edce.c)
10.2
Program Data
Program Data (g13edce.d)
10.3
Program Results
Program Results (g13edce.r)