/*************************************************
 *                   estimate_core.cpp
 *
 * 
 * written by T.Goto
 *************************************************/

#ifndef _EKF_COORDS_H_
#define _EKF_COORDS_H_

#include <octave/config.h>
#include <octave/Matrix.h>
#include <math.h>
#include <iostream>
#include <vector>
#include <utils/UtilTimer.h>
#include <octave_rtmex/octave_rtmex.h>


//define
#ifndef PI
#define PI 3.141592653589793238462643383279
#endif


using namespace std;

namespace rtm{
  class EKFCoords{
  public:
    EKFCoords();
    EKFCoords(double dt);
    ~EKFCoords();

    void init(double dt);
    void updateFeatures(const vector<int> &rmlist, const vector<ColumnVector> &new_lposs);

    ColumnVector estZofPos(int type);
    void setHofPos(int type);
    ColumnVector estZofQuat();
    void setHofQuat();
    ColumnVector estZofVel();
    void setHofVel();
    ColumnVector estZofAVel();
    void setHofAVel();
    ColumnVector estZofPoint(int fidx);
    void setHofPoint(int fidx);

    void setQ(const Matrix &Q);


    Matrix makeNewPointCov(const ColumnVector &lpos);
    Matrix makePointWorldCovfromStereo(const ColumnVector &lpos);
    Matrix makeStereoErrorCov(const ColumnVector &pos);
    Matrix makePointCovfromQuat(const ColumnVector &lpos);


    Matrix dpQuatbyAVel(const ColumnVector &quat);
    Matrix dpPointbyPoint(const ColumnVector &wpos, const ColumnVector &co);
    Matrix dpPointbyCoords(const ColumnVector &wpos, const ColumnVector &co);
    void predict();
    void update(const ColumnVector &Z);
    void predictX();
    void setG();
    void setR();
    void evalEKF();

  private:
    /******** 変数 ********/
    double dt_; // 周期
    int fn_;    // 特徴点数
    int sdim_;  // 状態変数の数
    coords estco_;

    Matrix G_; //状態方程式ヤコビアン
    Matrix H_; //観測方程式ヤコビアン
    Matrix R_; //状態ノイズ共分散
    Matrix Q_; //誤差分散
    Matrix S_; //状態共分散
    Matrix Sh_;//状態共分散予測
    Matrix K_; //カルマンフィルタ

    ColumnVector X_;  //状態項
    ColumnVector Xh_; //状態項予測
    ColumnVector Z_;  //観測項
    ColumnVector Zh_; //観測項予測
    ColumnVector U_;  //入力項
    ColumnVector d_Z_;//予想と観測の差

    UtilTimer utimer;

  public:
    void setX(const ColumnVector &X){X_ = X;}
    ColumnVector getX(){return X_;}

    void setU(const ColumnVector &U){U_ = U;}
    ColumnVector getU(){return U_;}

    void setS(const Matrix &S){S_ = S;}
    Matrix getS(){return S_;}

  };
};

#endif


