NAG Library Routine Document
g13ejf (kalman_unscented_state_revcom)
1
Purpose
g13ejf applies the Unscented Kalman Filter to a nonlinear state space model, with additive noise.
g13ejf uses reverse communication for evaluating the nonlinear functionals of the state space model.
2
Specification
Fortran Interface
Subroutine g13ejf ( 
irevcm, mx, my, y, lx, ldlx, ly, ldly, x, st, ldst, n, xt, ldxt, fxt, ldfxt, ropt, lropt, icomm, licomm, rcomm, lrcomm, ifail) 
Integer, Intent (In)  ::  mx, my, ldlx, ldly, ldst, ldxt, ldfxt, lropt, licomm, lrcomm  Integer, Intent (Inout)  ::  irevcm, n, icomm(licomm), ifail  Real (Kind=nag_wp), Intent (In)  ::  y(my), lx(ldlx,*), ly(ldly,*), ropt(lropt)  Real (Kind=nag_wp), Intent (Inout)  ::  x(mx), st(ldst,*), xt(ldxt,*), fxt(ldfxt,*), rcomm(lrcomm) 

C Header Interface
#include nagmk26.h
void 
g13ejf_ (Integer *irevcm, const Integer *mx, const Integer *my, const double y[], const double lx[], const Integer *ldlx, const double ly[], const Integer *ldly, double x[], double st[], const Integer *ldst, Integer *n, double xt[], const Integer *ldxt, double fxt[], const Integer *ldfxt, const double ropt[], const Integer *lropt, Integer icomm[], const Integer *licomm, double rcomm[], const Integer *lrcomm, Integer *ifail) 

3
Description
g13ejf applies the Unscented Kalman Filter (UKF), as described in
Julier and Uhlmann (1997b) to a nonlinear state space model, with additive noise, which, at time
$t$, can be described by:
where
${x}_{t}$ represents the unobserved state vector of length
${m}_{x}$ and
${y}_{t}$ the observed measurement vector of length
${m}_{y}$. The process noise is denoted
${v}_{t}$, which is assumed to have mean zero and covariance structure
${\Sigma}_{x}$, and the measurement noise by
${u}_{t}$, which is assumed to have mean zero and covariance structure
${\Sigma}_{y}$.
3.1
Unscented Kalman Filter Algorithm
Given
${\hat{x}}_{0}$, an initial estimate of the state and
${P}_{0}$ and initial estimate of the state covariance matrix, the UKF can be described as follows:
(a) 
Generate a set of sigma points (see section Section 3.2):

(b) 
Evaluate the known model function $F$:
The function $F$ is assumed to accept the ${m}_{x}\times n$ matrix, ${\mathcal{X}}_{t}$ and return an ${m}_{x}\times n$ matrix, ${\mathcal{F}}_{t}$. The columns of both ${\mathcal{X}}_{t}$ and ${\mathcal{F}}_{t}$ correspond to different possible states. The notation ${\mathcal{F}}_{t,i}$ is used to denote the $i$th column of ${\mathcal{F}}_{t}$, hence the result of applying $F$ to the $i$th possible state. 
(c) 
Time Update:

(d) 
Redraw another set of sigma points (see section Section 3.2):

(e) 
Evaluate the known model function $H$:
The function $H$ is assumed to accept the ${m}_{x}\times n$ matrix, ${\mathcal{Y}}_{t}$ and return an ${m}_{y}\times n$ matrix, ${\mathcal{H}}_{t}$. The columns of both ${\mathcal{Y}}_{t}$ and ${\mathcal{H}}_{t}$ correspond to different possible states. As above ${\mathcal{H}}_{t,i}$ is used to denote the $i$th column of ${\mathcal{H}}_{t}$. 
(f) 
Measurement Update:

