NAG C Library Function Document

nag_kalman_unscented_state_revcom (Integer *irevcm, Integer mx, Integer my, const double y[], const double lx[], Integer pdlx, const double ly[], Integer pdly, double x[], double st[], Integer pdst, Integer *n, double xt[], Integer pdxt, double fxt[], Integer pdfxt, const double ropt[], Integer lropt, Integer icomm[], Integer licomm, double rcomm[], Integer lrcomm, NagError *fail)

3

Description

nag_kalman_unscented_state_revcom (g13ejc) 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:

\begin{matrix} x_{t + 1} & = F (x_{t}) + v_{t} \\ y_{t} & = H (x_{t}) + u_{t} \end{matrix}

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

Σ_{x}

, and the measurement noise by

u_{t}

, which is assumed to have mean zero and covariance structure

Σ_{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):

X_{t}

= [{\hat{x}}_{t - 1} {\hat{x}}_{t - 1} + γ \sqrt{P_{t - 1}} {\hat{x}}_{t - 1} - γ \sqrt{P_{t - 1}}]

(1)

(b)

Evaluate the known model function

F

F_{t}

= F (X_{t})

(2)

The function

F

is assumed to accept the

m_{x} \times n

matrix,

X_{t}

and return an

m_{x} \times n

matrix,

F_{t}

. The columns of both

X_{t}

and

F_{t}

correspond to different possible states. The notation

F_{t, i}

is used to denote the

i

th column of

F_{t}

, hence the result of applying

F

to the

i

th possible state.

(c)

Time Update:

${\hat{x}}_{\bar{t}}$	$= \sum_{i = 1}^{n} W_{i}^{m} F_{t, i}$	(3)
$P_{\bar{t}}$	$= \sum_{i = 1}^{n} W_{i}^{c} (F_{t, i} - {\hat{x}}_{\hat{t}}) {(F_{t, i} - {\hat{x}}_{\hat{t}})}^{T} + Σ_{x}$	(4)

(d)

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

Y_{t}

= [{\hat{x}}_{\hat{t}} {\hat{x}}_{\hat{t}} + γ \sqrt{P_{\bar{t}}} {\hat{x}}_{\hat{t}} - γ \sqrt{P_{\bar{t}}}]

(5)

(e)

Evaluate the known model function

H

H_{t}

= H (Y_{t})

(6)

The function

H

is assumed to accept the

m_{x} \times n

matrix,

Y_{t}

and return an

m_{y} \times n

matrix,

H_{t}

. The columns of both

Y_{t}

and

H_{t}

correspond to different possible states. As above

H_{t, i}

is used to denote the

i

th column of

H_{t}

(f)

Measurement Update:

${\hat{y}}_{t}$	$= \sum_{i = 1}^{n} W_{i}^{m} H_{t, i}$	(7)
$P_{y y_{t}}$	$= \sum_{i = 1}^{n} W_{i}^{c} (H_{t, i} - {\hat{y}}_{t}) {(H_{t, i} - {\hat{y}}_{t})}^{T} + Σ_{y}$	(8)
$P_{x y_{t}}$	$= \sum_{i = 1}^{n} W_{i}^{c} (F_{t, i} - {\hat{x}}_{\hat{t}}) {(H_{t, i} - {\hat{y}}_{t})}^{T}$	(9)
$K_{t}$	$= P_{x y_{t}} P_{y y_{t}}^{- 1}$	(10)
${\hat{x}}_{t}$	$= {\hat{x}}_{\hat{t}} + K_{t} (y_{t} - {\hat{y}}_{t})$	(11)
$P_{t}$	$= P_{\bar{t}} - K_{t} P_{y y_{t}} K_{t}^{T}$	(12)

Here

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 nag_kalman_unscented_state_revcom (g13ejc) uses the square-root 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

μ

and covariance matrix

Σ

, the sigma points are usually constructed as:

X_{t} = [μ μ + γ \sqrt{Σ} μ - γ \sqrt{Σ}]

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,

γ

, are constructed as follows:

