/**************************************************
 *
 *         TransEstimatorCore.h
 *
 *
 **************************************************/


#ifndef __TRANS_ESTIMATOR_H__
#define __TRANS_ESTIMATOR_H__

#include <octave/config.h>
#include <octave/Matrix.h>
#include <math.h>
#include <iostream>
#include <octave_rtmex/octave_rtmex.h>
#include <BWF/bwf.h>


using namespace rtm;

class TECore {
 public:
  TECore();
  ~TECore();
  TECore(double dt);

  void init(double dt);
  /* オドメトリ有 */
  void convert(const ColumnVector &accs, const ColumnVector &quat,
	       const ColumnVector &ref, const ColumnVector &ref_var,
	       const bool &isStatic, ColumnVector &tvel);  

  /* オドメトリ無 */
  void convert(const ColumnVector &accs, const ColumnVector &quat,
	       const bool &isStatic, ColumnVector &tvel);

  void resetTransVelociy();

 private:
  double dt_;
  ColumnVector accs_;
  ColumnVector waccs_;
  ColumnVector tvel_;
  ColumnVector ref_vel_, ref_var_;
  quaternion quat_;
  bool isStatic_;

  /* kalman filter */
 public:
  void setQ(const Matrix &Q){ Q_ = Q;}
  Matrix getQ(){ return Q_;}

 private:
  ColumnVector initKalman();
  ColumnVector updateKalmanFilter(const ColumnVector &U, const ColumnVector &Z);
  ColumnVector g_func(const ColumnVector &X, const ColumnVector &U);
  ColumnVector h_func(const ColumnVector &X);

  ColumnVector U_, X_, Xh_;
  ColumnVector Z_, Zh_, d_Z_;
  Matrix G_, H_;
  Matrix K_, R_, Q_, S_, Sh_;

};

#endif
