# NAG Library Routine Document

## 1Purpose

g13ekf applies the Unscented Kalman Filter (UKF) to a nonlinear state space model, with additive noise.
g13ekf uses direct communication for evaluating the nonlinear functionals of the state space model.

## 2Specification

Fortran Interface
 Subroutine g13ekf ( mx, my, y, lx, ly, f, h, x, st,
 Integer, Intent (In) :: mx, my Integer, Intent (Inout) :: iuser(*), ifail Real (Kind=nag_wp), Intent (In) :: y(my), lx(mx,mx), ly(my,my) Real (Kind=nag_wp), Intent (Inout) :: x(mx), st(mx,mx), ruser(*) External :: f, h
#include nagmk26.h
 void g13ekf_ (const Integer *mx, const Integer *my, const double y[], const double lx[], const double ly[], void (NAG_CALL *f)(const Integer *mx, const Integer *n, const double xt[], double fxt[], Integer iuser[], double ruser[], Integer *info),void (NAG_CALL *h)(const Integer *mx, const Integer *my, const Integer *n, const double yt[], double hyt[], Integer iuser[], double ruser[], Integer *info),double x[], double st[], Integer iuser[], double ruser[], Integer *ifail)

## 3Description

g13ekf 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:
 $xt+1 =Fxt+vt yt =Hxt+ut$
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.1Unscented Kalman Filter Algorithm

Given ${\stackrel{^}{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 3.2):
 $Xt$ (1)
(b) Evaluate the known model function $F$:
 $Ft$ $=FXt$ (2)
The function $F$ is assumed to accept the ${m}_{x}×n$ matrix, ${\mathcal{X}}_{t}$ and return an ${m}_{x}×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:
 $x^t_$ $= ∑ i=1 n Wim ⁢ Ft,i$ (3) $Pt_$ $= ∑ i=1 n Wic ⁢ Ft,i - x ^ t _ ⁢ Ft,i - x ^ t _ T + Σx$ (4)
(d) Redraw another set of sigma points (see Section 3.2):
 $Yt$ (5)
(e) Evaluate the known model function $H$:
 $Ht$ $=HYt$ (6)
The function $H$ is assumed to accept the ${m}_{x}×n$ matrix, ${\mathcal{Y}}_{t}$ and return an ${m}_{y}×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:
 $y ^ t$ $= ∑ i=1 n Wim ⁢ Ht,i$ (7) $Pyyt$ $= ∑ i=1 n Wic ⁢ Ht,i - y ^ t ⁢ Ht,i - y ^ t T + Σy$ (8) $P xyt$ $= ∑ i=1 n Wic ⁢ Ft,i - x ^ t _ ⁢ Ht,i - y ^ t T$ (9) $Kt$ $= P xyt Pyyt-1$ (10) $x^t$ $= x ^ t _ + Kt ⁢ yt - y ^ t$ (11) $Pt$ $= Pt_ - Kt ⁢ Pyyt KtT$ (12)
Here ${\mathcal{K}}_{t}$ is the Kalman gain matrix, ${\stackrel{^}{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 g13ekf uses the square-root form described in the Haykin (2001).

### 3.2Sigma 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:
 $λ =α2⁢L+κ-L γ =L+λ Wim = λL+λ i=1 12⁢L+λ i=2,3,…,2⁢L+1 Wic = λL+λ +1-α2+β i=1 12⁢L+λ i=2,3,…,2⁢L+1$
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 $3-L$ and $2$ respectively.
The constants, $\kappa$, $\alpha$ and $\beta$ are given by $\kappa =3-{m}_{x}$, $\alpha =1.0$ and $\beta =2$. If more control is required over the construction of the sigma points then the reverse communication routine, g13ejf, can be used instead.
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

## 5Arguments

1:     $\mathbf{mx}$ – IntegerInput
On entry: ${m}_{x}$, the number of state variables.
Constraint: ${\mathbf{mx}}\ge 1$.
2:     $\mathbf{my}$ – IntegerInput
On entry: ${m}_{y}$, the number of observed variables.
Constraint: ${\mathbf{my}}\ge 1$.
3:     $\mathbf{y}\left({\mathbf{my}}\right)$ – Real (Kind=nag_wp) arrayInput
On entry: ${y}_{t}$, the observed data at the current time point.
4:     $\mathbf{lx}\left({\mathbf{mx}},{\mathbf{mx}}\right)$ – Real (Kind=nag_wp) arrayInput
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 ${\Sigma }_{x}$ is time dependent, the value supplied should be for time $t$.
5:     $\mathbf{ly}\left({\mathbf{my}},{\mathbf{my}}\right)$ – Real (Kind=nag_wp) arrayInput
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$.
6:     $\mathbf{f}$ – Subroutine, supplied by the user.External Procedure
The state function, $F$ as described in (b).
The specification of f is:
Fortran Interface
 Subroutine f ( mx, n, xt, fxt, info)
 Integer, Intent (In) :: mx, n Integer, Intent (Inout) :: iuser(*), info Real (Kind=nag_wp), Intent (In) :: xt(mx,n) Real (Kind=nag_wp), Intent (Inout) :: ruser(*) Real (Kind=nag_wp), Intent (Out) :: fxt(mx,n)
#include nagmk26.h
 void f (const Integer *mx, const Integer *n, const double xt[], double fxt[], Integer iuser[], double ruser[], Integer *info)
1:     $\mathbf{mx}$ – IntegerInput
On entry: ${m}_{x}$, the number of state variables.
2:     $\mathbf{n}$ – IntegerInput
On entry: $n$, the number of sigma points.
3:     $\mathbf{xt}\left({\mathbf{mx}},{\mathbf{n}}\right)$ – Real (Kind=nag_wp) arrayInput
On entry: ${X}_{t}$, the sigma points generated in (a). 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 ,{m}_{x}$ and $\mathit{j}=1,2,\dots ,n$.
4:     $\mathbf{fxt}\left({\mathbf{mx}},{\mathbf{n}}\right)$ – Real (Kind=nag_wp) arrayOutput
On exit: $F\left({X}_{t}\right)$.
For the $j$th sigma point the value for the $i$th parameter should be held in ${\mathbf{fxt}}\left(\mathit{i},\mathit{j}\right)$, for $\mathit{i}=1,2,\dots ,{m}_{x}$ and $\mathit{j}=1,2,\dots ,n$.
5:     $\mathbf{iuser}\left(*\right)$ – Integer arrayUser Workspace
6:     $\mathbf{ruser}\left(*\right)$ – Real (Kind=nag_wp) arrayUser Workspace
f is called with the arguments iuser and ruser as supplied to g13ekf. You should use the arrays iuser and ruser to supply information to f.
7:     $\mathbf{info}$ – IntegerInput/Output
On entry: ${\mathbf{info}}=0$.
On exit: set info to a nonzero value if you wish g13ekf to terminate with ${\mathbf{ifail}}={\mathbf{61}}$.
f must either be a module subprogram USEd by, or declared as EXTERNAL in, the (sub)program from which g13ekf is called. Arguments denoted as Input must not be changed by this procedure.
Note: f should not return floating-point NaN (Not a Number) or infinity values, since these are not handled by g13ekf. If your code inadvertently does return any NaNs or infinities, g13ekf is likely to produce unexpected results.
7:     $\mathbf{h}$ – Subroutine, supplied by the user.External Procedure
The measurement function, $H$ as described in (e).
The specification of h is:
Fortran Interface
 Subroutine h ( mx, my, n, yt, hyt, info)
 Integer, Intent (In) :: mx, my, n Integer, Intent (Inout) :: iuser(*), info Real (Kind=nag_wp), Intent (In) :: yt(mx,n) Real (Kind=nag_wp), Intent (Inout) :: ruser(*) Real (Kind=nag_wp), Intent (Out) :: hyt(my,n)
#include nagmk26.h
 void h (const Integer *mx, const Integer *my, const Integer *n, const double yt[], double hyt[], Integer iuser[], double ruser[], Integer *info)
1:     $\mathbf{mx}$ – IntegerInput
On entry: ${m}_{x}$, the number of state variables.
2:     $\mathbf{my}$ – IntegerInput
On entry: ${m}_{y}$, the number of observed variables.
3:     $\mathbf{n}$ – IntegerInput
On entry: $n$, the number of sigma points.
4:     $\mathbf{yt}\left({\mathbf{mx}},{\mathbf{n}}\right)$ – Real (Kind=nag_wp) arrayInput
On entry: ${Y}_{t}$, the sigma points generated in (d). For the $j$th sigma point, the value for the $i$th parameter is held in ${\mathbf{yt}}\left(\mathit{i},\mathit{j}\right)$, for $\mathit{i}=1,2,\dots ,{m}_{x}$ and $\mathit{j}=1,2,\dots ,n$, where ${m}_{x}$ is the number of state variables and $n$ is the number of sigma points.
5:     $\mathbf{hyt}\left({\mathbf{my}},{\mathbf{n}}\right)$ – Real (Kind=nag_wp) arrayOutput
On exit: $H\left({Y}_{t}\right)$.
For the $j$th sigma point the value for the $i$th parameter should be held in ${\mathbf{hyt}}\left(\mathit{i},\mathit{j}\right)$, for $\mathit{i}=1,2,\dots ,{m}_{y}$ and $\mathit{j}=1,2,\dots ,n$.
6:     $\mathbf{iuser}\left(*\right)$ – Integer arrayUser Workspace
7:     $\mathbf{ruser}\left(*\right)$ – Real (Kind=nag_wp) arrayUser Workspace
h is called with the arguments iuser and ruser as supplied to g13ekf. You should use the arrays iuser and ruser to supply information to h.
8:     $\mathbf{info}$ – IntegerInput/Output
On entry: ${\mathbf{info}}=0$.
On exit: set info to a nonzero value if you wish g13ekf to terminate with ${\mathbf{ifail}}={\mathbf{71}}$.
h must either be a module subprogram USEd by, or declared as EXTERNAL in, the (sub)program from which g13ekf is called. Arguments denoted as Input must not be changed by this procedure.
Note: h should not return floating-point NaN (Not a Number) or infinity values, since these are not handled by g13ekf. If your code inadvertently does return any NaNs or infinities, g13ekf is likely to produce unexpected results.
8:     $\mathbf{x}\left({\mathbf{mx}}\right)$ – Real (Kind=nag_wp) arrayInput/Output
On entry: ${\stackrel{^}{x}}_{t-1}$ the state vector for the previous time point.
On exit: ${\stackrel{^}{x}}_{t}$ the updated state vector.
9:     $\mathbf{st}\left({\mathbf{mx}},{\mathbf{mx}}\right)$ – Real (Kind=nag_wp) arrayInput/Output
On entry: ${S}_{t}$, such that ${S}_{t-1}{S}_{t-1}^{\mathrm{T}}={P}_{t-1}$, 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 exit: ${S}_{t}$, the lower triangular part of a Cholesky factorization of the updated state covariance matrix.
10:   $\mathbf{iuser}\left(*\right)$ – Integer arrayUser Workspace
11:   $\mathbf{ruser}\left(*\right)$ – Real (Kind=nag_wp) arrayUser Workspace
iuser and ruser are not used by g13ekf, but are passed directly to f and h and may be used to pass information to these routines.
12:   $\mathbf{ifail}$ – IntegerInput/Output
On 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, if you are not familiar with this argument, the recommended value is $0$. When the value $-\mathbf{1}\text{​ or ​}\mathbf{1}$ is used it is essential to test the value of ifail on exit.
On exit: ${\mathbf{ifail}}={\mathbf{0}}$ unless the routine detects an error or a warning has been flagged (see Section 6).

## 6Error 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{mx}}=〈\mathit{\text{value}}〉$.
Constraint: ${\mathbf{mx}}\ge 1$.
${\mathbf{ifail}}=21$
On entry, ${\mathbf{my}}=〈\mathit{\text{value}}〉$.
Constraint: ${\mathbf{my}}\ge 1$.
${\mathbf{ifail}}=61$
User requested termination in f.
${\mathbf{ifail}}=71$
User requested termination in h.
${\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$
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.

Not applicable.

## 8Parallelism and Performance

g13ekf 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 implementation-specific information.

None.

## 10Example

This example implements the following nonlinear state space model, with the state vector $x$ and state update function $F$ given by:
 $mx =3 xt+1 = ξt+1 ηt+1 θt+1 T =Fxt+vt = xt+ cos⁡θt -sin⁡θt 0 sin⁡θt cos⁡θt 0 001 0.5⁢r 0.5⁢r 00 r/d -r/d ϕRt ϕLt +vt$
where $r$ and $d$ are known constants and ${\varphi }_{Rt}$ and ${\varphi }_{Lt}$ are time-dependent knowns. The measurement vector $y$ and measurement function $H$ is given by:
 $my =2 yt =δt,αtT =Hxt+ut = Δ-ξt⁢cos⁡A-ηt⁢sin⁡A θt-A +ut$
where $A$ and $\Delta$ are known constants. The initial values, ${x}_{0}$ and ${P}_{0}$, are given by
 $x0 = 0 0 0 , P0 = 0.100 00.10 000.1$
and the Cholesky factorizations of the error covariance matrices, ${L}_{x}$ and ${L}_{x}$ by
 $Lx = 0.100 00.10 000.1 , Ly = 0.010 00.01$

### 10.1Program Text

Program Text (g13ekfe.f90)

### 10.2Program Data

Program Data (g13ekfe.d)

### 10.3Program Results

Program Results (g13ekfe.r)

The example described above can be thought of 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.
© The Numerical Algorithms Group Ltd, Oxford, UK. 2017