Integer type:  int32  int64  nag_int  show int32  show int32  show int64  show int64  show nag_int  show nag_int

Chapter Contents
Chapter Introduction
NAG Toolbox

# NAG Toolbox: nag_tsa_kalman_unscented_state (g13ek)

## Purpose

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

## Syntax

[x, st, user, ifail] = g13ek(y, lx, ly, f, h, x, st, 'mx', mx, 'my', my, 'user', user)
[x, st, user, ifail] = nag_tsa_kalman_unscented_state(y, lx, ly, f, h, x, st, 'mx', mx, 'my', my, 'user', user)

## Description

nag_tsa_kalman_unscented_state (g13ek) 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}$.

### Unscented 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 Sigma Points):
 $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 Sigma Points):
 $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 nag_tsa_kalman_unscented_state (g13ek) uses the square-root form described in the Haykin (2001).

### 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:
 $λ =α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 function, nag_tsa_kalman_unscented_state_revcom (g13ej), can be used instead.

## 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

## Parameters

### Compulsory Input Parameters

1:     $\mathrm{y}\left({\mathbf{my}}\right)$ – double array
${y}_{t}$, the observed data at the current time point.
2:     $\mathrm{lx}\left({\mathbf{mx}},{\mathbf{mx}}\right)$ – double array
${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, then the value supplied should be for time $t$.
3:     $\mathrm{ly}\left({\mathbf{my}},{\mathbf{my}}\right)$ – double array
${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, then the value supplied should be for time $t$.
4:     $\mathrm{f}$ – function handle or string containing name of m-file
The state function, $F$ as described in (b).
[fxt, user, info] = f(xt, user, info)

Input Parameters

1:     $\mathrm{xt}\left({\mathbf{mx}},\mathbf{n}\right)$ – double array
${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$, where ${m}_{x}$ is the number of state variables and $n$ is the number of sigma points.
2:     $\mathrm{user}$ – Any MATLAB object
f is called from nag_tsa_kalman_unscented_state (g13ek) with the object supplied to nag_tsa_kalman_unscented_state (g13ek).
3:     $\mathrm{info}$int64int32nag_int scalar
${\mathbf{info}}=0$.

Output Parameters

1:     $\mathrm{fxt}\left({\mathbf{mx}},\mathbf{n}\right)$ – double array
$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$, where ${m}_{x}$ is the number of observed variables and $n$ is the number of sigma points supplied in xt.
2:     $\mathrm{user}$ – Any MATLAB object
3:     $\mathrm{info}$int64int32nag_int scalar
Set info to a nonzero value if you wish nag_tsa_kalman_unscented_state (g13ek) to terminate with ${\mathbf{ifail}}={\mathbf{61}}$.
5:     $\mathrm{h}$ – function handle or string containing name of m-file
The measurement function, $H$ as described in (e).
[hyt, user, info] = h(yt, user, info)

Input Parameters

1:     $\mathrm{yt}\left({\mathbf{mx}},\mathbf{n}\right)$ – double array
${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.
2:     $\mathrm{user}$ – Any MATLAB object
h is called from nag_tsa_kalman_unscented_state (g13ek) with the object supplied to nag_tsa_kalman_unscented_state (g13ek).
3:     $\mathrm{info}$int64int32nag_int scalar
${\mathbf{info}}=0$.

Output Parameters

1:     $\mathrm{hyt}\left({\mathbf{my}},\mathbf{n}\right)$ – double array
$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$, where ${m}_{y}$ is the number of observed variables and $n$ is the number of sigma points supplied in yt.
2:     $\mathrm{user}$ – Any MATLAB object
3:     $\mathrm{info}$int64int32nag_int scalar
Set info to a nonzero value if you wish nag_tsa_kalman_unscented_state (g13ek) to terminate with ${\mathbf{ifail}}={\mathbf{71}}$.
6:     $\mathrm{x}\left({\mathbf{mx}}\right)$ – double array
The dimension of the array x must be at least ${\mathbf{mx}}$
${\stackrel{^}{x}}_{t-1}$ the state vector for the previous time point.
7:     $\mathrm{st}\left({\mathbf{mx}},{\mathbf{mx}}\right)$ – double array
${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.

### Optional Input Parameters

1:     $\mathrm{mx}$int64int32nag_int scalar
Default: the dimension of the array x and the first dimension of the arrays st, lx and the second dimension of the arrays st, lx. (An error is raised if these dimensions are not equal.)
${m}_{x}$, the number of state variables.
Constraint: ${\mathbf{mx}}\ge 1$.
2:     $\mathrm{my}$int64int32nag_int scalar
Default: the dimension of the array y and the first dimension of the array ly and the second dimension of the array ly. (An error is raised if these dimensions are not equal.)
${m}_{y}$, the number of observed variables.
Constraint: ${\mathbf{my}}\ge 1$.
3:     $\mathrm{user}$ – Any MATLAB object
user is not used by nag_tsa_kalman_unscented_state (g13ek), but is passed to f and h. Note that for large objects it may be more efficient to use a global variable which is accessible from the m-files than to use user.

### Output Parameters

1:     $\mathrm{x}\left({\mathbf{mx}}\right)$ – double array
The dimension of the array x will be ${\mathbf{mx}}$
${\stackrel{^}{x}}_{t}$ the updated state vector.
2:     $\mathrm{st}\left({\mathbf{mx}},{\mathbf{mx}}\right)$ – double array
The second dimension of the array st will be ${\mathbf{mx}}$.
${S}_{t}$, the lower triangular part of a Cholesky factorization of the updated state covariance matrix.
3:     $\mathrm{user}$ – Any MATLAB object
4:     $\mathrm{ifail}$int64int32nag_int scalar
${\mathbf{ifail}}={\mathbf{0}}$ unless the function detects an error (see Error Indicators and Warnings).

## Error Indicators and Warnings

Errors or warnings detected by the function:
${\mathbf{ifail}}=11$
Constraint: ${\mathbf{mx}}\ge 1$.
${\mathbf{ifail}}=21$
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$
${\mathbf{ifail}}=-399$
Your licence key may have expired or may not have been installed correctly.
${\mathbf{ifail}}=-999$
Dynamic memory allocation failed.

Not applicable.

None.

## Example

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$
function g13ek_example

fprintf('g13ek example results\n\n');

% Cholesky factorisation of the covariance matrix for
% the process noise
lx = 0.1 * eye(3);

% Cholesky factorisation of the covariance matrix for
% the observation noise
ly = 0.01 * eye(2);

% Initial state vector
ix = zeros(size(lx,1),1);
x = ix;

% Cholesky factorisation of the initial state covariance matrix
st = 0.1 * eye(3);

% Constant terms in the state space model
r = 3;
d = 4;
Delta = 5.814;
A = 0.464;

% Observed data, y = (delta, alpha)
y = [5.2620 4.3470 3.8180 2.7060 1.8780 0.6840 0.7520 ...
0.4640 0.5970 0.8420 1.4120 1.5270 2.3990 2.6610 3.3270;
5.9230 5.7830 6.1810 0.0850 0.4420 0.8360 1.3000 ...
1.7000 1.7810 2.0400 2.2860 2.8200 3.1470 3.5690 3.6590];

% Number of time points to run the system for
ntime = size(y,2);

% phi_r and phi_l (these are the same across all time points in
% this example)
phi_r = ones(ntime,1) * 0.4;
phi_l = ones(ntime,1) * 0.1;

mx = numel(x);

% Reserve some space to hold the state
cx = zeros(mx,ntime);

% Loop over each time point
for t = 1:ntime

% Observed data at time point t
y_t = y(:,t);
phi_rt = phi_r(t);
phi_lt = phi_l(t);

% Store the information required by f and h in a cell array
user = {Delta;A;r;d;phi_rt;phi_lt};

% Update the state and its covariance matrix
[x,st,user,ifail] = g13ek( ...
y_t,lx,ly,@f,@h,x,st,'user',user);

% Store the current state
cx(:,t) = x(:);
end

% Print the results
ttext = ['  Time  ' blanks(ceil((11*mx- 16)/2)) ' Estimate of State' ...
blanks(ceil((11*mx -16)/2))];
fprintf('%s\n',ttext);
ttext(:) = '-';
fprintf('%s\n',ttext);
for t = 1:ntime
fprintf('  %3d   ', t);
fprintf(' %10.3f', cx(1:mx,t));
fprintf('\n');
end

fprintf('\nEstimate of Cholesky Factorisation of the State\n');
fprintf('Covariance Matrix at the Last Time Point\n');
for i=1:mx
for j=1:i
fprintf(' %10.2e',st(i,j));
end
fprintf('\n');
end

% Plot the results
fig1 = figure;

% calculate and plot the position and facing of the robot as if there
% were no slippage in the wheels
pos_no_slippage(:,1) = ix;
rot_mat = [r/2 r/2; 0 0;r/d -r/d];
for t=1:ntime
v_r = rot_mat * [phi_r(t); phi_l(t)];
theta = pos_no_slippage(3,t);
T = [cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0; 0 0 1];
pos_no_slippage(:,t+1) = pos_no_slippage(:,t) + T*v_r;
end

% formula (of the form y = a + b x) for the position of the wall
b = -cos(A) / sin(A);
a = Delta * (sin(A) + cos(A)^2/sin(A));

% actual position and facing of the robot
% (this would usually be unknown, but this example
% is based on a simulation and hence we know the answer)
pos_actual = [0.000 0.617 1.590 2.192 ...
3.238 3.947 4.762 4.734 ...
4.529 3.955 3.222 2.209 ...
2.047 1.137 0.903 0.443;
0.000 0.000 0.101 0.079 ...
0.474 0.908 1.947 1.850 ...
2.904 3.757 4.675 5.425 ...
5.492 5.362 5.244 4.674;
0.000 0.103 0.036 0.361 ...
0.549 0.906 1.299 1.763 ...
2.164 2.245 2.504 2.749 ...
3.284 3.610 4.033 4.123];

% produce the plot
h(1) = plot_robot(pos_no_slippage,'s','green','green');
hold on
h(2) = plot_robot(pos_actual,'c','red','red');
h(3) = plot_robot([zeros(3,1) cx],'c','blue','none');
hold off

% Add reference line for the wall
yl = ylim;
line([(yl(1) - a)/b (yl(2) - a) / b],yl,'Color','black');
xlim([-0.5 7]);

title({'{\bf g13ek Example Plot}',
'Illustration of Position and Orientation',
' of Hypothetical Robot'});

label = ['Initial' 'Actual' 'Updated'];
h(4) = legend(h,'Initial','Actual','Updated','Location','NorthEast');
set(h(4),'FontSize',get(h(4),'FontSize')*0.8);

% Add text to indicate wall
text(4.6,3.9,'Wall','Rotation',-63);

function [fxt,user,info] = f(xt,user,info)
r = user{3};
d = user{4};
phi_rt = user{5};
phi_lt = user{6};

t1 = 0.5*r*(phi_rt+phi_lt);
t3 = (r/d)*(phi_rt-phi_lt);

fxt(1,:) = xt(1,:) + cos(xt(3,:))*t1;
fxt(2,:) = xt(2,:) + sin(xt(3,:))*t1;
fxt(3,:) = xt(3,:) + t3;

% Set info nonzero to terminate execution for any reason.
info = int64(0);

function [hyt,user,info] = h(yt,user,info)
Delta = user{1};
A = user{2};

hyt(1,:) = Delta - yt(1,:)*cos(A) - yt(2,:)*sin(A);
hyt(2,:) = yt(3,:) - A;

% Make sure that the theta is in the same range as the observed
% data, which in this case is [0, 2*pi)
hyt(2,(hyt(2,:) < 0)) = hyt(2,(hyt(2,:) < 0)) + 2 * pi;

% Set info nonzero to terminate execution for any reason.
info = int64(0);

function [h] =  plot_robot(x,symbol,colour,fill)
alen = 0.3;
h = scatter(x(1,:),x(2,:),60,colour,symbol,'MarkerFaceColor',fill);
aend = [x(1,:)+alen*cos(x(3,:)); x(2,:)+alen*sin(x(3,:))];
line([x(1,:); aend(1,:)],[x(2,:); aend(2,:)],'Color',colour);

g13ek example results

Time            Estimate of State
--------------------------------------------
1         0.664     -0.092      0.104
2         1.598      0.081      0.314
3         2.128      0.213      0.378
4         3.134      0.674      0.660
5         3.809      1.181      0.906
6         4.730      2.000      1.298
7         4.429      2.474      1.762
8         4.357      3.246      2.162
9         3.907      3.852      2.246
10         3.360      4.398      2.504
11         2.552      4.741      2.750
12         2.191      5.193      3.281
13         1.309      5.018      3.610
14         1.071      4.894      4.031
15         0.618      4.322      4.124

Estimate of Cholesky Factorisation of the State
Covariance Matrix at the Last Time Point
1.92e-01
-3.82e-01   2.22e-02
1.58e-06   2.23e-07   9.95e-03 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.