#ifndef __TVE_CORE_H__
#define __TVE_CORE_H__


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



/****************************************
 *
 * TrueValueEstimatorCore
 * 静止判定とオフセットを推定する
 *
 ****************************************/

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

  void init(double dt);

  void convert(const ColumnVector &accs, const ColumnVector &gyros,
	       ColumnVector &avel, bool &isStatic);

 private:
  bool evalIsStatic();
  ColumnVector getEstAvel();
  void updateOffset();


  double dt_;
  ColumnVector avel_, avel_offset_, in_gyros_;
  ColumnVector accs_, lp_accs_;
  bool isStatic_;

  BWF lp_bwf_[3];

  Matrix Cov_; /* avel_offsetの共分散行列 */

};


#endif
