NAG Toolbox: nag_tsa_kalman_unscented_state_revcom (g13ej)

Purpose

Syntax

Description

Unscented Kalman Filter Algorithm

Sigma Points

References

Parameters

Compulsory Input Parameters

1:     $\mathrm{irevcm}$ – int64int32nag_int scalar

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 nag_tsa_kalman_unscented_state_revcom (g13ej) has been called at least once before at an earlier time step.

On intermediate re-entry: irevcm must remain unchanged.

Constraint: $\mathbf{irevcm}=0$, $1$, $2$ or $3$.

2:     $\mathrm{y}\left(\mathbf{my}\right)$ – double array

$y_t$, the observed data at the current time point.

3:     $\mathrm{lx}\left(\mathit{ldlx},:\right)$ – double array

The first dimension of the array lx must be at least $\mathbf{mx}$.

The second dimension of the array lx must be at least $\mathbf{mx}$.

$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$.

4:     $\mathrm{ly}\left(\mathit{ldly},:\right)$ – double array

The first dimension of the array ly must be at least $\mathbf{my}$.

The second dimension of the array ly must be at least $\mathbf{my}$.

$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$.

5:     $\mathrm{x}\left(\mathbf{mx}\right)$ – double array

On initial entry: ${\hat{x}}_{t-1}$ the state vector for the previous time point.

On intermediate re-entry: x must remain unchanged.

6:     $\mathrm{st}\left(\mathit{ldst},:\right)$ – double array

The first dimension of the array st must be at least $\mathbf{mx}$.

The second dimension of the array st must be at least $\mathbf{mx}$.

On initial 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 intermediate re-entry: st must remain unchanged.

7:     $\mathrm{xt}\left(\mathit{ldxt1},:\right)$ – double array

The first dimension, $\mathit{ldxt1}$, of the array xt must satisfy

• if $\mathbf{irevcm}=1$ or $2$, $\mathit{ldxt1}=\mathbf{mx}$;
• otherwise $\mathit{ldxt1}\ge 0$.

The second dimension of the array xt must be at least $\mathit{n}$ if $\mathbf{irevcm}=1$ or $2$, and at least $0$ otherwise.

On initial entry: need not be set.

On intermediate re-entry: xt must remain unchanged.

8:     $\mathrm{fxt}\left(\mathit{ldfxt1},:\right)$ – double array

The first dimension, $\mathit{ldfxt1}$, of the array fxt must satisfy

• if $\mathbf{irevcm}=1$, $\mathit{ldfxt1}=\mathbf{mx}$;
• if $\mathbf{irevcm}=2$, $\mathit{ldfxt1}=\mathbf{my}$;
• otherwise $\mathit{ldfxt1}>=0$.

The second dimension of the array fxt must be at least $\mathit{n}$ if $\mathbf{irevcm}=1$ or $2$, and at least $0$ otherwise.

On initial entry: need not be set.

On intermediate re-entry: $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 $\mathit{fxt}\left(i,\mathit{j}\right)$, for $\mathit{j}=1,2,\dots ,\mathit{n}$. When $\mathbf{irevcm}=1$, $i=1,2,\dots ,\mathbf{mx}$ and when $\mathbf{irevcm}=2$, $i=1,2,\dots ,\mathbf{my}$.

9:     $\mathrm{icomm}\left(:\right)$ – int64int32nag_int array

The dimension of the array must be at least $30$ if $\mathbf{irevcm}=1$, $2$ or $3$, and at least $0$ otherwise

10:   $\mathrm{rcomm}\left(:\right)$ – double array

The dimension of the array must be at least $30+\mathbf{my}+\mathbf{mx}\times \mathbf{my}+\left(1+128\right)\times \max \left(\mathbf{mx},\mathbf{my}\right)$ if $\mathbf{irevcm}=1$, $2$ or $3$, and at least $0$ otherwise

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 array 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{ropt}\left(\mathit{lropt}\right)$ – double array

