Visual Servoing Platform  version 3.6.1 under development (2024-12-07)
vpUnscentedKalman.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Display a point cloud using PCL library.
32  */
33 
34 #ifndef VP_UNSCENTED_KALMAN_H
35 #define VP_UNSCENTED_KALMAN_H
36 
37 #include <visp3/core/vpConfig.h>
38 
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>
43 
44 #include <functional> // std::function
45 #include <memory> // std::shared_ptr
46 
47 BEGIN_VISP_NAMESPACE
134 class VISP_EXPORT vpUnscentedKalman
135 {
136 public:
143  typedef std::function<vpColVector(const vpColVector &, const double &)> vpCommandOnlyFunction;
144 
151  typedef std::function<vpColVector(const vpColVector &, const vpColVector &, const double &)> vpCommandStateFunction;
152 
160  typedef std::function<vpColVector(const std::vector<vpColVector> &, const std::vector<double> &)> vpMeanFunction;
161 
167  typedef std::function<vpColVector(const vpColVector &)> vpMeasurementFunction;
168 
174  typedef std::function<vpColVector(const vpColVector &, const double &)> vpProcessFunction;
175 
183  typedef std::function<vpColVector(const vpColVector &, const vpColVector &)> vpAddSubFunction;
184 
198  vpUnscentedKalman(const vpMatrix &Q, const vpMatrix &R, std::shared_ptr<vpUKSigmaDrawerAbstract> &drawer, const vpProcessFunction &f, const vpMeasurementFunction &h);
199 
206  void init(const vpColVector &mu0, const vpMatrix &P0);
207 
214  {
215  m_b = b;
216  }
217 
224  {
225  m_bx = bx;
226  }
227 
234  inline void setMeasurementMeanFunction(const vpMeanFunction &meanFunc)
235  {
236  m_measMeanFunc = meanFunc;
237  }
238 
245  inline void setMeasurementResidualFunction(const vpAddSubFunction &measResFunc)
246  {
247  m_measResFunc = measResFunc;
248  }
249 
256  inline void setStateAddFunction(const vpAddSubFunction &stateAddFunc)
257  {
258  m_stateAddFunction = stateAddFunc;
259  }
260 
267  inline void setStateMeanFunction(const vpMeanFunction &meanFunc)
268  {
269  m_stateMeanFunc = meanFunc;
270  }
271 
278  inline void setStateResidualFunction(const vpAddSubFunction &stateResFunc)
279  {
280  m_stateResFunc = stateResFunc;
281  }
282 
288  inline void setProcessCovariance(const vpMatrix &Q)
289  {
290  m_Q = Q;
291  }
292 
298  inline void setMeasurementCovariance(const vpMatrix &R)
299  {
300  m_R = R;
301  }
302 
313  void filter(const vpColVector &z, const double &dt, const vpColVector &u = vpColVector());
314 
324  void predict(const double &dt, const vpColVector &u = vpColVector());
325 
331  void update(const vpColVector &z);
332 
338  inline vpMatrix getPest() const
339  {
340  return m_Pest;
341  }
342 
348  inline vpMatrix getPpred() const
349  {
350  return m_Ppred;
351  }
352 
358  inline vpColVector getXest() const
359  {
360  return m_Xest;
361  }
362 
368  inline vpColVector getXpred() const
369  {
370  return m_mu;
371  }
372 
380  inline static vpColVector simpleAdd(const vpColVector &a, const vpColVector &toAdd)
381  {
382  vpColVector res = a + toAdd;
383  return res;
384  }
385 
393  inline static vpColVector simpleResidual(const vpColVector &a, const vpColVector &toSubtract)
394  {
395  vpColVector res = a - toSubtract;
396  return res;
397  }
398 
406  inline static vpColVector simpleMean(const std::vector<vpColVector> &vals, const std::vector<double> &wm)
407  {
408  size_t nbPoints = vals.size();
409  if (nbPoints == 0) {
410  throw(vpException(vpException::dimensionError, "No points to add when computing the mean"));
411  }
412  vpColVector mean = vals[0] * wm[0];
413  for (size_t i = 1; i < nbPoints; ++i) {
414  mean += vals[i] * wm[i];
415  }
416  return mean;
417  }
418 private:
419  bool m_hasUpdateBeenCalled;
420  vpColVector m_Xest;
421  vpMatrix m_Pest;
422  vpMatrix m_Q;
423  std::vector<vpColVector> m_chi;
424  std::vector<double> m_wm;
425  std::vector<double> m_wc;
426  std::vector<vpColVector> m_Y;
427  vpColVector m_mu;
428  vpMatrix m_Ppred;
429  vpMatrix m_R;
430  std::vector<vpColVector> m_Z;
431  vpColVector m_muz;
432  vpMatrix m_Pz;
433  vpMatrix m_Pxz;
434  vpColVector m_y;
435  vpMatrix m_K;
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
452  {
453  vpColVector m_mu;
454  vpMatrix m_P;
455  } vpUnscentedTransformResult;
456 
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);
468 };
469 END_VISP_NAMESPACE
470 #endif
471 #endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
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.