ekf.h

Go to the documentation of this file.

//=================================================================================================
// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * 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.
//     * Neither the name of the Flight Systems and Automatic Control group,
//       TU Darmstadt, 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 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 HECTOR_POSE_ESTIMATION_FILTER_EKF_H
#define HECTOR_POSE_ESTIMATION_FILTER_EKF_H

#include <hector_pose_estimation/filter.h>

namespace hector_pose_estimation {
namespace filter {

class EKF : public Filter
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  EKF(State &state);
  virtual ~EKF();

  virtual std::string getType() const { return "EKF"; }

  virtual bool init(PoseEstimation &estimator);
  virtual bool preparePredict(double dt);
  virtual bool predict(const SystemPtr& system, double dt);
  virtual bool doPredict(double dt);

  class Predictor
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Predictor(EKF *filter)
      : filter_(filter)
      , x_diff(filter->state().getVectorDimension())
      , A(filter->state().getCovarianceDimension(), filter->state().getCovarianceDimension())
      , Q(filter->state().getCovarianceDimension(), filter->state().getCovarianceDimension())
    {
      x_diff.setZero();
      A.setZero();
      Q.setZero();
    }
    virtual ~Predictor() {}
    virtual bool predict(double dt) = 0;

  protected:
    EKF *filter_;

  public:
    State::Vector x_diff;
    State::SystemMatrix A;
    State::Covariance Q;
  };

  template <class ConcreteModel, typename Enabled = void>
  class Predictor_ : public Filter::template Predictor_<ConcreteModel>, public Predictor
  {
  public:
    typedef ConcreteModel Model;
    typedef typename Filter::template Predictor_<ConcreteModel> Base;
    using Filter::template Predictor_<ConcreteModel>::state;

    Predictor_(EKF *filter, Model *model)
      : Base(filter, model)
      , Predictor(filter)
    {}
    virtual ~Predictor_() {}

    virtual bool predict(double dt);
  };

  class Corrector
  {
  public:
    Corrector(EKF *filter) : filter_(filter) {}
    virtual ~Corrector() {}

  protected:
    EKF *filter_;
  };

  template <class ConcreteModel, typename Enabled = void>
  class Corrector_ : public Filter::template Corrector_<ConcreteModel>, public Corrector
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef ConcreteModel Model;
    typedef typename Filter::template Corrector_<ConcreteModel> Base;
    using Filter::template Corrector_<ConcreteModel>::state;

    Corrector_(EKF *filter, Model *model)
      : Base(filter, model)
      , Corrector(filter)
      , y_pred(model->getDimension())
      , error(model->getDimension())
      , C(model->getDimension(), filter->state().getCovarianceDimension())
      , CP(model->getDimension(), filter->state().getCovarianceDimension())
      , S(model->getDimension(), model->getDimension())
      , K(filter->state().getCovarianceDimension(), model->getDimension())
      , update(filter->state().getCovarianceDimension())
    {
      y_pred.setZero();
      error.setZero();
      C.setZero();
      CP.setZero();
      S.setZero();
      K.setZero();
      update.setZero();
    }
    virtual ~Corrector_() {}

    virtual bool correct(const typename ConcreteModel::MeasurementVector& y, const typename ConcreteModel::NoiseVariance& R);
    virtual typename ConcreteModel::MeasurementVector getResidual() const { return error; }

  public:
    typename Model::MeasurementVector y_pred;
    typename Model::MeasurementVector error;
    typename Model::MeasurementMatrix C;
    typename Matrix_<ConcreteModel::MeasurementDimension, Dynamic>::type CP;
    typename Model::NoiseVariance S;
    typename Model::GainMatrix K;
    typename Model::UpdateVector update;
  };

public:
  State::Vector x_diff;
  State::SystemMatrix A;
  State::Covariance Q;
};

} // namespace filter
} // namespace hector_pose_estimation

#include "ekf.inl"

#endif // HECTOR_POSE_ESTIMATION_FILTER_EKF_H