First wave of optimizations
First wave finished 0.227697 ms
Second wave of optimizations
Finish second wave without improvement of computation time
Correct the second wave.

The order of object members seems VERY important.
Third wave of optimizations. Measurement dynamics.
improve EKF efficiency
Simplify the interfaces and the observation loop for the observer encapsulation
Remove unused variable
Fourth wave of optimization, the state dynamics is factorized.
Warnings care
Rebase corrections to optimization
Improve the initialization of the contact mode