Here
${\mathcal{K}}_{t}$ is the Kalman gain matrix,
${\hat{x}}_{t}$ is the estimated state vector at time
$t$ and
${P}_{t}$ the corresponding covariance matrix. Rather than implementing the standard UKF as stated above
g13ejf uses the squareroot form described in the
Haykin (2001).
3.2
Sigma Points
A nonlinear state space model involves propagating a vector of random variables through a nonlinear system and we are interested in what happens to the mean and covariance matrix of those variables. Rather than trying to directly propagate the mean and covariance matrix, the UKF uses a set of carefully chosen sample points, referred to as sigma points, and propagates these through the system of interest. An estimate of the propagated mean and covariance matrix is then obtained via the weighted sample mean and covariance matrix.
For a vector of
$m$ random variables,
$x$, with mean
$\mu $ and covariance matrix
$\Sigma $, the sigma points are usually constructed as:
When calculating the weighted sample mean and covariance matrix two sets of weights are required, one used when calculating the weighted sample mean, denoted
${W}^{m}$ and one used when calculated the weighted sample covariance matrix, denoted
${W}^{c}$. The weights and multiplier,
$\gamma $, are constructed as follows:
where, usually
$L=m$ and
$\alpha ,\beta $ and
$\kappa $ are constants. The total number of sigma points,
$n$, is given by
$2L+1$. The constant
$\alpha $ is usually set to somewhere in the range
${10}^{4}\le \alpha \le 1$ and for a Gaussian distribution, the optimal values of
$\kappa $ and
$\beta $ are
$3L$ and
$2$ respectively.
Rather than redrawing another set of sigma points in
(d) of the UKF an alternative method can be used where the sigma points used in
(a) are augmented to take into account the process noise. This involves replacing equation
(5) with:
Augmenting the sigma points in this manner requires setting $L$ to $2L$ (and hence $n$ to $2n1$) and recalculating the weights. These new values are then used for the rest of the algorithm. The advantage of augmenting the sigma points is that it keeps any oddmoments information captured by the original propagated sigma points, at the cost of using a larger number of points.
4
References
Haykin S (2001) Kalman Filtering and Neural Networks John Wiley and Sons
Julier S J (2002) The scaled unscented transformation Proceedings of the 2002 American Control Conference (Volume 6) 4555–4559
Julier S J and Uhlmann J K (1997a) A consistent, debiased method for converting between polar and Cartesian coordinate systems Proceedings of AeroSense97, International Society for Optics and Phonotonics 110–121
Julier S J and Uhlmann J K (1997b) A new extension of the Kalman Filter to nonlinear systems International Symposium for Aerospace/Defense, Sensing, Simulation and Controls (Volume 3) 26
5
Arguments
Note: this routine uses
reverse communication. Its use involves an initial entry, intermediate exits and reentries, and a final exit, as indicated by the argument
irevcm. Between intermediate exits and reentries,
all arguments other than fxt must remain unchanged.
 1: $\mathbf{irevcm}$ – IntegerInput/Output

On initial entry: must be set to
$0$ or
$3$.
If ${\mathbf{irevcm}}=0$, it is assumed that $t=0$, otherwise it is assumed that $t\ne 0$ and that g13ejf has been called at least once before at an earlier time step.
On intermediate exit:
${\mathbf{irevcm}}=1$ or
$2$. The value of
irevcm specifies what intermediate values are returned by this routine and what values the calling program must assign to arguments of
g13ejf before reentering the routine. Details of the output and required input are given in the individual argument descriptions.
On intermediate reentry:
irevcm must remain unchanged.
On final exit: ${\mathbf{irevcm}}=3$
Constraint:
${\mathbf{irevcm}}=0$, $1$, $2$ or $3$.
Note: any values you return to g13ejf as part of the reverse communication procedure should not include floatingpoint NaN (Not a Number) or infinity values, since these are not handled by g13ejf. If your code inadvertently does return any NaNs or infinities, g13ejf is likely to produce unexpected results.
 2: $\mathbf{mx}$ – IntegerInput

On entry: ${m}_{x}$, the number of state variables.
Constraint:
${\mathbf{mx}}\ge 1$.
 3: $\mathbf{my}$ – IntegerInput

On entry: ${m}_{y}$, the number of observed variables.
Constraint:
${\mathbf{my}}\ge 1$.
 4: $\mathbf{y}\left({\mathbf{my}}\right)$ – Real (Kind=nag_wp) arrayInput