Optional arguments. The default value will be used for $\mathbf{ropt}\left(i\right)$ if $\mathit{lropt}<i$. Setting $\mathit{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 $3-2\times \mathbf{mx}$ when $\mathit{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 $\mathit{ldly}\ne 0$ and the second set of sigma points are augmented, otherwise $\mathbf{ropt}\left(5\right)>-\mathbf{mx}$;
• $\mathbf{ropt}\left(i\right)>0$, for $i=3,6$.

Output Parameters

1:     $\mathrm{irevcm}$ – int64int32nag_int scalar

On intermediate exit: $\mathbf{irevcm}=1$ or $2$. The value of irevcm specifies what intermediate values are returned by this function and what values the calling program must assign to arguments of nag_tsa_kalman_unscented_state_revcom (g13ej) before re-entering the routine. Details of the output and required input are given in the individual argument descriptions.

On final exit: $\mathbf{irevcm}=3$

2:     $\mathrm{x}\left(\mathbf{mx}\right)$ – double array

On intermediate exit: when

$\mathbf{irevcm}=1$

x is unchanged.

$\mathbf{irevcm}=2$

${\hat{x}}_t^{\_}$.

On final exit: ${\hat{x}}_t$ the updated state vector.

3:     $\mathrm{st}\left(\mathit{ldst},:\right)$ – double array

The first dimension of the array st will be $\mathbf{mx}$.

The second dimension of the array st will be $\mathbf{mx}$.

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 final exit: $S_t$, the lower triangular part of a Cholesky factorization of the updated state covariance matrix.

4:     $\mathrm{xt}\left(\mathit{ldxt},:\right)$ – double array

The second dimension of the array xt will be $\mathit{n}$.

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 ,\mathit{n}$.

On final exit: the contents of xt are undefined.

5:     $\mathrm{icomm}\left(\mathit{licomm}\right)$ – int64int32nag_int array

$\mathit{licomm}=30$.

On intermediate exit: icomm is used for storage between calls to nag_tsa_kalman_unscented_state_revcom (g13ej).

$\mathit{licomm}=30$.

On final exit: icomm is not defined.

6:     $\mathrm{rcomm}\left(\mathit{lrcomm}\right)$ – double array

$\mathit{lrcomm}=30+\mathbf{my}+\mathbf{mx}\times \mathbf{my}+2\times \max \left(\mathbf{mx},\mathbf{my}\right)$.

On intermediate exit: rcomm is used for storage between calls to nag_tsa_kalman_unscented_state_revcom (g13ej).

$\mathit{lrcomm}=30+\mathbf{my}+\mathbf{mx}\times \mathbf{my}+2\times \max \left(\mathbf{mx},\mathbf{my}\right)$.

On final exit: rcomm is not defined.

7:     $\mathrm{ifail}$ – int64int32nag_int scalar

On final exit: $\mathbf{ifail}=\mathbf{0}$ unless the function detects an error (see Error Indicators and Warnings).

Error Indicators and Warnings

   $\mathbf{ifail}=11$

Constraint: $\mathbf{irevcm}=0$, $1$, $2$ or $3$.

   $\mathbf{ifail}=21$

Constraint: $\mathbf{mx}\ge 1$.

   $\mathbf{ifail}=22$

mx has changed between calls.

   $\mathbf{ifail}=31$

Constraint: $\mathbf{my}\ge 1$.

   $\mathbf{ifail}=32$

my has changed between calls.

   $\mathbf{ifail}=61$

Constraint: $\mathit{ldlx}=0$ or $\mathit{ldlx}\ge \mathbf{mx}$.

   $\mathbf{ifail}=81$

Constraint: $\mathit{ldly}\ge \mathbf{my}$.

   $\mathbf{ifail}=111$

Constraint: $\mathit{ldst}\ge \mathbf{mx}$.

   $\mathbf{ifail}=171$

Constraint: $\mathbf{ropt}\left(1\right)=1$ or $2$.

   $\mathbf{ifail}=172$

Constraint: $\kappa >\_$.

   $\mathbf{ifail}=173$

Constraint: $\alpha >0$.

   $\mathbf{ifail}=181$

Constraint: $0\le \mathit{lropt}\le 7$.

   $\mathbf{ifail}=191$

icomm has been corrupted between calls.

   $\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.

   $\mathbf{ifail}=-399$

Your licence key may have expired or may not have been installed correctly.

   $\mathbf{ifail}=-999$

Dynamic memory allocation failed.

Accuracy

Further Comments

Example

Open in the MATLAB editor: g13ej_example

function g13ej_example


fprintf('g13ej 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.262 5.923; 4.347 5.783; 3.818 6.181;
      2.706 0.085; 1.878 0.442; 0.684 0.836;
      0.752 1.300; 0.464 1.700; 0.597 1.781;
      0.842 2.040; 1.412 2.286; 1.527 2.820;
      2.399 3.147; 2.661 3.569; 3.327 3.659];

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

% 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);
my = size(ly,1);

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

irevcm = int64(0);
xt = [];
fxt = [];
icomm = int64([]);
rcomm = [];

% 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);

  % Call the Unscented Kalman Filter routine
  while true
    [irevcm,x,st,xt,icomm,rcomm,ifail] = ...
    g13ej( ...
           irevcm,y_t,lx,ly, x,st,xt,fxt,icomm,rcomm);
    switch irevcm
      case 1
        % Evaluate F(x)
        fxt = f(xt,r,d,phi_rt,phi_lt);
      case 2
        % Evaluate H(x)
        fxt = h(xt,Delta,A);
      otherwise
        % irevcm = 3, finished
        break;
    end
  end

  % 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]);

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

% Add legend
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] = f(xt,r,d,phi_rt,phi_lt)
  fxt = zeros(size(xt));

  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;

function [hyt] = h(yt,Delta,A)
  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;

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);

g13ej 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