UKSigmaDrawerMerwe¶

class UKSigmaDrawerMerwe(self, n: int, alpha: float, beta: float, kappa: float, resFunc: Callable[[visp._visp.core.ColVector, visp._visp.core.ColVector], visp._visp.core.ColVector], addFunc: Callable[[visp._visp.core.ColVector, visp._visp.core.ColVector], visp._visp.core.ColVector])¶

Bases: UKSigmaDrawerAbstract

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

The method has four parameters: \(n\) , 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 \(2n + 1\) sigma points \(\chi_i \in {R}^n\) such as:

\[\begin{split}\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} \end{split}\]

where the subscript \(i\) 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.

Additionnal note: 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.

Methods

`__init__`
`computeWeights`	Computed the weigths that correspond to the sigma poitns that have been drawn.
`drawSigmaPoints`	Draw the sigma points according to the current mean and covariance of the state of the Unscented Kalman filter.

Inherited Methods

`SigmaPointsWeights`

Operators

`__doc__`
`__init__`
`__module__`

Attributes

`__annotations__`

__init__(self, n: int, alpha: float, beta: float, kappa: float, resFunc: Callable[[visp._visp.core.ColVector, visp._visp.core.ColVector], visp._visp.core.ColVector], addFunc: Callable[[visp._visp.core.ColVector, visp._visp.core.ColVector], visp._visp.core.ColVector])¶

computeWeights(self) → visp._visp.core.UKSigmaDrawerAbstract.SigmaPointsWeights¶

Computed the weigths that correspond to the sigma poitns that have been drawn.

Returns:: vpSigmaPointsWeights The weights that correspond to the sigma points.

drawSigmaPoints(self, mean: visp._visp.core.ColVector, covariance: visp._visp.core.Matrix) → list[visp._visp.core.ColVector]¶

Draw the sigma points according to the current mean and covariance of the state of the Unscented Kalman filter.

Parameters:

mean: visp._visp.core.ColVector¶: The current mean of the state of the UKF.
covariance: visp._visp.core.Matrix¶: The current process covariance of the UKF.

Returns:

std::vector<vpColVector> The sigma points.