Add quaternion kf test

[eisoku9618]

#955 (comment) の @k-okada 先生のコメントを受けて，Quaternionを使った姿勢推定のテストコードを加えました．

RPYを使った姿勢推定と比較して特異点付近でも姿勢推定を行えていることを確認できるようになりました．

• test1 : Quaternionバージョン
  
• test1 : RPYバージョン
  
• test2 : Quaternionバージョン
  
• test2 : RPYバージョン
  

[eisoku9618]

以下のような動きです．

[eisoku9618]

@snozawa さん，ロボットの初期姿勢を0 0 0ではなくしたかったので，
このPRに含まれる以下のコミットで初期姿勢を85度回転するように変更してしまいました（xmlファイルを書き換えまいた）が，大丈夫でしょうか？
それに応じて，これまであったテストプログラムが影響無いようにこれまでのテストプログラムでは逆に85度回転するようにしています．．．

• eisoku9618@42cab97

（初期姿勢が変なバージョンのlaunchとxmlだけを新たに作った方がよろしいでしょうか？）

[k-okada]

Refer to this link for build results (access rights to CI server needed):
https://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/hrpsys-qnx/2567/
Test PASSed.