On entry: ${y}_{t}$, the observed data at the current time point.
 5: $\mathbf{lx}\left({\mathbf{ldlx}},*\right)$ – Real (Kind=nag_wp) arrayInput

Note: the second dimension of the array
lx
must be at least
${\mathbf{mx}}$.
On entry:
${L}_{x}$, such that
${L}_{x}{L}_{x}^{\mathrm{T}}={\Sigma}_{x}$, i.e., the lower triangular part of a Cholesky decomposition of the process noise covariance structure. Only the lower triangular part of
lx is referenced.
If
${\mathbf{ldlx}}=0$, there is no process noise (
${v}_{t}=0$ for all
$t$) and
lx is not referenced.
If ${\Sigma}_{x}$ is time dependent, the value supplied should be for time $t$.
 6: $\mathbf{ldlx}$ – IntegerInput

On entry: the first dimension of the array
lx as declared in the (sub)program from which
g13ejf is called.
Constraint:
${\mathbf{ldlx}}=0$ or ${\mathbf{ldlx}}\ge {\mathbf{mx}}$.
 7: $\mathbf{ly}\left({\mathbf{ldly}},*\right)$ – Real (Kind=nag_wp) arrayInput

Note: the second dimension of the array
ly
must be at least
${\mathbf{my}}$.
On entry:
${L}_{y}$, such that
${L}_{y}{L}_{y}^{\mathrm{T}}={\Sigma}_{y}$, i.e., the lower triangular part of a Cholesky decomposition of the observation noise covariance structure. Only the lower triangular part of
ly is referenced.
If ${\Sigma}_{y}$ is time dependent, the value supplied should be for time $t$.
 8: $\mathbf{ldly}$ – IntegerInput

On entry: the first dimension of the array
ly as declared in the (sub)program from which
g13ejf is called.
Constraint:
${\mathbf{ldly}}\ge {\mathbf{my}}$.
 9: $\mathbf{x}\left({\mathbf{mx}}\right)$ – Real (Kind=nag_wp) arrayInput/Output

On initial entry: ${\hat{x}}_{t1}$ the state vector for the previous time point.
On intermediate exit:
when
 ${\mathbf{irevcm}}=1$
 x is unchanged.
 ${\mathbf{irevcm}}=2$
 ${\hat{x}}_{t}^{\_}$.
On intermediate reentry:
x must remain unchanged.
On final exit: ${\hat{x}}_{t}$ the updated state vector.
 10: $\mathbf{st}\left({\mathbf{ldst}},*\right)$ – Real (Kind=nag_wp) arrayInput/Output

Note: the second dimension of the array
st
must be at least
${\mathbf{mx}}$.
On initial entry:
${S}_{t}$, such that
${S}_{t1}{S}_{t1}^{\mathrm{T}}={P}_{t1}$, i.e., the lower triangular part of a Cholesky decomposition of the state covariance matrix at the previous time point. Only the lower triangular part of
st is referenced.
On intermediate exit:
when
 ${\mathbf{irevcm}}=1$
 st is unchanged.
 ${\mathbf{irevcm}}=2$
 ${S}_{t}^{\_}$, the lower triangular part of a Cholesky factorization of ${P}_{t}^{\_}$.
On intermediate reentry:
st must remain unchanged.
On final exit: ${S}_{t}$, the lower triangular part of a Cholesky factorization of the updated state covariance matrix.
 11: $\mathbf{ldst}$ – IntegerInput

On entry: the first dimension of the array
st as declared in the (sub)program from which
g13ejf is called.
Constraint:
${\mathbf{ldst}}\ge {\mathbf{mx}}$.
 12: $\mathbf{n}$ – IntegerInput/Output

