34 #ifndef VP_UNSCENTED_KALMAN_H
35 #define VP_UNSCENTED_KALMAN_H
37 #include <visp3/core/vpConfig.h>
39 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
40 #include <visp3/core/vpColVector.h>
41 #include <visp3/core/vpMatrix.h>
42 #include <visp3/core/vpUKSigmaDrawerAbstract.h>
236 m_measMeanFunc = meanFunc;
247 m_measResFunc = measResFunc;
258 m_stateAddFunction = stateAddFunc;
269 m_stateMeanFunc = meanFunc;
280 m_stateResFunc = stateResFunc;
408 size_t nbPoints = vals.
size();
413 for (
size_t i = 1; i < nbPoints; ++i) {
414 mean += vals[i] * wm[i];
419 bool m_hasUpdateBeenCalled;
423 std::vector<vpColVector> m_chi;
424 std::vector<double> m_wm;
425 std::vector<double> m_wc;
426 std::vector<vpColVector> m_Y;
430 std::vector<vpColVector> m_Z;
436 vpProcessFunction m_f;
437 vpMeasurementFunction m_h;
438 std::shared_ptr<vpUKSigmaDrawerAbstract> m_sigmaDrawer;
439 vpCommandOnlyFunction m_b;
440 vpCommandStateFunction m_bx;
441 vpMeanFunction m_measMeanFunc;
442 vpAddSubFunction m_measResFunc;
443 vpAddSubFunction m_stateAddFunction;
444 vpMeanFunction m_stateMeanFunc;
445 vpAddSubFunction m_stateResFunc;
451 typedef struct vpUnscentedTransformResult
455 } vpUnscentedTransformResult;
466 static vpUnscentedTransformResult unscentedTransform(
const std::vector<vpColVector> &sigmaPoints,
const std::vector<double> &wm,
467 const std::vector<double> &wc,
const vpMatrix &cov,
const vpAddSubFunction &resFunc,
const vpMeanFunction &meanFunc);
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
@ dimensionError
Bad dimension.
Implementation of a matrix and operations on matrices.
void setMeasurementResidualFunction(const vpAddSubFunction &measResFunc)
Set the measurement residual function to use when computing a subtraction in the measurement space.
void setMeasurementMeanFunction(const vpMeanFunction &meanFunc)
Set the measurement mean function to use when computing a mean in the measurement space.
std::function< vpColVector(const std::vector< vpColVector > &, const std::vector< double > &)> vpMeanFunction
Mean function, which computes the weighted mean of either the prior or the prior expressed in the mea...
static vpColVector simpleAdd(const vpColVector &a, const vpColVector &toAdd)
Simple function to compute an addition, which just does .
vpMatrix getPest() const
Get the estimated (i.e. filtered) covariance of the state.
static vpColVector simpleResidual(const vpColVector &a, const vpColVector &toSubtract)
Simple function to compute a residual, which just does .
void setMeasurementCovariance(const vpMatrix &R)
Permit to change the covariance introduced at each update step.
static vpColVector simpleMean(const std::vector< vpColVector > &vals, const std::vector< double > &wm)
Simple function to compute a mean, which just does .
std::function< vpColVector(const vpColVector &, const vpColVector &)> vpAddSubFunction
Function that computes either the equivalent of an addition or the equivalent of a subtraction in the...
std::function< vpColVector(const vpColVector &, const vpColVector &, const double &)> vpCommandStateFunction
Command model function, which projects effect of the command on the state, when the effect of the com...
vpColVector getXpred() const
Get the predicted state (i.e. the prior).
vpColVector getXest() const
Get the estimated (i.e. filtered) state.
void setStateMeanFunction(const vpMeanFunction &meanFunc)
Set the state mean function to use when computing a mean in the state space.
vpMatrix getPpred() const
Get the predicted covariance of the state, i.e. the covariance of the prior.
void setCommandStateFunction(const vpCommandStateFunction &bx)
Set the command function to use when computing the prior.
std::function< vpColVector(const vpColVector &)> vpMeasurementFunction
Measurement function, which converts the prior points in the measurement space. The argument is a poi...
void setProcessCovariance(const vpMatrix &Q)
Permit to change the covariance introduced at each prediction step.
std::function< vpColVector(const vpColVector &, const double &)> vpCommandOnlyFunction
Command model function, which projects effect of the command on the state. The first argument is the ...
std::function< vpColVector(const vpColVector &, const double &)> vpProcessFunction
Process model function, which projects the sigma points forward in time. The first argument is a sigm...
void setCommandOnlyFunction(const vpCommandOnlyFunction &b)
Set the command function to use when computing the prior.
void setStateResidualFunction(const vpAddSubFunction &stateResFunc)
Set the state residual function to use when computing a subtraction in the state space.
void setStateAddFunction(const vpAddSubFunction &stateAddFunc)
Set the state addition function to use when computing a addition in the state space.