=============================================================================
自己姿勢推定コンポーネント version 1.0 2011/08/25
東京大学　情報理工学系研究科　情報システム工学室　次世代プロジェクト担当
Mail: itngr@jsk.t.u-tokyo.ac.jp
=============================================================================

-----概要-----

モーションセンサ(IMU)のデータを元に，拡張カルマンフィルタを用いて，
自己位置・姿勢を推定するコンポーネントです．

-----コンポーネントの使い方-----

-- confファイルの設定
以下の説明に従って，必要に応じてNxWMSReaderTest.confの内容を書き換えます．
   NxWMSReader0.frequency: 100
    1秒間における実行ループの回数．
   NxWMSReader0.inputpackets: 5
    一度に送るデータ量．センサの仕様上5にしておく

-- コンパイル
../idlと../functionsでmakeした後，このディレクトリでmakeを行ってください．
以下のようなファイルが生成されます．
1. ./build/bin/NxMotionEstimatorComp
2. ./build/lib/NxMotionEstimator.so
3. ./build/bin/NxMotionEstimatorTest.conf (このディレクトリにあるファイルのコピー)
4. ./build/bin/rtc.conf (上位ディレクトリにあるファイルのコピー)

-- 起動方法
例えば以下のように実行できます．
% cd ./build/bin
% ./NxMotionEstimatorComp
RTSystemEditorなどでActivateを行い，動作を実行できます．

-----コンポーネントの入出力-----

-- InPort

*TimedMotion3DSensorData in_sensor

センサから得られた加速度・角速度・地磁気の3次元の値を受けとります．

TimedMotion3DSensorDataは，

============================================================

  struct ThreeAxisData
  {
    double x;
    double y;
    double z;
  };

  struct Motion3DSensorData
  {
    ThreeAxisData accs;
    ThreeAxisData gyros;
    ThreeAxisData mags;
  };

  typedef sequence<Motion3DSensorData> Motion3DSensorDataList;

  struct TimedMotion3DSensorData
  {
    // Time stamp.
    RTC::Time tm;
    // Sequence of motion sensor data.
    Motion3DSensorDataList motion3dsensors;
  };

============================================================

のように定義されております．

-- OutPort

*TimedEstimatedMotionData out_result

推定した自己位置(x,y,z)と自己姿勢クオータニオン(q1,q2,q3,q4)を
出力します．

TimedEstimatedMotionDataは，

============================================================

  struct ThreeAxisData
  {
    double x;
    double y;
    double z;
  };

  struct Quaternion
  {
    double q1;
    double q2;
    double q3;
    double q4;
  };
  
  struct Motion3DSensorData
  {
    ThreeAxisData accs;
    ThreeAxisData gyros;
    ThreeAxisData mags;    
  };

  typedef double Mat44[4][4];

  typedef sequence<Motion3DSensorData> Motion3DSensorDataList;

  struct TimedMotion3DSensorData
  {
    // Time stamp.
    RTC::Time tm;
    // Sequence of motion sensor data.
    Motion3DSensorDataList motion3dsensors;
  };

  struct EstimatedMotionData
  {
    ThreeAxisData accel;
    ThreeAxisData angularvel;
    ThreeAxisData translationalvel;
    Quaternion quat;
    Mat44 quatcov;
    short isstatic;
  };

  struct TimedEstimatedMotionData
  {
    // Time stamp.
    RTC::Time tm;
    EstimatedMotionData estimatedmotion;
  };  

============================================================

のように定義されております。

-----ライセンス-----

本ソフトウェアは new BSD License に基づき配布されます．
詳しくは../LICENSE.txtファイルをご覧ください．