On initial entry: the value used in the sizing of the
fxt and
xt arrays. The value of
n supplied must be at least as big as the maximum number of sigma points that the algorithm will use.
g13ejf allows sigma points to be calculated in two ways during the measurement update; they can be redrawn or augmented. Which is used is controlled by
ropt.
If redrawn sigma points are used, then the maximum number of sigma points will be $2{m}_{x}+1$, otherwise the maximum number of sigma points will be $4{m}_{x}+1$.
On intermediate exit:
the number of sigma points actually being used.
On intermediate reentry:
n must remain unchanged.
On final exit: reset to its value on initial entry.
Constraints:
if
${\mathbf{irevcm}}=0$ or
$3$,
 if redrawn sigma points are used, ${\mathbf{n}}\ge 2\times {\mathbf{mx}}+1$;
 otherwise ${\mathbf{n}}\ge 4\times {\mathbf{mx}}+1$.
 13: $\mathbf{xt}\left({\mathbf{ldxt}},*\right)$ – Real (Kind=nag_wp) arrayInput/Output

Note: the second dimension of the array
xt
must be at least
$\mathrm{max}\phantom{\rule{0.125em}{0ex}}\left({\mathbf{my}},{\mathbf{n}}\right)$.
On initial entry: need not be set.
On intermediate exit:
${X}_{t}$ when
${\mathbf{irevcm}}=1$, otherwise
${Y}_{t}$.
For the $j$th sigma point, the value for the $i$th parameter is held in
${\mathbf{xt}}\left(\mathit{i},\mathit{j}\right)$, for $\mathit{i}=1,2,\dots ,{\mathbf{mx}}$ and $\mathit{j}=1,2,\dots ,{\mathbf{n}}$.
On intermediate reentry:
xt must remain unchanged.
On final exit: the contents of
xt are undefined.
 14: $\mathbf{ldxt}$ – IntegerInput

On entry: the first dimension of the array
xt as declared in the (sub)program from which
g13ejf is called.
Constraint:
${\mathbf{ldxt}}\ge {\mathbf{mx}}$.
 15: $\mathbf{fxt}\left({\mathbf{ldfxt}},*\right)$ – Real (Kind=nag_wp) arrayInput/Output

Note: the second dimension of the array
fxt
must be at least
${\mathbf{n}}+\mathrm{max}\phantom{\rule{0.125em}{0ex}}\left({\mathbf{mx}},{\mathbf{my}}\right)$.
On initial entry: need not be set.
On intermediate exit:
the contents of
fxt are undefined.
On intermediate reentry:
$F\left({X}_{t}\right)$ when
${\mathbf{irevcm}}=1$, otherwise
$H\left({Y}_{t}\right)$ for the values of
${X}_{t}$ and
${Y}_{t}$ held in
xt.
For the $j$th sigma point the value for the $i$th parameter should be held in
${\mathbf{fxt}}\left(i,\mathit{j}\right)$, for $\mathit{j}=1,2,\dots ,{\mathbf{n}}$. When ${\mathbf{irevcm}}=1$, $i=1,2,\dots ,{\mathbf{mx}}$ and when ${\mathbf{irevcm}}=2$, $i=1,2,\dots ,{\mathbf{my}}$.
On final exit: the contents of
fxt are undefined.
 16: $\mathbf{ldfxt}$ – IntegerInput

On entry: the first dimension of the array
fxt as declared in the (sub)program from which
g13ejf is called.
Constraint:
${\mathbf{ldfxt}}\ge \mathrm{max}\phantom{\rule{0.125em}{0ex}}\left({\mathbf{mx}},{\mathbf{my}}\right)$.
 17: $\mathbf{ropt}\left({\mathbf{lropt}}\right)$ – Real (Kind=nag_wp) arrayInput

On entry: optional arguments. The default value will be used for
${\mathbf{ropt}}\left(i\right)$ if
${\mathbf{lropt}}<i$. Setting
${\mathbf{lropt}}=0$ will use the default values for all optional arguments and
ropt need not be set.
 ${\mathbf{ropt}}\left(1\right)$
 If set to $1$ then the second set of sigma points are redrawn, as given by equation (5). If set to $2$ then the second set of sigma points are generated via augmentation, as given by equation (13).
