[KinematicInertial] Fix use of state-observation

[arntanguy]

This PR fixes the mistakes introduced by #388 (I have temporarely reverted it on master with d4de8d3) During walking I could see no differences checking the results of the old versus new method.

However, while perturbing the robot manually (dragging it around in choreonoid), there are still slight differences. For example (displayed as RPY angles)

[info] New:   0.22885 -0.218569    2.3249
[info] Old:  0.234254 -0.212752   2.29964
[info] New:  0.231229 -0.220776   2.32437
[info] Old:  0.236799 -0.214775   2.29859
[info] New:  0.233628 -0.223039   2.32383
[info] Old:   0.23937 -0.216849   2.29751
[info] New:  0.236055 -0.225358   2.32327
[info] Old:  0.241975 -0.218971    2.2964
[info] New:  0.238506 -0.227731    2.3227
[info] Old:  0.244613 -0.221141   2.29526

@mehdi-benallegue Could you confirm that this is indeed correct?

[gergondet]

Hi @arntanguy

I have made some tests and found the correct version to call in state-observation (the one was that was copy-pasted from the state-observation repo back then) although it required a fix in state-observation https://github.com/jrl-umi3218/state-observation/commit/ba303180559df1277308eff4c47c3e79b16207f6

Will merge when the CI lands