Handle discontinuities

[andreadelprete]

We noticed that changes in the number of contacts can result in discontinuities in the floating-base state estimation. This will be a crucial issue to implement walking on HRP-2, and we need to find a way to avoid it.

[andreadelprete]

After some thoughts, it seems to me that the right way to handle this issue is to modify online the covariance matrix of the F/T sensor measurements. My idea is to define a range of values for the normal force (i.e. from 100 N to 10 N) in which the associated F/T measurement is used less and less by the estimator. This can be achieved by simply increasing the associated covariance matrix. Then, when the normal force reaches the lower bound of the interval, the contact is completely removed, but at this point it shouldn't result in a discontinuity.
@amifsud @thomasfla

[andreadelprete]

Since we plan to stop using the flexibility estimator (until a few issues haven't been solved) this issue is no longer urgent.