vpUnscentedKalman.h

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Display a point cloud using PCL library.
 */
 
#ifndef VP_UNSCENTED_KALMAN_H
#define VP_UNSCENTED_KALMAN_H
 
#include <visp3/core/vpConfig.h>
 
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpUKSigmaDrawerAbstract.h>
 
#include <functional> // std::function
#include <memory> // std::shared_ptr
 
BEGIN_VISP_NAMESPACE
class VISP_EXPORT vpUnscentedKalman
{
public:
  typedef std::function<vpColVector(const vpColVector &, const double &)> vpCommandOnlyFunction;

  typedef std::function<vpColVector(const vpColVector &, const vpColVector &, const double &)> vpCommandStateFunction;

  typedef std::function<vpColVector(const std::vector<vpColVector> &, const std::vector<double> &)> vpMeanFunction;

  typedef std::function<vpColVector(const vpColVector &)> vpMeasurementFunction;

  typedef std::function<vpColVector(const vpColVector &, const double &)> vpProcessFunction;

  typedef std::function<vpColVector(const vpColVector &, const vpColVector &)> vpAddSubFunction;

  vpUnscentedKalman(const vpMatrix &Q, const vpMatrix &R, std::shared_ptr<vpUKSigmaDrawerAbstract> &drawer, const vpProcessFunction &f, const vpMeasurementFunction &h);

  void init(const vpColVector &mu0, const vpMatrix &P0);

  inline void setCommandOnlyFunction(const vpCommandOnlyFunction &b)
  {
    m_b = b;
  }

  inline void setCommandStateFunction(const vpCommandStateFunction &bx)
  {
    m_bx = bx;
  }

  inline void setMeasurementMeanFunction(const vpMeanFunction &meanFunc)
  {
    m_measMeanFunc = meanFunc;
  }

  inline void setMeasurementResidualFunction(const vpAddSubFunction &measResFunc)
  {
    m_measResFunc = measResFunc;
  }

  inline void setStateAddFunction(const vpAddSubFunction &stateAddFunc)
  {
    m_stateAddFunction = stateAddFunc;
  }

  inline void setStateMeanFunction(const vpMeanFunction &meanFunc)
  {
    m_stateMeanFunc = meanFunc;
  }

  inline void setStateResidualFunction(const vpAddSubFunction &stateResFunc)
  {
    m_stateResFunc = stateResFunc;
  }

  inline void setProcessCovariance(const vpMatrix &Q)
  {
    m_Q = Q;
  }

  inline void setMeasurementCovariance(const vpMatrix &R)
  {
    m_R = R;
  }

  void filter(const vpColVector &z, const double &dt, const vpColVector &u = vpColVector());

  void predict(const double &dt, const vpColVector &u = vpColVector());

  void update(const vpColVector &z);

  inline vpMatrix getPest() const
  {
    return m_Pest;
  }

  inline vpMatrix getPpred() const
  {
    return m_Ppred;
  }

  inline vpColVector getXest() const
  {
    return m_Xest;
  }

  inline vpColVector getXpred() const
  {
    return m_mu;
  }

  inline static vpColVector simpleAdd(const vpColVector &a, const vpColVector &toAdd)
  {
    vpColVector res = a + toAdd;
    return res;
  }

  inline static vpColVector simpleResidual(const vpColVector &a, const vpColVector &toSubtract)
  {
    vpColVector res = a - toSubtract;
    return res;
  }

  inline static vpColVector simpleMean(const std::vector<vpColVector> &vals, const std::vector<double> &wm)
  {
    size_t nbPoints = vals.size();
    if (nbPoints == 0) {
      throw(vpException(vpException::dimensionError, "No points to add when computing the mean"));
    }
    vpColVector mean = vals[0] * wm[0];
    for (size_t i = 1; i < nbPoints; ++i) {
      mean += vals[i] * wm[i];
    }
    return mean;
  }
private:
  bool m_hasUpdateBeenCalled; 
  vpColVector m_Xest; 
  vpMatrix m_Pest; 
  vpMatrix m_Q; 
  std::vector<vpColVector> m_chi; 
  std::vector<double> m_wm; 
  std::vector<double> m_wc; 
  std::vector<vpColVector> m_Y; 
  vpColVector m_mu; 
  vpMatrix m_Ppred; 
  vpMatrix m_R; 
  std::vector<vpColVector> m_Z; 
  vpColVector m_muz; 
  vpMatrix m_Pz; 
  vpMatrix m_Pxz; 
  vpColVector m_y; 
  vpMatrix m_K; 
  vpProcessFunction m_f; 
  vpMeasurementFunction m_h; 
  std::shared_ptr<vpUKSigmaDrawerAbstract> m_sigmaDrawer; 
  vpCommandOnlyFunction m_b; 
  vpCommandStateFunction m_bx; 
  vpMeanFunction m_measMeanFunc; 
  vpAddSubFunction m_measResFunc; 
  vpAddSubFunction m_stateAddFunction; 
  vpMeanFunction m_stateMeanFunc; 
  vpAddSubFunction m_stateResFunc; 

  typedef struct vpUnscentedTransformResult
  {
    vpColVector m_mu; 
    vpMatrix m_P; 
  } vpUnscentedTransformResult;

  static vpUnscentedTransformResult unscentedTransform(const std::vector<vpColVector> &sigmaPoints, const std::vector<double> &wm,
    const std::vector<double> &wc, const vpMatrix &cov, const vpAddSubFunction &resFunc, const vpMeanFunction &meanFunc);
};
END_VISP_NAMESPACE
#endif
#endif