kalman_filter.hpp

Go to the documentation of this file.

/***************************************************************************
 *   Software License Agreement (BSD License)                              *
 *   Copyright (C) 2015 by Horatiu George Todoran <todorangrg@gmail.com>   *
 *                                                                         *
 *   Redistribution and use in source and binary forms, with or without    *
 *   modification, are permitted provided that the following conditions    *
 *   are met:                                                              *
 *                                                                         *
 *   1. Redistributions of source code must retain the above copyright     *
 *      notice, this list of conditions and the following disclaimer.      *
 *   2. Redistributions in binary form must reproduce the above copyright  *
 *      notice, this list of conditions and the following disclaimer in    *
 *      the documentation and/or other materials provided with the         *
 *      distribution.                                                      *
 *   3. Neither the name of the copyright holder nor the names of its      *
 *      contributors may be used to endorse or promote products derived    *
 *      from this software without specific prior written permission.      *
 *                                                                         *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS   *
 *   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT     *
 *   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS     *
 *   FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE        *
 *   COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,  *
 *   INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,  *
 *   BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;      *
 *   LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER      *
 *   CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT    *
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY *
 *   WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE           *
 *   POSSIBILITY OF SUCH DAMAGE.                                           *
 ***************************************************************************/
#ifndef KALMAN_FILTER_H
#define KALMAN_FILTER_H

#include <float.h>
#include <eigen3/Eigen/Dense>
#include <cmath>
#include <cfloat>
#include <tuw_control/utils.h>

#include <iostream>

namespace tuw
{
template <typename NumType, int XDim>
class KalmanFilter
{
  // special class member functions
public:
  KalmanFilter() = default;

public:
  virtual ~KalmanFilter() = default;

public:
  KalmanFilter(const KalmanFilter&) = default;

public:
  KalmanFilter& operator=(const KalmanFilter&) = default;

public:
  KalmanFilter(KalmanFilter&&) = default;

public:
  KalmanFilter& operator=(KalmanFilter&&) = default;

public:
  using NumericalType = NumType;  
public:
  static constexpr const int xDim = XDim;  

public:
  void init(const Eigen::Matrix<NumType, XDim, 1>& _x0, const Eigen::Matrix<NumType, XDim, XDim>& _Sigma0)
  {
    x_ = _x0;
    Sigma_ = _Sigma0;
  }

public:
  void predict(const Eigen::Matrix<NumType, XDim, 1>& _f, const Eigen::Matrix<NumType, XDim, XDim>& _Phi,
               const Eigen::Matrix<NumType, XDim, XDim>& _Q)
  {
    x_ = _f;
    Sigma_ = _Phi * Sigma_ * _Phi.transpose() + _Q;
  }

public:
  template <int UpdateDim>
  void update(const Eigen::Matrix<NumType, UpdateDim, 1>& _deltah, const Eigen::Matrix<NumType, UpdateDim, XDim>& _C,
              const Eigen::Matrix<NumType, UpdateDim, UpdateDim>& _R)
  {
    Eigen::Matrix<NumType, UpdateDim, UpdateDim> S = _C * Sigma_ * _C.transpose() + _R;
    Eigen::Matrix<NumType, XDim, UpdateDim> K = Sigma_ * _C.transpose() * S.inverse();
    x_ += K * _deltah;
    Sigma_ = (Eigen::Matrix<NumType, XDim, XDim>::Identity(Sigma_.rows(), Sigma_.cols()) - K * _C) * Sigma_;
  }

public:
  const Eigen::Matrix<NumType, XDim, 1>& x() const
  {
    return x_;
  }
public:
  const Eigen::Matrix<NumType, XDim, XDim>& Sigma() const
  {
    return Sigma_;
  }

protected:
  Eigen::Matrix<NumType, XDim, 1> x_;  

protected:
  Eigen::Matrix<NumType, XDim, XDim> Sigma_;  
};
template <typename NumType, int XDim>
constexpr const int KalmanFilter<NumType, XDim>::xDim;

template <typename NumType, int XDim, size_t UDim, typename ParamType>
class KalmanFilterPredictInterface : public KalmanFilter<NumType, XDim>
{
  // special class member functions
public:
  KalmanFilterPredictInterface(ParamType& _params) : KalmanFilter<NumType, XDim>(), params_(_params)
  {
  }

public:
  virtual ~KalmanFilterPredictInterface() = default;

public:
  KalmanFilterPredictInterface(const KalmanFilterPredictInterface&) = default;

public:
  KalmanFilterPredictInterface& operator=(const KalmanFilterPredictInterface&) = default;

public:
  KalmanFilterPredictInterface(KalmanFilterPredictInterface&&) = default;

public:
  KalmanFilterPredictInterface& operator=(KalmanFilterPredictInterface&&) = default;

public:
  using ParamsType = ParamType;  

public:
  void init(const Eigen::Matrix<NumType, XDim, 1>& _x0)
  {
    this->x_ = _x0;
    this->Sigma_.resize(_x0.rows(), _x0.rows());
    computeSigmaInit();
  }

public:
  template <int i = XDim, typename std::enable_if<(i >= 0)>::type* = nullptr>
  void predict(const Eigen::Matrix<NumType, UDim, 1>& _u, const double& _Ta)
  {
    precompute(_Ta);
    computePhi();
    computef(_u);
    computeQ();
    KalmanFilter<NumType, XDim>::predict(f_, Phi_, Q_);
  }

public:
  template <int i = XDim, typename std::enable_if<(i == -1)>::type* = nullptr>
  void predict(const Eigen::Matrix<NumType, UDim, 1>& _u, const double& _Ta)
  {
    f_.resize(this->x_.rows(), 1);
    Phi_.resize(this->x_.rows(), this->x_.rows());
    Q_.resize(this->x_.rows(), this->x_.rows());
    predict<0>(_u, _Ta);
  }

public:
  template <int i = UDim, typename std::enable_if<(i == 0)>::type* = nullptr>
  void predict(const double& _Ta)
  {
    static Eigen::Matrix<double, 0, 1> uEmpty_;
    f_.resize(this->x_.rows(), 1);
    Phi_.resize(this->x_.rows(), this->x_.rows());
    Q_.resize(this->x_.rows(), this->x_.rows());
    predict(uEmpty_, _Ta);
  }

public:
  const ParamType& param() const
  {
    return params_;
  }

protected:
  Eigen::Matrix<NumType, XDim, 1> f_;  
protected:
  Eigen::Matrix<NumType, XDim, XDim> Phi_;  
protected:
  Eigen::Matrix<NumType, XDim, XDim> Q_;  
protected:
  ParamType& params_;  

protected:
  virtual void precompute(const double& _Ta) = 0;
protected:
  virtual void computeSigmaInit() = 0;
protected:
  virtual void computePhi() = 0;
protected:
  virtual void computef(const Eigen::Matrix<NumType, UDim, 1>& _u) = 0;
protected:
  virtual void computeQ() = 0;
};

template <typename KFPredType, int HDim>
class KalmanFilterUpdateInterface
{
  // special class member functions
public:
  KalmanFilterUpdateInterface() = default;

public:
  virtual ~KalmanFilterUpdateInterface() = default;

public:
  KalmanFilterUpdateInterface(const KalmanFilterUpdateInterface&) = default;

public:
  KalmanFilterUpdateInterface& operator=(const KalmanFilterUpdateInterface&) = default;

public:
  KalmanFilterUpdateInterface(KalmanFilterUpdateInterface&&) = default;

public:
  KalmanFilterUpdateInterface& operator=(KalmanFilterUpdateInterface&&) = default;

public:
  static constexpr const int hDim = HDim;  

public:
  const Eigen::Matrix<typename KFPredType::NumericalType, HDim, 1>& deltah() const
  {
    return deltah_;
  }  
public:
  const Eigen::Matrix<typename KFPredType::NumericalType, HDim, KFPredType::xDim>& H() const
  {
    return H_;
  }  
public:
  const Eigen::Matrix<typename KFPredType::NumericalType, HDim, HDim>& R() const
  {
    return R_;
  }  

protected:
  Eigen::Matrix<typename KFPredType::NumericalType, HDim, 1> deltah_;  
protected:
  Eigen::Matrix<typename KFPredType::NumericalType, HDim, KFPredType::xDim> H_;  
protected:
  Eigen::Matrix<typename KFPredType::NumericalType, HDim, HDim> R_;  

protected:
  virtual void precompute(const KFPredType* _kf) = 0;
protected:
  virtual void computeH(const KFPredType* _kf) = 0;
protected:
  virtual void computeDeltah(const KFPredType* _kf,
                             const Eigen::Matrix<typename KFPredType::NumericalType, hDim, 1>& _zObs) = 0;
protected:
  virtual void computeR(const KFPredType* _kf) = 0;

