fkanehiro/hrpsys-base — pull request #296 by eisoku9618 (2014-08-10 21:48:27+00:00)
Title: update KalmanFilter. KF -> EKF and RPY -> Quaternion

https://github.com/fkanehiro/hrpsys-base/issues/295 で書いた

- 内部でRPYを使っている
- 直立姿勢周りでの近似

を直そうと思い，KalmanFilter.cpp/hを

- Quaternionを使う
- ExtendedKalmanFilterにする

というように変えてみました．

これにより， https://github.com/start-jsk/rtmros_tutorials/pull/76 のテストプログラムにて，
```lisp
(main :deflection-ypr-angle (float-vector 0 (/ pi 18) 0) :wrt :local :init-ypr-angle (float-vector 0 (/ pi 2) 0) :time 1500)
```
の動作が
![i](https://cloud.githubusercontent.com/assets/4509039/3870651/ddae2426-20d7-11e4-8751-40f56e928576.png)
だったのが
![ii](https://cloud.githubusercontent.com/assets/4509039/3870653/e3aa6f42-20d7-11e4-8788-0c3ed095b918.png)
になり，

```lisp
(main :deflection-ypr-angle (float-vector 0 (/ pi 6) 0) :wrt :local :init-ypr-angle (float-vector 0 0 (/ -pi 2)) :time 1500)
```
の動作が
![j](https://cloud.githubusercontent.com/assets/4509039/3870656/f0190d42-20d7-11e4-96ff-b7cb2c710c88.png)
だったのが
![jj](https://cloud.githubusercontent.com/assets/4509039/3870657/f526e96c-20d7-11e4-80bb-0f51fe75e3f2.png)
になりました．