\begin{matrix} λ & = α^{2} (L + κ) - L \\ γ & = \sqrt{L + λ} \\ W_{i}^{m} & = \{\begin{matrix} \frac{λ}{L + λ} & i = 1 \\ \frac{1}{2 (L + λ)} & i = 2, 3, \dots, 2 L + 1 \end{matrix} \\ W_{i}^{c} & = \{\begin{matrix} \frac{λ}{L + λ} + 1 - α^{2} + β & i = 1 \\ \frac{1}{2 (L + λ)} & i = 2, 3, \dots, 2 L + 1 \end{matrix} \end{matrix}

where, usually

L = m

and

α, β

and

κ

are constants. The total number of sigma points,

n

, is given by

2 L + 1

. The constant

α

is usually set to somewhere in the range

10^{- 4} \leq α \leq 1

and for a Gaussian distribution, the optimal values of

κ

and

β

are

3 - L

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:

Y_{t}

= [X_{t} X_{t, 1} + γ \sqrt{Σ_{x}} X_{t, 1} - γ \sqrt{Σ_{x}}]

(13)

Augmenting the sigma points in this manner requires setting

L

2 L

(and hence

n

2 n - 1

) 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 odd-moments 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 function uses reverse communication. Its use involves an initial entry, intermediate exits and re-entries, and a final exit, as indicated by the argument irevcm. Between intermediate exits and re-entries, all arguments other than fxt must remain unchanged.

1: $irevcm$ – Integer *Input/Output

On initial entry: must be set to

0

3

irevcm = 0

, it is assumed that

t = 0

, otherwise it is assumed that

t \neq 0

and that nag_kalman_unscented_state_revcom (g13ejc) has been called at least once before at an earlier time step.

On intermediate exit:

irevcm = 1

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_kalman_unscented_state_revcom (g13ejc) before re-entering the routine. Details of the output and required input are given in the individual argument descriptions.

On intermediate re-entry: irevcm must remain unchanged.

On final exit:

irevcm = 3

Constraint:

irevcm = 0

1

2

3

Note: any values you return to nag_kalman_unscented_state_revcom (g13ejc) as part of the reverse communication procedure should not include floating-point NaN (Not a Number) or infinity values, since these are not handled by nag_kalman_unscented_state_revcom (g13ejc). If your code inadvertently does return any NaNs or infinities, nag_kalman_unscented_state_revcom (g13ejc) is likely to produce unexpected results.

2: $mx$ – IntegerInput

On entry:

m_{x}

, the number of state variables.

Constraint:

mx \geq 1

3: $my$ – IntegerInput

On entry:

m_{y}

, the number of observed variables.

Constraint:

my \geq 1

4: $y [my]$ – const doubleInput

On entry:

y_{t}

, the observed data at the current time point.

5: $lx [\dim]$ – const doubleInput

Note: the dimension, dim, of the array lx must be at least

pdlx \times mx

The

(i, j)

th element of the matrix is stored in

lx [(j - 1) \times pdlx + i - 1]

On entry:

L_{x}

, such that

L_{x} L_{x}^{T} = Σ_{x}

, i.e., the lower triangular part of a Cholesky decomposition of the process noise covariance structure. Only the lower triangular part of the matrix stored in lx is referenced.

pdlx = 0

, there is no process noise (

v_{t} = 0

for all

t

) and lx is not referenced and may be NULL.

Σ_{x}

is time dependent, the value supplied should be for time

t

6: $pdlx$ – IntegerInput

On entry: the stride separating matrix row elements in the array lx.

Constraint:

pdlx = 0

pdlx \geq mx

7: $ly [\dim]$ – const doubleInput

Note: the dimension, dim, of the array ly must be at least

pdly \times my

The

(i, j)

th element of the matrix is stored in

ly [(j - 1) \times pdly + i - 1]

On entry:

L_{y}

, such that

L_{y} L_{y}^{T} = Σ_{y}

, i.e., the lower triangular part of a Cholesky decomposition of the observation noise covariance structure. Only the lower triangular part of the matrix stored in ly is referenced.

Σ_{y}

is time dependent, the value supplied should be for time

t

8: $pdly$ – IntegerInput

On entry: the stride separating matrix row elements in the array ly.

Constraint:

pdly \geq my

9: $x [mx]$ – doubleInput/Output

On initial entry:

{\hat{x}}_{t - 1}

the state vector for the previous time point.

On intermediate exit: when

$irevcm = 1$: x is unchanged.
$irevcm = 2$: ${\hat{x}}_{\bar{t}}$ .

On intermediate re-entry: x must remain unchanged.

On final exit:

{\hat{x}}_{t}

the updated state vector.

10: $st [\dim]$ – doubleInput/Output

Note: the dimension, dim, of the array st must be at least

pdst \times mx

The

(i, j)

th element of the matrix is stored in

st [(j - 1) \times pdst + i - 1]

On initial entry:

S_{t}

, such that

S_{t - 1} S_{t - 1}^{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 the matrix stored in st is referenced.

On intermediate exit: when

$irevcm = 1$: st is unchanged.
$irevcm = 2$: $S_{\bar{t}}$ , the lower triangular part of a Cholesky factorization of $P_{\bar{t}}$ .

On intermediate re-entry: st must remain unchanged.

On final exit:

S_{t}

, the lower triangular part of a Cholesky factorization of the updated state covariance matrix.

11: $pdst$ – IntegerInput

On entry: the stride separating matrix row elements in the array st.

Constraint:

pdst \geq mx

12: $n$ – Integer *Input/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. nag_kalman_unscented_state_revcom (g13ejc) 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 re-entry: n must remain unchanged.

On final exit: reset to its value on initial entry.

Constraints:

irevcm = 0

3

if redrawn sigma points are used, $n \geq 2 \times mx + 1$ ;
otherwise $n \geq 4 \times mx + 1$ .

13: $xt [\dim]$ – doubleInput/Output

Note: the dimension, dim, of the array xt must be at least

pdxt \times \max (my, n)

On initial entry: need not be set.

On intermediate exit:

X_{t}

when

irevcm = 1

, otherwise

Y_{t}

For the

j

th sigma point, the value for the

i

th parameter is held in

xt [(j - 1) \times pdxt + i - 1]

, for

i = 1, 2, \dots, mx

and

j = 1, 2, \dots, n

On intermediate re-entry: xt must remain unchanged.

On final exit: the contents of xt are undefined.

14: $pdxt$ – IntegerInput

On entry: the stride separating row elements in the two-dimensional data stored in the array xt.

Constraint:

pdxt \geq mx

15: $fxt [\dim]$ – doubleInput/Output

Note: the dimension, dim, of the array fxt must be at least

pdfxt \times (n + \max (mx, my))

On initial entry: need not be set.

On intermediate exit: the contents of fxt are undefined.

On intermediate re-entry:

F (X_{t})

when

irevcm = 1

, otherwise

H (Y_{t})

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

fxt [(j - 1) \times pdfxt + i - 1]

, for

j = 1, 2, \dots, n

. When

irevcm = 1

i = 1, 2, \dots, mx

and when

irevcm = 2

i = 1, 2, \dots, my

On final exit: the contents of fxt are undefined.

16: $pdfxt$ – IntegerInput

On entry: the stride separating row elements in the two-dimensional data stored in the array fxt.

Constraint:

pdfxt \geq \max (mx, my)

17: $ropt [lropt]$ – const doubleInput

On entry: optional arguments. The default value will be used for

ropt [i - 1]

lropt < i

. Setting

lropt = 0

will use the default values for all optional arguments and ropt need not be set and may be NULL.

$ropt [0]$: 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., $ropt [0] = 1$ )
$ropt [1]$: $κ_{x}$ , value of $κ$ used when constructing the first set of sigma points, $X_{t}$ .
Defaults to $3 - mx$ .
$ropt [2]$: $α_{x}$ , value of $α$ used when constructing the first set of sigma points, $X_{t}$ .
Defaults to $1$ .
$ropt [3]$: $β_{x}$ , value of $β$ used when constructing the first set of sigma points, $X_{t}$ .
Defaults to $2$ .
$ropt [4]$: Value of $κ$ used when constructing the second set of sigma points, $Y_{t}$ .
Defaults to $3 - 2 \times mx$ when $pdlx \neq 0$ and the second set of sigma points are augmented and $κ_{x}$ otherwise.
$ropt [5]$: Value of $α$ used when constructing the second set of sigma points, $Y_{t}$ .
Defaults to $α_{x}$ .
$ropt [6]$: Value of $β$ used when constructing the second set of sigma points, $Y_{t}$ .
Defaults to $β_{x}$ .

Constraints:

$ropt [0] = 1$ or $2$ ;
$ropt [1] > - mx$ ;
$ropt [4] > - 2 \times mx$ when $pdly \neq 0$ and the second set of sigma points are augmented, otherwise $ropt [4] > - mx$ ;
$ropt [i - 1] > 0$ , for $i = 3, 4, 5, 6$ .

18: $lropt$ – IntegerInput

On entry: length of the options array ropt.

Constraint:

0 \leq lropt \leq 7

19: $icomm [licomm]$ – IntegerCommunication Array

On initial entry: icomm need not be set.

On intermediate exit: icomm is used for storage between calls to nag_kalman_unscented_state_revcom (g13ejc).

On intermediate re-entry: icomm must remain unchanged.

On final exit: icomm is not defined.

20: $licomm$ – IntegerInput

On entry: the length of the array icomm. If licomm is too small and

licomm \geq 2

then

fail . code =

NE_TOO_SMALL is returned and the minimum value for licomm and lrcomm are given by

icomm [0]

and

icomm [1]

respectively.

Constraint:

licomm \geq 30

21: $rcomm [lrcomm]$ – doubleCommunication Array

On initial entry: rcomm need not be set.

On intermediate exit: rcomm is used for storage between calls to nag_kalman_unscented_state_revcom (g13ejc).

On intermediate re-entry: rcomm must remain unchanged.

On final exit: rcomm is not defined.

22: $lrcomm$ – IntegerInput

On entry: the length of the array rcomm. If lrcomm is too small and

licomm \geq 2

then

fail . code =

NW_INT is returned and the minimum value for licomm and lrcomm are given by

icomm [0]

and

icomm [1]

respectively.

Suggested value:

lrcomm = 30 + my + mx \times my + (1 + nb) \times \max (mx, my)

, where

nb

is the optimal block size. In most cases a block size of

128

will be sufficient.

Constraint:

lrcomm \geq 30 + my + mx \times my + 2 \times \max (mx, my)

23: $fail$ – NagError *Input/Output

The NAG error argument (see Section 3.7 in How to Use the NAG Library and its Documentation).

6

Error Indicators and Warnings

NE_ALLOC_FAIL: Dynamic memory allocation failed.
See Section 2.3.1.2 in How to Use the NAG Library and its Documentation for further information.
NE_ARRAY_SIZE: On entry, $pdfxt = 〈value〉$ and $mx = 〈value〉$ .
Constraint: if $irevcm = 1$ , $pdfxt \geq mx$ .

On entry, $pdfxt = 〈value〉$ and $my = 〈value〉$ .
Constraint: if $irevcm = 2$ , $pdfxt \geq my$ .

On entry, $pdlx = 〈value〉$ and $mx = 〈value〉$ .
Constraint: $pdlx = 0$ or $pdlx \geq mx$ .

On entry, $pdly = 〈value〉$ and $my = 〈value〉$ .
Constraint: $pdly \geq my$ .

On entry, $pdst = 〈value〉$ and $mx = 〈value〉$ .
Constraint: $pdst \geq mx$ .

On entry, $pdxt = 〈value〉$ and $mx = 〈value〉$ .
Constraint: $pdxt \geq mx$ .
NE_BAD_PARAM: On entry, argument $〈value〉$ had an illegal value.
NE_ILLEGAL_COMM: icomm has been corrupted between calls.

rcomm has been corrupted between calls.
NE_INT: On entry, $lropt = 〈value〉$ .
Constraint: $0 \leq lropt \leq 7$ .

On entry, $irevcm = 〈value〉$ .
Constraint: $irevcm = 0$ , $1$ , $2$ or $3$ .

On entry, $mx = 〈value〉$ .
Constraint: $mx \geq 1$ .

On entry, $my = 〈value〉$ .
Constraint: $my \geq 1$ .

On entry, augmented sigma points requested, $n = 〈value〉$ and $mx = 〈value〉$ .
Constraint: $n \geq 〈value〉$ .

On entry, redrawn sigma points requested, $n = 〈value〉$ and $mx = 〈value〉$ .
Constraint: $n \geq 〈value〉$ .
NE_INT_CHANGED: mx has changed between calls.
On intermediate entry, $mx = 〈value〉$ .
On initial entry, $mx = 〈value〉$ .

my has changed between calls.
On intermediate entry, $my = 〈value〉$ .
On initial entry, $my = 〈value〉$ .

n has changed between calls.
On intermediate entry, $n = 〈value〉$ .
On intermediate exit, $n = 〈value〉$ .
NE_INTERNAL_ERROR: An internal error has occurred in this function. Check the function call and any array sizes. If the call is correct then please contact NAG for assistance.
See Section 2.7.6 in How to Use the NAG Library and its Documentation for further information.
NE_INVALID_OPTION: On entry, $ropt [0] = 〈value〉$ .
Constraint: $ropt [0] = 1$ or $2$ .

On entry, $ropt [〈value〉] = 〈value〉$ .
Constraint: $α > 0$ .

On entry, $ropt [〈value〉] = 〈value〉$ .
Constraint: $κ > 〈value〉$ .
NE_MAT_NOT_POS_DEF: A weight was negative and it was not possible to downdate the Cholesky factorization.

Unable to calculate the Cholesky factorization of the updated state covariance matrix.

Unable to calculate the Kalman gain matrix.
NE_NO_LICENCE: Your licence key may have expired or may not have been installed correctly.
See Section 2.7.5 in How to Use the NAG Library and its Documentation for further information.
NE_TOO_SMALL: On entry, $licomm = 〈value〉$ .
Constraint: $licomm \geq 2$ .
icomm is too small to return the required array sizes.
NW_INT: On entry, $licomm = 〈value〉$ and $lrcomm = 〈value〉$ .
Constraint: $licomm \geq 30$ and $lrcomm \geq 30 + my + mx \times my + 2 \times \max (mx, my)$ .
The minimum required values for licomm and lrcomm are returned in $icomm [0]$ and $icomm [1]$ respectively.

7

Accuracy

Not applicable.

8

Parallelism and Performance

nag_kalman_unscented_state_revcom (g13ejc) 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 function. Please also consult the Users' Note for your implementation for any additional implementation-specific information.

9

Further Comments

As well as implementing the Unscented Kalman Filter, nag_kalman_unscented_state_revcom (g13ejc) can also be used to apply the Unscented Transform (see Julier (2002)) to the function

F

, by setting

pdlx = 0

and terminating the calling sequence when

irevcm = 2

rather than

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

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:

\begin{matrix} m_{x} & = 3 \\ x_{t + 1} & = {(\begin{matrix} ξ_{t + 1} & η_{t + 1} & θ_{t + 1} \end{matrix})}^{T} \\ = F (x_{t}) + v_{t} \\ = x_{t} + (\begin{array}{l} \cos θ_{t} & - \sin θ_{t} & 0 \\ \sin θ_{t} & \cos θ_{t} & 0 \\ 0 & 0 & 1 \end{array}) (\begin{array}{l} 0.5 r & 0.5 r \\ 0 & 0 \\ r / d & - r / d \end{array}) (\begin{array}{l} ϕ_{R t} \\ ϕ_{L t} \end{array}) + v_{t} \end{matrix}

where

r

and

d

are known constants and

ϕ_{R t}

and

ϕ_{L t}

are time-dependent knowns. The measurement vector

y

and measurement function

H

is given by:

\begin{matrix} m_{y} & = 2 \\ y_{t} & = {(δ_{t}, α_{t})}^{T} \\ = H (x_{t}) + u_{t} \\ = (\begin{matrix} Δ - ξ_{t} \cos A - η_{t} \sin A \\ θ_{t} - A \end{matrix}) + u_{t} \end{matrix}

where

A

and

Δ

are known constants. The initial values,

x_{0}

and

P_{0}

, are given by

\begin{matrix} x_{0} & = & (\begin{matrix} 0 \\ 0 \\ 0 \end{matrix}), & P_{0} & = & (\begin{matrix} 0.1 & 0 & 0 \\ 0 & 0.1 & 0 \\ 0 & 0 & 0.1 \end{matrix}) \end{matrix}

and the Cholesky factorizations of the error covariance matrices,

L_{x}

and

L_{x}

\begin{matrix} \begin{matrix} L_{x} & = & (\begin{matrix} 0.1 & 0 & 0 \\ 0 & 0.1 & 0 \\ 0 & 0 & 0.1 \end{matrix}) \end{matrix}, & \begin{matrix} L_{y} & = & (\begin{matrix} 0.01 & 0 \\ 0 & 0.01 \end{matrix}) \end{matrix} . \end{matrix}

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

(ξ, η)

giving the

x

and

y

coordinates and

θ

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

ϕ_{R t}

and the left at a velocity of

ϕ_{L t}

. In this example, these velocities are fixed with

ϕ_{R t} = 0.4

and

ϕ_{L t} = 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,

Δ

, from the origin and its angle,

A

, from the

x

-axis. The robot has a sensor that is able to measure

y

, with

δ

being the distance to the wall and

α

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.

NAG C Library Manual, Mark 26

NAG Library Manual, Mark 26

NAG AD Library Manual, Mark 26

g13 Chapter Contents

g13 Chapter Introduction

NAG C Library Function Document

nag_kalman_unscented_state_revcom (g13ejc)

▸▿ Contents

1 Purpose

2 Specification

3 Description

3.1 Unscented Kalman Filter Algorithm

3.2 Sigma Points

4 References

5 Arguments

6 Error Indicators and Warnings

7 Accuracy

8 Parallelism and Performance

9 Further Comments

10 Example

10.1 Program Text

10.2 Program Data

10.3 Program Results

1

Purpose

2

Specification

3

Description

3.1

Unscented Kalman Filter Algorithm

3.2

Sigma Points

4

References

5

Arguments

6

Error Indicators and Warnings

7

Accuracy

8

Parallelism and Performance

9

Further Comments

10

Example

10.1

Program Text

10.2

Program Data

10.3

Program Results