g13ek: FL CL CPP AD

NAG CL Interface
g13ekc (kalman_unscented_state)

Keyword Search:

NAG Library Manual, Mark 27

Interfaces: FL CL CPP AD

NAG CL Interface Introduction

G13 (Tsa) Chapter Contents

G13 (Tsa) Chapter Introduction

g13ek: FL CL CPP AD

▸▿ 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

g13ekc applies the Unscented Kalman Filter (UKF) to a nonlinear state space model, with additive noise.

g13ekc uses direct communication for evaluating the nonlinear functionals of the state space model.

2 Specification

#include <nag.h>

void

g13ekc (Integer mx, Integer my, const double y[], const double lx[], const double ly[],

void	(f)(Integer mx, Integer n, const double xt[], double fxt[], Nag_Comm comm, Integer *info),

void	(h)(Integer mx, Integer my, Integer n, const double yt[], double hyt[], Nag_Comm comm, Integer *info),

double x[], double st[], Nag_Comm *comm, NagError *fail)

The function may be called by the names: g13ekc, nag_tsa_kalman_unscented_state or nag_kalman_unscented_state.

3 Description

g13ekc 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 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 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 g13ekc 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 calculating 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.

The constants,

κ

α

and

β

are given by

κ = 3 - m_{x}

α = 1.0

and

β = 2

. If more control is required over the construction of the sigma points then the reverse communication function, g13ejc, can be used instead.

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

1: $mx$ – Integer Input

On entry:

m_{x}

, the number of state variables.

Constraint:

mx \geq 1

2: $my$ – Integer Input

On entry:

m_{y}

, the number of observed variables.

Constraint:

my \geq 1

3: $y [my]$ – const double Input

On entry:

y_{t}

, the observed data at the current time point.

4: $lx [mx \times mx]$ – const double Input

Note: the

(i, j)

th element of the matrix is stored in

lx [(j - 1) \times mx + 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.

Σ_{x}

is time dependent, the value supplied should be for time

t

5: $ly [my \times my]$ – const double Input

Note: the

(i, j)

th element of the matrix is stored in

ly [(j - 1) \times my + 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

6: $f$ – function, supplied by the user External Function

The state function,

F

as described in (b).

The specification of f is:

void	f (Integer mx, Integer n, const double xt[], double fxt[], Nag_Comm comm, Integer info)

1: $mx$ – Integer Input

On entry:

m_{x}

, the number of state variables.

2: $n$ – Integer Input

On entry:

n

, the number of sigma points.

3: $xt [mx \times n]$ – const double Input

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

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

, for

i = 1, 2, \dots, m_{x}

and

j = 1, 2, \dots, n

4: $fxt [mx \times n]$ – double Output

On exit:

F (X_{t})

For the

j

th sigma point the value for the

i

th parameter should be held in

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

, for

i = 1, 2, \dots, m_{x}

and

j = 1, 2, \dots, n

5: $comm$ – Nag_Comm *

Pointer to structure of type Nag_Comm; the following members are relevant to f.

user – double *
iuser – Integer *
p – Pointer: The type Pointer will be void *. Before calling g13ekc you may allocate memory and initialize these pointers with various quantities for use by f when called from g13ekc (see Section 3.1.1 in the Introduction to the NAG Library CL Interface).

6: $info$ – Integer * Input/Output

On entry:

info = 0

On exit: set info to a nonzero value if you wish g13ekc to terminate with

fail . code =

NE_USER_STOP.

Note: f should not return floating-point NaN (Not a Number) or infinity values, since these are not handled by g13ekc. If your code inadvertently does return any NaNs or infinities, g13ekc is likely to produce unexpected results.

7: $h$ – function, supplied by the user External Function

The measurement function,

H

as described in (e).

The specification of h is:

void	h (Integer mx, Integer my, Integer n, const double yt[], double hyt[], Nag_Comm comm, Integer info)

1: $mx$ – Integer Input

On entry:

m_{x}

, the number of state variables.

2: $my$ – Integer Input

On entry:

m_{y}

, the number of observed variables.

3: $n$ – Integer Input

On entry:

n

, the number of sigma points.

4: $yt [mx \times n]$ – const double Input

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

yt [(j - 1) \times mx + i - 1]

, for

i = 1, 2, \dots, m_{x}

and

j = 1, 2, \dots, n

, where

m_{x}

is the number of state variables and

n

is the number of sigma points.

5: $hyt [my \times n]$ – double Output

On exit:

H (Y_{t})

For the

j

th sigma point the value for the

i

th parameter should be held in

hyt [(j - 1) \times my + i - 1]

, for

i = 1, 2, \dots, m_{y}

and

j = 1, 2, \dots, n

6: $comm$ – Nag_Comm *

Pointer to structure of type Nag_Comm; the following members are relevant to h.

user – double *
iuser – Integer *
p – Pointer: The type Pointer will be void *. Before calling g13ekc you may allocate memory and initialize these pointers with various quantities for use by h when called from g13ekc (see Section 3.1.1 in the Introduction to the NAG Library CL Interface).

7: $info$ – Integer * Input/Output

On entry:

info = 0

On exit: set info to a nonzero value if you wish g13ekc to terminate with

fail . code =

NE_USER_STOP.

Note: h should not return floating-point NaN (Not a Number) or infinity values, since these are not handled by g13ekc. If your code inadvertently does return any NaNs or infinities, g13ekc is likely to produce unexpected results.

8: $x [mx]$ – double Input/Output

On entry:

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

the state vector for the previous time point.

On exit:

{\hat{x}}_{t}

the updated state vector.

9: $st [mx \times mx]$ – double Input/Output

Note: the

(i, j)

th element of the matrix is stored in

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

On 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 exit:

S_{t}

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

10: $comm$ – Nag_Comm *

The NAG communication argument (see Section 3.1.1 in the Introduction to the NAG Library CL Interface).

11: $fail$ – NagError * Input/Output

The NAG error argument (see Section 7 in the Introduction to the NAG Library CL Interface).

6 Error Indicators and Warnings

NE_ALLOC_FAIL: Dynamic memory allocation failed.
See Section 3.1.2 in the Introduction to the NAG Library CL Interface for further information.
NE_BAD_PARAM: On entry, argument $〈value〉$ had an illegal value.
NE_INT: On entry, $mx = 〈value〉$ .
Constraint: $mx \geq 1$ .

On entry, $my = 〈value〉$ .
Constraint: $my \geq 1$ .
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 7.5 in the Introduction to the NAG Library CL Interface for further information.
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 8 in the Introduction to the NAG Library CL Interface for further information.
NE_USER_STOP: User requested termination in f.

User requested termination in h.

7 Accuracy

Not applicable.

8 Parallelism and Performance

g13ekc 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

None.

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 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 Library Manual, Mark 27

Interfaces: FL CL CPP AD

NAG CL Interface Introduction

G13 (Tsa) Chapter Contents

G13 (Tsa) Chapter Introduction

g13ek: FL CL CPP AD

NAG CL Interfaceg13ekc (kalman_​unscented_​state)

▸▿ 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

NAG CL Interface
g13ekc (kalman_unscented_state)