kalmanFilter.hpp

#ifndef KALMANFILTER_HPP_
#define KALMANFILTER_HPP_

#include "rtslam/rtSlam.hpp"
#include "jmath/ixaxpy.hpp"
#include "jmath/ublasExtra.hpp"
#include "rtslam/innovation.hpp"

namespace jafar {
  namespace rtslam {
    using namespace jblas;


    class ExtendedKalmanFilterIndirect {
      private:
        size_t size_; // state size
//        size_t measurementSize;
//        size_t expectationSize;
//        size_t innovationSize;

      private:
        vec x_;
        sym_mat P_;

      #if STATS
      private:
        double sum_innov_;
        double max_innov_;
        unsigned n_innov_;
        bool first_innov_;
      #endif

      public:
        //        boost::posix_time::time_duration curTime;
        mat K;
        mat PJt_tmp;

        ExtendedKalmanFilterIndirect(size_t _size);

        size_t size(){
          return size_;
        }
        jblas::vec & x() {
          return x_;
        }
        jblas::sym_mat & P() {
          return P_;
        }
        double & x(size_t i) {
          return x_(i);
        }
        double & P(size_t i, size_t j) {
          return P_(i, j);
        }

        #if STATS
        double avg_innov() { return sum_innov_ / n_innov_; }
        double max_innov() { return max_innov_; }
        #endif


        void predict(const ind_array & iax, const mat & F_v, const ind_array & iav, const mat & F_u, const sym_mat & U);

        void predict(const ind_array & iax, const mat & F_v, const ind_array & iav, const sym_mat & Q);

        void initialize(const ind_array & iax, const mat & G_rs, const ind_array & ia_rs, const ind_array & ia_l, const mat & G_y, const sym_mat & R);

        void initialize(const ind_array & iax, const mat & G_v, const ind_array & ia_rs, const ind_array & ia_l, const mat & G_y, const sym_mat & R, const mat & G_n, const sym_mat & N);

        void reparametrize(const ind_array & iax, const mat & J_l, const ind_array & ia_old, const ind_array & ia_new);

        void computeKalmanGain(const ind_array & ia_x, Innovation & inn, const mat & INN_rsl, const ind_array & ia_rsl);

        void correct(const ind_array & iax, Innovation & inn, const mat & INN_rsl, const ind_array & ia_rsl, bool correct_P = true);

      protected:
        
        vec stackedInnovation_x;
        sym_mat stackedInnovation_P;
        sym_mat stackedInnovation_iP;

        struct StackedCorrection
        {
          StackedCorrection(Innovation & inn, const mat & INN_rsl, const ind_array & ia_rsl):
            inn(inn), INN_rsl(INN_rsl), ia_rsl(ia_rsl)
          {
// JFR_DEBUG("StackedCorrection " << this->INN_rsl << " " << INN_rsl);
          }
          Innovation inn;
          mat INN_rsl;
          ind_array ia_rsl;
        };

        typedef std::list<StackedCorrection> CorrectionList;

        struct CorrectionStack
        {
          CorrectionList stack;
          size_t inn_size;
          
          CorrectionStack() { clear(); }
          void clear() { inn_size = 0; stack.clear(); }
        };
        CorrectionStack corrStack;
        
      public:
        void stackCorrection(Innovation & inn, const mat & INN_rsl, const ind_array & ia_rsl);
        void correctAllStacked(const ind_array & iax, bool correct_P = true);
        void clearStack();

    };

  }
}

#endif /* KALMANFILTER_HPP_ */