start-jsk/rtmros_tutorials — pull request #76 by eisoku9618 (2014-08-10 21:30:30+00:00)
Title: add test program for kf precision

hrpsys-baseの姿勢推定コンポーネントの精度を評価するテストプログラムを書きました．

![image](https://cloud.githubusercontent.com/assets/4509039/3870551/3865fda2-20d4-11e4-9776-1d973eaa3d0b.png)

```bash
roscd hrpsys_ros_bridge_tutorials/test/test-kf/
roseus
```

```lisp
(load "check-kf-precision.l")
(main :deflection-ypr-angle (float-vector 0 (/ pi 6) 0) :wrt :world :init-ypr-angle (float-vector 0 0 0))
```
で下記のプロットが出ます．

![a](https://cloud.githubusercontent.com/assets/4509039/3870552/49de028c-20d4-11e4-9e1f-76a476038c7f.png)

このロボットはIMUがend-coordsについているようなので，

画像の左の列：kf_rpyが出しているRPYとルートリンクを地面に固定してForwardKinematicsで求めたIMUのRPY
画像の右の上・真ん中：IMUが出力している加速度・ジャイロ
画像の右下：end-coordsの位置

をプロットしています．

```lisp
(load "check-kf-precision.l")
(main :deflection-ypr-angle (float-vector 0 (/ pi 6) 0) :wrt :world :init-ypr-angle (float-vector 0 0 0))
```

では，初期姿勢を ```init-ypr-angle``` で与え，
そこから目標姿勢 ```deflection-ypr-angle``` までIMUの位置はずらさずに振り子のような形で動くようなIKを解いています．