  template <typename KFPredTypeI, typename... KFUpdateType>
  friend class KalmanFilterInterface;
};
template <typename KFPredType, int HDim>
constexpr const int KalmanFilterUpdateInterface<KFPredType, HDim>::hDim;

template <typename KFPredType, typename... KFUpdateType>
class KalmanFilterInterface : public KFPredType
{
  // special class member functions
public:
  KalmanFilterInterface(typename KFPredType::ParamsType& _params) : KFPredType(_params)
  {
  }

public:
  virtual ~KalmanFilterInterface() = default;

public:
  KalmanFilterInterface(const KalmanFilterInterface&) = default;

public:
  KalmanFilterInterface& operator=(const KalmanFilterInterface&) = default;

public:
  KalmanFilterInterface(KalmanFilterInterface&&) = default;

public:
  KalmanFilterInterface& operator=(KalmanFilterInterface&&) = default;

public:
  template <typename KFUpdateTypeI = typename std::tuple_element<0, std::tuple<KFUpdateType...>>::type,
            int i = KFUpdateTypeI::hDim, typename std::enable_if<(i >= 0)>::type* = nullptr>
  void update(const Eigen::Matrix<typename KFPredType::NumericalType, KFUpdateTypeI::hDim, 1>& _zObs)
  {
    auto& updI = std::get<KFUpdateTypeI>(updaters_);
    updI.precompute(this);
    updI.computeH(this);
    updI.computeDeltah(this, _zObs);
    updI.computeR(this);
    KFPredType::update(updI.deltah(), updI.H(), updI.R());
  }

public:
  template <typename KFUpdateTypeI = typename std::tuple_element<0, std::tuple<KFUpdateType...>>::type,
            int i = KFUpdateTypeI::hDim, typename std::enable_if<(i == -1)>::type* = nullptr>
  void update(const Eigen::Matrix<typename KFPredType::NumericalType, KFUpdateTypeI::hDim, 1>& _zObs)
  {
    auto& updI = std::get<KFUpdateTypeI>(updaters_);
    updI.deltah_.resize(_zObs.rows(), 1);
    updI.H_.resize(_zObs.rows(), this->x_.rows());
    updI.R_.resize(_zObs.rows(), _zObs.rows());
    update<KFUpdateTypeI, 0>(_zObs);
  }

private:
  std::tuple<KFUpdateType...> updaters_;  
};
}

#endif  // KALMAN_FILTER_H