#include <vpUKSigmaDrawerMerwe.h>

Inheritance diagram for vpUKSigmaDrawerMerwe:

Public Types
typedef vpUnscentedKalman::vpAddSubFunction	vpAddSubFunction
typedef struct vpUKSigmaDrawerAbstract::vpSigmaPointsWeights	vpSigmaPointsWeights

Public Member Functions
	vpUKSigmaDrawerMerwe (const unsigned int &n, const double &alpha, const double &beta, const double &kappa, const vpAddSubFunction &resFunc=vpUnscentedKalman::simpleResidual, const vpAddSubFunction &addFunc=vpUnscentedKalman::simpleAdd)
virtual std::vector< vpColVector >	drawSigmaPoints (const vpColVector &mean, const vpMatrix &covariance) VP_OVERRIDE
virtual vpSigmaPointsWeights	computeWeights () VP_OVERRIDE

Protected Member Functions
void	computeLambda ()

Protected Attributes
double	m_alpha
double	m_beta
double	m_kappa
double	m_lambda
vpAddSubFunction	m_resFunc
vpAddSubFunction	m_addFunc
unsigned int	m_n

Detailed Description

This class defines a class to draw sigma points following the E. A. Wan and R. van der Merwe's method proposed in [57].

The method has four parameters: , which is the dimension of the input, and $\alpha$ , $\beta$ and $\kappa$ , which are three reals. For notational convenience, we define $\lambda = \alpha^2 (n - \kappa) - n$ .

Be $\boldsymbol{\mu} \in {R}^n$ the mean and $\boldsymbol{\Sigma} \in {R}^{n x n}$ the covariance matrix of the input of the algorithm. The algorithm will draw sigma points $\chi_i \in {R}^n$ such as:

$\‍[\begin{array}{lcl} \chi_0 &=& \boldsymbol{\mu} \\ \chi_i &=& \begin{cases} \boldsymbol{\mu} + \left[ \sqrt{(n + \lambda) \boldsymbol{\Sigma}} \right]_i^T & i = 1 .. n \\ \boldsymbol{\mu} - \left[ \sqrt{(n + \lambda) \boldsymbol{\Sigma}} \right]_{i - n}^T & i = n + 1 .. 2n \end{cases} \end{array} \‍]$

where the subscript denotes that we keep the $i^{th}$ of the matrix.

Several definitions of the square root of a matrix exists. We decided to use the following definition: $\textbf{L}$ is the square root of the matrix $\boldsymbol{\Sigma}$ if $\boldsymbol{\Sigma}$ can be written as:

$\boldsymbol{\Sigma} = \textbf{L} \textbf{L}^T$

This definition is favored because it can be computed using the Cholesky's decomposition.

The computation of the weights that go along the sigma points is the following. The weight used for the computation of the mean of $\chi_0$ is computed such as:

$w_0^m = \frac{\lambda}{n + \lambda}$

The weight used for the computation of the mean of $\chi_0$ is computed such as:

$w_0^c = \frac{\lambda}{n + \lambda} + 1 - \alpha^2 + \beta$

The weights for the other sigma points $\chi_1 ... \chi_{2n}$ are the same for the mean and covariance and are computed as follow:

$w_i^m = w_i^c = \frac{1}{2(n + \lambda)} i = 1..2n$

Note: the weights do not sum to one. Negative values can even be expected.

Additionalnote: the original author recommended to set $\beta = 2$ for Gaussian problems, $\kappa = 3 - n$ and $0 \leq \alpha \leq 1$ , where a larger value for $\alpha$ spreads the sigma points further from the mean, which can be a problem for highly non-linear problems.

Definition at line 96 of file vpUKSigmaDrawerMerwe.h.

Member Typedef Documentation

◆ vpAddSubFunction

typedef vpUnscentedKalman::vpAddSubFunction vpUKSigmaDrawerMerwe::vpAddSubFunction

Definition at line 99 of file vpUKSigmaDrawerMerwe.h.

◆ vpSigmaPointsWeights

typedef struct vpUKSigmaDrawerAbstract::vpSigmaPointsWeights vpUKSigmaDrawerAbstract::vpSigmaPointsWeights

inherited

The weights corresponding to the sigma points drawing.

Constructor & Destructor Documentation

◆ vpUKSigmaDrawerMerwe()

BEGIN_VISP_NAMESPACE vpUKSigmaDrawerMerwe::vpUKSigmaDrawerMerwe	(	const unsigned int &	n,
		const double &	alpha,
		const double &	beta,
		const double &	kappa,
		const vpAddSubFunction &	resFunc = vpUnscentedKalman::simpleResidual,
		const vpAddSubFunction &	addFunc = vpUnscentedKalman::simpleAdd )

Construct a new vpUKSigmaDrawerMerwe object.

Parameters

[in]	n	The size of the state vector.
[in]	alpha	A factor, which should be a real in the interval [0; 1]. The larger alpha is, the further the sigma points are spread from the mean.
[in]	beta	Another factor, which should be set to 2 if the problem is Gaussian.
[in]	kappa	A third factor, whose value should be set to 3 - n for most problems.
[in]	resFunc	Residual function expressed in the state space.
[in]	addFunc	Addition function expressed in the state space.