Default is for the sigma points to be redrawn (i.e., ${\mathbf{ropt}}\left(1\right)=1$)
 ${\mathbf{ropt}}\left(2\right)$
 ${\kappa}_{x}$, value of $\kappa $ used when constructing the first set of sigma points, ${\mathcal{X}}_{t}$.
Defaults to $3{\mathbf{mx}}$.
 ${\mathbf{ropt}}\left(3\right)$
 ${\alpha}_{x}$, value of $\alpha $ used when constructing the first set of sigma points, ${\mathcal{X}}_{t}$.
Defaults to $1$.
 ${\mathbf{ropt}}\left(4\right)$
 ${\beta}_{x}$, value of $\beta $ used when constructing the first set of sigma points, ${\mathcal{X}}_{t}$.
Defaults to $2$.
 ${\mathbf{ropt}}\left(5\right)$
 Value of $\kappa $ used when constructing the second set of sigma points, ${\mathcal{Y}}_{t}$.
Defaults to $32\times {\mathbf{mx}}$ when ${\mathbf{ldlx}}\ne 0$ and the second set of sigma points are augmented and ${\kappa}_{x}$ otherwise.
 ${\mathbf{ropt}}\left(6\right)$
 Value of $\alpha $ used when constructing the second set of sigma points, ${\mathcal{Y}}_{t}$.
Defaults to ${\alpha}_{x}$.
 ${\mathbf{ropt}}\left(7\right)$
 Value of $\beta $ used when constructing the second set of sigma points, ${\mathcal{Y}}_{t}$.
Defaults to ${\beta}_{x}$.
Constraints:
 ${\mathbf{ropt}}\left(1\right)=1$ or $2$;
 ${\mathbf{ropt}}\left(2\right)>{\mathbf{mx}}$;
 ${\mathbf{ropt}}\left(5\right)>2\times {\mathbf{mx}}$ when ${\mathbf{ldly}}\ne 0$ and the second set of sigma points are augmented, otherwise${\mathbf{ropt}}\left(5\right)>{\mathbf{mx}}$;
 ${\mathbf{ropt}}\left(\mathit{i}\right)>0$, for $\mathit{i}=3,4,5,6$.
 18: $\mathbf{lropt}$ – IntegerInput

On entry: length of the options array
ropt.
Constraint:
$0\le {\mathbf{lropt}}\le 7$.
 19: $\mathbf{icomm}\left({\mathbf{licomm}}\right)$ – Integer arrayCommunication Array

On initial entry:
icomm need not be set.
On intermediate exit:
icomm is used for storage between calls to
g13ejf.
On intermediate reentry:
icomm must remain unchanged.
On final exit:
icomm is not defined.
 20: $\mathbf{licomm}$ – IntegerInput

On entry: the length of the array
icomm. If
licomm is too small and
${\mathbf{licomm}}\ge 2$ then
${\mathbf{ifail}}={\mathbf{201}}$ is returned and the minimum value for
licomm and
lrcomm are given by
${\mathbf{icomm}}\left(1\right)$ and
${\mathbf{icomm}}\left(2\right)$ respectively.
Constraint:
${\mathbf{licomm}}\ge 30$.
 21: $\mathbf{rcomm}\left({\mathbf{lrcomm}}\right)$ – Real (Kind=nag_wp) arrayCommunication Array

On initial entry:
rcomm need not be set.
On intermediate exit:
rcomm is used for storage between calls to
g13ejf.
On intermediate reentry:
rcomm must remain unchanged.
On final exit:
rcomm is not defined.
 22: $\mathbf{lrcomm}$ – IntegerInput

