Update around kf

[eisoku9618]

update時に

• roll：accから求めたroll
• pitch：accから求めたpitch
• yaw：0.0

だったので，#550 となってしまっていたところを，

• roll：accから求めたroll
• pitch：accから求めたpitch
• yaw：IMUがついているリンクのyaw

にしました．

[eisoku9618]

腰にIMUが付いているロボットで

PR前

だと，回転しているのに回転していないことになっていて

PR後

だと，しっかりと回転しました

[k-okada]

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

[k-okada]

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

[snozawa]

@eisoku9618
これのサンプルプログラム、テストプログラムってあります？
なんでも良いのですが、例えば
　この変更の前だと動かず、変更後だと動く
プログラムとかがあるとよさそうです。

[eisoku9618]

これのサンプルプログラム、テストプログラムってあります？

加えた変更は

1. このPR：従来のIMU座標のrpyの推定で，加速度のみからyawを求めるときに0としていた部分を，FKを使った値にした
2. output the body frame relative to a world reference frame as baseRpyCurr... #586 のPR：従来のkfがstに出していたrpyポートはそのままにした上で，baseRpyCurrentというポートを追加した

で，テストは

#582 (comment)

や

https://github.com/fkanehiro/hrpsys-base/pull/582/files#diff-09092c2f0677809ee35e50a1e5c56a8bR89 をもとに戻した場合

https://github.com/fkanehiro/hrpsys-base/pull/582/files#diff-09092c2f0677809ee35e50a1e5c56a8bR89 をもとに戻さずこのPRのようにした場合

になります．

これとは別に，サンプルロボットのIMUを腰に移動させてもらいましたが，https://github.com/fkanehiro/openhrp3/pull/56，

上の2つのグラフを比較することで，IMUが腰に付いている場合に腰を回すとyawがおかしくなっているのが治っていることがわかると思います．