Fix state estimation

@tflayols and I discovered the state estimation was not made correctly. Upon further examination, we discovered the the update of the estimated feet position (i.e. the odometry and possibly the foot drift compensation) is not working. This was not a problem at all when doing tests on spot, such as executing a sinusoid with the CoM, but it causes estimation errors when walking forward.

The most likely explanation is a bug in the implementation, but we were not able to localize it. @tflayols provided a quick fix. This provided a more rigorous state estimation, which is now working quite well. However, this basically discards an important part of the original state estimation scheme, and we think a real solution is badly needed.

I am still investigating the impact of the current fix, but the simulations do not seem to be particularly affected. It is important to confirm the results on the real robot, which I plan to do by Thursday, if the robot is working.