On entry: the length of the array
rcomm. If
lrcomm is too small and
${\mathbf{licomm}}\ge 2$ then
${\mathbf{ifail}}={\mathbf{202}}$ is returned and the minimum value for
licomm and
lrcomm are given by
${\mathbf{icomm}}\left(1\right)$ and
${\mathbf{icomm}}\left(2\right)$ respectively.
Suggested value:
${\mathbf{lrcomm}}=30+{\mathbf{my}}+{\mathbf{mx}}\times {\mathbf{my}}+\left(1+\mathit{nb}\right)\times \mathrm{max}\phantom{\rule{0.125em}{0ex}}\left({\mathbf{mx}},{\mathbf{my}}\right)$, where $\mathit{nb}$ is the optimal block size. In most cases a block size of $128$ will be sufficient.
Constraint:
${\mathbf{lrcomm}}\ge 30+{\mathbf{my}}+{\mathbf{mx}}\times {\mathbf{my}}+2\times \mathrm{max}\phantom{\rule{0.125em}{0ex}}\left({\mathbf{mx}},{\mathbf{my}}\right)$.
 23: $\mathbf{ifail}$ – IntegerInput/Output

On initial entry:
ifail must be set to
$0$,
$1\text{ or}1$. If you are unfamiliar with this argument you should refer to
Section 3.4 in How to Use the NAG Library and its Documentation for details.
For environments where it might be inappropriate to halt program execution when an error is detected, the value
$1\text{ or}1$ is recommended. If the output of error messages is undesirable, then the value
$1$ is recommended. Otherwise, because for this routine the values of the output arguments may be useful even if
${\mathbf{ifail}}\ne {\mathbf{0}}$ on exit, the recommended value is
$1$.
When the value $\mathbf{1}\text{ or}1$ is used it is essential to test the value of ifail on exit.
On final exit:
${\mathbf{ifail}}={\mathbf{0}}$ unless the routine detects an error or a warning has been flagged (see
Section 6).
6
Error Indicators and Warnings
If on entry
${\mathbf{ifail}}=0$ or
$1$, explanatory error messages are output on the current error message unit (as defined by
x04aaf).
Errors or warnings detected by the routine:
 ${\mathbf{ifail}}=11$

On entry, ${\mathbf{irevcm}}=\u2329\mathit{\text{value}}\u232a$.
Constraint: ${\mathbf{irevcm}}=0$, $1$, $2$ or $3$.
 ${\mathbf{ifail}}=21$

On entry, ${\mathbf{mx}}=\u2329\mathit{\text{value}}\u232a$.
Constraint: ${\mathbf{mx}}\ge 1$.
 ${\mathbf{ifail}}=22$

mx has changed between calls.
On intermediate entry,
${\mathbf{mx}}=\u2329\mathit{\text{value}}\u232a$.
On initial entry,
${\mathbf{mx}}=\u2329\mathit{\text{value}}\u232a$.
 ${\mathbf{ifail}}=31$

On entry, ${\mathbf{my}}=\u2329\mathit{\text{value}}\u232a$.
Constraint: ${\mathbf{my}}\ge 1$.
 ${\mathbf{ifail}}=32$

my has changed between calls.
On intermediate entry,
${\mathbf{my}}=\u2329\mathit{\text{value}}\u232a$.
On initial entry,
${\mathbf{my}}=\u2329\mathit{\text{value}}\u232a$.
 ${\mathbf{ifail}}=61$

On entry, ${\mathbf{ldlx}}=\u2329\mathit{\text{value}}\u232a$ and ${\mathbf{mx}}=\u2329\mathit{\text{value}}\u232a$.
Constraint: ${\mathbf{ldlx}}=0$ or ${\mathbf{ldlx}}\ge {\mathbf{mx}}$.
 ${\mathbf{ifail}}=81$

On entry, ${\mathbf{ldly}}=\u2329\mathit{\text{value}}\u232a$ and ${\mathbf{my}}=\u2329\mathit{\text{value}}\u232a$.
Constraint: ${\mathbf{ldly}}\ge {\mathbf{my}}$.
 ${\mathbf{ifail}}=111$

On entry, ${\mathbf{ldst}}=\u2329\mathit{\text{value}}\u232a$ and ${\mathbf{mx}}=\u2329\mathit{\text{value}}\u232a$.
Constraint: ${\mathbf{ldst}}\ge {\mathbf{mx}}$.
 ${\mathbf{ifail}}=121$

