Visual Servoing Platform
version 3.6.1 under development (2024-10-09)
|
#include <visp3/core/vpUKSigmaDrawerMerwe.h>
Public Types | |
typedef vpUnscentedKalman::vpAddSubFunction | vpAddSubFunction |
typedef struct vpUKSigmaDrawerAbstract::vpSigmaPointsWeights | vpSigmaPointsWeights |
Public Member Functions | |
vpUKSigmaDrawerMerwe (const int &n, const double &alpha, const double &beta, const double &kappa, const vpAddSubFunction &resFunc=vpUnscentedKalman::simpleResidual, const vpAddSubFunction &addFunc=vpUnscentedKalman::simpleAdd) | |
virtual | ~vpUKSigmaDrawerMerwe () |
virtual std::vector< vpColVector > | drawSigmaPoints (const vpColVector &mean, const vpMatrix &covariance) override |
virtual vpSigmaPointsWeights | computeWeights () 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 |
This class defines a class to draw sigma points following the E. A. Wan and R. van der Merwe's method proposed in [51].
The method has four parameters: , which is the dimension of the input, and , and , which are three reals. For notational convenience, we define .
Be the mean and the covariance matrix of the input of the algorithm. The algorithm will draw sigma points such as:
where the subscript denotes that we keep the of the matrix.
Several definitions of the square root of a matrix exists. We decided to use the following definition: is the square root of the matrix if can be written as:
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 is computed such as:
The weight used for the computation of the mean of is computed such as:
The weights for the other sigma points are the same for the mean and covariance and are computed as follow:
Note: the weights do not sum to one. Negative values can even be expected.
Additionnal note: the original author recommended to set for Gaussian problems, and , where a larger value for 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.
Definition at line 99 of file vpUKSigmaDrawerMerwe.h.
|
inherited |
The weights corresponding to the sigma points drawing.
BEGIN_VISP_NAMESPACE vpUKSigmaDrawerMerwe::vpUKSigmaDrawerMerwe | ( | const 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.
[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. |
Definition at line 45 of file vpUKSigmaDrawerMerwe.cpp.
References computeLambda().
|
inlinevirtual |
Destructor that does nothing.
Definition at line 119 of file vpUKSigmaDrawerMerwe.h.
|
inlineprotected |
Definition at line 138 of file vpUKSigmaDrawerMerwe.h.
References vpUKSigmaDrawerAbstract::m_n.
Referenced by vpUKSigmaDrawerMerwe().
|
overridevirtual |
Computed the weigths that correspond to the sigma poitns that have been drawn.
Implements vpUKSigmaDrawerAbstract.
Definition at line 71 of file vpUKSigmaDrawerMerwe.cpp.
References m_alpha, m_beta, m_lambda, vpUKSigmaDrawerAbstract::m_n, vpUKSigmaDrawerAbstract::vpSigmaPointsWeights::m_wc, and vpUKSigmaDrawerAbstract::vpSigmaPointsWeights::m_wm.
|
overridevirtual |
Draw the sigma points according to the current mean and covariance of the state of the Unscented Kalman filter.
[in] | mean | The current mean of the state of the UKF. |
[in] | covariance | The current process covariance of the UKF. |
Implements vpUKSigmaDrawerAbstract.
Definition at line 57 of file vpUKSigmaDrawerMerwe.cpp.
References vpMatrix::cholesky(), vpMatrix::getRow(), m_addFunc, m_lambda, vpUKSigmaDrawerAbstract::m_n, m_resFunc, and vpRowVector::transpose().
|
protected |
Addition function expressed in the state space.
Definition at line 149 of file vpUKSigmaDrawerMerwe.h.
Referenced by drawSigmaPoints().
|
protected |
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.
Definition at line 143 of file vpUKSigmaDrawerMerwe.h.
Referenced by computeWeights().
|
protected |
Another factor, which should be set to 2 if the problem is Gaussian.
Definition at line 145 of file vpUKSigmaDrawerMerwe.h.
Referenced by computeWeights().
|
protected |
A third factor, whose value should be set to 3 - n for most problems.
Definition at line 146 of file vpUKSigmaDrawerMerwe.h.
|
protected |
, where is the size of the state vector.
Definition at line 147 of file vpUKSigmaDrawerMerwe.h.
Referenced by computeWeights(), and drawSigmaPoints().
|
protectedinherited |
The size of the state of the UKF.
Definition at line 81 of file vpUKSigmaDrawerAbstract.h.
Referenced by computeLambda(), computeWeights(), and drawSigmaPoints().
|
protected |
Residual function expressed in the state space.
Definition at line 148 of file vpUKSigmaDrawerMerwe.h.
Referenced by drawSigmaPoints().