nag_kalman_sqrt_filt_info_var (g13ecc) performs a combined measurement and time update of one iteration of the time-varying Kalman filter. The method employed for this update is the square root information filter with the system matrices in their original form.
#include <nag.h> |
#include <nagg13.h> |
void |
nag_kalman_sqrt_filt_info_var (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) |
|
For the state space system defined by
the estimate of
given observations
to
is denoted by
with
. The function performs one recursion of the square root information filter algorithm, summarised as follows:
where
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
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 triangular).
The new state filtered state estimate is computed via
The function returns
and
(see the
g13 Chapter Introduction for more information concerning the information filter).
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
- 1:
n – IntegerInput
On entry: the actual state dimension, , i.e., the order of the matrices and .
Constraint:
.
- 2:
m – IntegerInput
On entry: the actual input dimension, , i.e., the order of the matrix .
Constraint:
.
- 3:
p – IntegerInput
On entry: the actual output dimension, , i.e., the order of the matrix .
Constraint:
.
- 4:
inp_ab – Nag_ab_inputInput
On entry: indicates how the matrix
is to be passed to the function.
- Array b must contain the product .
- Then array b must contain .
- 5:
t[] – 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 .
- 6:
tdt – IntegerInput
-
On entry: the stride separating matrix column elements in the array
t.
Constraint:
.
- 7:
ainv[] – const doubleInput
-
Note: the th element of the matrix is stored in .
On entry: the leading by part of this array must contain the inverse of the state transition matrix.
- 8:
tda – IntegerInput
-
On entry: the stride separating matrix column elements in the array
ainv.
Constraint:
.
- 9:
b[] – const doubleInput
-
Note: the th element of the matrix is stored in .
On entry: the leading by part of this array must contain (if ) or its product with (if ).
- 10:
tdb – IntegerInput
-
On entry: the stride separating matrix column elements in the array
b.
Constraint:
.
- 11:
rinv[] – const doubleInput
-
Note: the th element of the matrix is stored in .
On entry: if the measurement 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 (see below) then the array
rinv must be set to
NULL.
- 12:
tdr – IntegerInput
-
On entry: the stride separating matrix column elements in the array
rinv.
Constraint:
if
rinv is defined.
- 13:
c[] – const doubleInput
-
Note: the th element of the matrix is stored in .
On entry: if the array
rinv has been set to
NULL then the leading
by
part of this array must contain
, the output weight matrix (or its product with
) of the discrete system at instant
.
- 14:
tdc – IntegerInput
-
On entry: the stride separating matrix column elements in the array
c.
Constraint:
.
- 15:
qinv[] – 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.
- 16:
tdq – IntegerInput
-
On entry: the stride separating matrix column elements in the array
qinv.
Constraint:
.
- 17:
x[n] – doubleInput/Output
-
On entry: this array must contain the estimated state
On exit: the estimated state .
- 18:
rinvy[p] – const doubleInput
-
On entry: this array must contain , the product of the upper triangular matrix and the measured output .
- 19:
z[m] – const doubleInput
-
On entry: this array must contain , the mean value of the state process noise.
- 20:
tol – 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.
- 21:
fail – NagError *Input/Output
-
The NAG error argument (see
Section 3.6 in the Essential Introduction).
- 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_BAD_PARAM
-
On entry, argument
inp_ab had an illegal value.
- NE_INT_ARG_LT
-
On entry, .
Constraint: .
On entry, .
Constraint: .
On entry, .
Constraint: .
- NE_MAT_SINGULAR
-
The matrix inverse(S) is singular.
The use of the square root algorithm improves the stability of the computations.
Not applicable.
The algorithm requires approximately
operations and is backward stable (see
Verhaegen and van Dooren (1986)).
To apply three iterations of the Kalman filter (in square root information form) to the system . The same data is used for all three iterative steps.