On entry, augmented sigma points requested, ${\mathbf{n}}=\u2329\mathit{\text{value}}\u232a$ and ${\mathbf{mx}}=\u2329\mathit{\text{value}}\u232a$.
Constraint: ${\mathbf{n}}\ge \u2329\mathit{\text{value}}\u232a$.
 ${\mathbf{ifail}}=122$

On entry, redrawn sigma points requested, ${\mathbf{n}}=\u2329\mathit{\text{value}}\u232a$ and ${\mathbf{mx}}=\u2329\mathit{\text{value}}\u232a$.
Constraint: ${\mathbf{n}}\ge \u2329\mathit{\text{value}}\u232a$.
 ${\mathbf{ifail}}=123$

n has changed between calls.
On intermediate entry,
${\mathbf{n}}=\u2329\mathit{\text{value}}\u232a$.
On intermediate exit,
${\mathbf{n}}=\u2329\mathit{\text{value}}\u232a$.
 ${\mathbf{ifail}}=141$

On entry, ${\mathbf{ldxt}}=\u2329\mathit{\text{value}}\u232a$ and ${\mathbf{mx}}=\u2329\mathit{\text{value}}\u232a$.
Constraint: ${\mathbf{ldxt}}\ge {\mathbf{mx}}$.
 ${\mathbf{ifail}}=161$

On entry, ${\mathbf{ldfxt}}=\u2329\mathit{\text{value}}\u232a$ and ${\mathbf{mx}}=\u2329\mathit{\text{value}}\u232a$.
Constraint: if ${\mathbf{irevcm}}=1$, ${\mathbf{ldfxt}}\ge {\mathbf{mx}}$.
 ${\mathbf{ifail}}=162$

On entry, ${\mathbf{ldfxt}}=\u2329\mathit{\text{value}}\u232a$ and ${\mathbf{my}}=\u2329\mathit{\text{value}}\u232a$.
Constraint: if ${\mathbf{irevcm}}=2$, ${\mathbf{ldfxt}}\ge {\mathbf{my}}$.
 ${\mathbf{ifail}}=171$

On entry, ${\mathbf{ropt}}\left(1\right)=\u2329\mathit{\text{value}}\u232a$.
Constraint: ${\mathbf{ropt}}\left(1\right)=1$ or $2$.
 ${\mathbf{ifail}}=172$

On entry, ${\mathbf{ropt}}\left(\u2329\mathit{\text{value}}\u232a\right)=\u2329\mathit{\text{value}}\u232a$.
Constraint: $\kappa >\u2329\mathit{\text{value}}\u232a$.
 ${\mathbf{ifail}}=173$

On entry, ${\mathbf{ropt}}\left(\u2329\mathit{\text{value}}\u232a\right)=\u2329\mathit{\text{value}}\u232a$.
Constraint: $\alpha >0$.
 ${\mathbf{ifail}}=181$

On entry, ${\mathbf{lropt}}=\u2329\mathit{\text{value}}\u232a$.
Constraint: $0\le {\mathbf{lropt}}\le 7$.
 ${\mathbf{ifail}}=191$

icomm has been corrupted between calls.
 ${\mathbf{ifail}}=201$

On entry,
${\mathbf{licomm}}=\u2329\mathit{\text{value}}\u232a$.
Constraint:
${\mathbf{licomm}}\ge 2$.
icomm is too small to return the required array sizes.
 ${\mathbf{ifail}}=202$

On entry,
${\mathbf{licomm}}=\u2329\mathit{\text{value}}\u232a$ and
${\mathbf{lrcomm}}=\u2329\mathit{\text{value}}\u232a$.
Constraint:
${\mathbf{licomm}}\ge 30$ and
${\mathbf{lrcomm}}\ge 30+{\mathbf{my}}+{\mathbf{mx}}\times {\mathbf{my}}+2\times \mathrm{max}\phantom{\rule{0.125em}{0ex}}\left({\mathbf{mx}},{\mathbf{my}}\right)$.
The minimum required values for
licomm and
lrcomm are returned in
${\mathbf{icomm}}\left(1\right)$ and
${\mathbf{icomm}}\left(2\right)$ respectively.
 ${\mathbf{ifail}}=211$

