update KalmanFilter. KF -> EKF and RPY -> Quaternion

[eisoku9618]

#295 で書いた

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

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

• Quaternionを使う
• ExtendedKalmanFilterにする

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

これにより， start-jsk/rtmros_tutorials#76 のテストプログラムにて，

(main :deflection-ypr-angle (float-vector 0 (/ pi 18) 0) :wrt :local :init-ypr-angle (float-vector 0 (/ pi 2) 0) :time 1500)

の動作が

だったのが

になり，

(main :deflection-ypr-angle (float-vector 0 (/ pi 6) 0) :wrt :local :init-ypr-angle (float-vector 0 0 (/ -pi 2)) :time 1500)

の動作が

だったのが

になりました．

[k-okada]

Test PASSed.
Refer to this link for build results: http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080job/hrpsys-qnx/1561/

[eisoku9618]

補足ですが，

• 加速度・ジャイロにノイズが乗らないhrpsys-simulatorにて add test program for kf precision start-jsk/rtmros_tutorials#76 のテストコードが良い挙動を示すようなパラメータを設定しました
• RPYしか対応していない＋RPYでないと比較できないので，QuaternionをRPYに変換していますが，状態量はQuaternionとドリフト項です
• 変数名は wikipedia に準拠しています
• 姿勢推定EKFに需要があるか分からなかったのでコードが汚い部分がありますがPRを送りました．レビューしていただければ幸いです．

気づいている問題点

• yawが思ったよりずれてしまう
• VRUやAHRSにはEKFというよりはAdaptive Kalman Filterがよく使われている気がしますが，このPRはEKFです

また， #218 はついでに直してあります．

[eisoku9618]

http://en.wikipedia.org/wiki/Extended_Kalman_filter を元に，

状態方程式は以下

観測方程式は以下

になります．

[k-okada]

趣味もあるけど，こういう変更は，結局何を変更したかわかりづらいので，
ー　中の振る舞いを変えるならその変更のコミットを
ー　インデントなどのフォーマットをかえるならそのコミットを
と分けるのが良いです．