rcomm has been corrupted between calls.
 ${\mathbf{ifail}}=301$

A weight was negative and it was not possible to downdate the Cholesky factorization.
 ${\mathbf{ifail}}=302$

Unable to calculate the Kalman gain matrix.
 ${\mathbf{ifail}}=303$

Unable to calculate the Cholesky factorization of the updated state covariance matrix.
 ${\mathbf{ifail}}=99$
An unexpected error has been triggered by this routine. Please
contact
NAG.
See
Section 3.9 in How to Use the NAG Library and its Documentation for further information.
 ${\mathbf{ifail}}=399$
Your licence key may have expired or may not have been installed correctly.
See
Section 3.8 in How to Use the NAG Library and its Documentation for further information.
 ${\mathbf{ifail}}=999$
Dynamic memory allocation failed.
See
Section 3.7 in How to Use the NAG Library and its Documentation for further information.
7
Accuracy
Not applicable.
8
Parallelism and Performance
g13ejf makes calls to BLAS and/or LAPACK routines, which may be threaded within the vendor library used by this implementation. Consult the documentation for the vendor library for further information.
Please consult the
X06 Chapter Introduction for information on how to control and interrogate the OpenMP environment used within this routine. Please also consult the
Users' Note for your implementation for any additional implementationspecific information.
As well as implementing the Unscented Kalman Filter,
g13ejf can also be used to apply the Unscented Transform (see
Julier (2002)) to the function
$F$, by setting
${\mathbf{ldlx}}=0$ and terminating the calling sequence when
${\mathbf{irevcm}}=2$ rather than
${\mathbf{irevcm}}=3$. In this situation, on initial entry,
x and
st would hold the mean and Cholesky factorization of the covariance matrix of the untransformed sample and on exit (when
${\mathbf{irevcm}}=2$) they would hold the mean and Cholesky factorization of the covariance matrix of the transformed sample.
10
Example
This example implements the following nonlinear state space model, with the state vector
$x$ and state update function
$F$ given by:
where
$r$ and
$d$ are known constants and
${\varphi}_{Rt}$ and
${\varphi}_{Lt}$ are timedependent knowns. The measurement vector
$y$ and measurement function
$H$ is given by:
where
$A$ and
$\Delta $ are known constants. The initial values,
${x}_{0}$ and
${P}_{0}$, are given by
and the Cholesky factorizations of the error covariance matrices,
${L}_{x}$ and
${L}_{x}$ by
10.1
Program Text
Program Text (g13ejfe.f90)
10.2
Program Data
Program Data (g13ejfe.d)
10.3
Program Results
Program Results (g13ejfe.r)
The example described above can be thought of as relating to the movement of a hypothetical robot. The unknown state, $x$, is the position of the robot (with respect to a reference frame) and facing, with $\left(\xi ,\eta \right)$ giving the $x$ and $y$ coordinates and $\theta $ the angle (with respect to the $x$axis) that the robot is facing. The robot has two drive wheels, of radius $r$ on an axle of length $d$. During time period $t$ the right wheel is believed to rotate at a velocity of ${\varphi}_{Rt}$ and the left at a velocity of ${\varphi}_{Lt}$. In this example, these velocities are fixed with ${\varphi}_{Rt}=0.4$ and ${\varphi}_{Lt}=0.1$. The state update function, $F$, calculates where the robot should be at each time point, given its previous position. However, in reality, there is some random fluctuation in the velocity of the wheels, for example, due to slippage. Therefore the actual position of the robot and the position given by equation $F$ will differ.
In the area that the robot is moving there is a single wall. The position of the wall is known and defined by its distance, $\Delta $, from the origin and its angle, $A$, from the $x$axis. The robot has a sensor that is able to measure $y$, with $\delta $ being the distance to the wall and $\alpha $ the angle to the wall. The measurement function $H$ gives the expected distance and angle to the wall if the robot's position is given by ${x}_{t}$. Therefore the state space model allows the robot to incorporate the sensor information to update the estimate of its position.