Add COM Kalman Filter fusing kinematics, CoP and IMU measurements

[prashanthr05]

During some walking experiments, it was observed that the DCM velocity (Divergent Component of Motion) suffered from sudden spikes causing the walking controller to fail. This quantity is computed using the Center of Mass velocity which in turn in measured using robot momenta using kinematics. This boils down to computing link velocities using the joint velocity measurements which are usually noisy.
We may improve the CoM velocity estimation by either avoiding to use the joint velocities through a fusion of forward kinematics, Center of Pressure (CoP) and IMU or a complete fusion including, assuming a simplified model for COM evolution and that the IMU lies close to the CoM of the robot in most cases.
The former choice is the approach taken in the work "Center of Mass Estimator for Humanoids and its Application in Modelling Error Compensation, Fall Detection and Prevention, Xinjilefu et al".

In this PR, we implement this estimator for estimating the center of mass position, velocity and offset by fusing IMU and kinematics measurements with a constant COM height LIPM model.

Major assumptions,

• constant height CoM LIPM model
• CoM is always close to the same rigid body as the IMU and the velocity of the CoM wrt IMU is approximated to zero
• constant angular velocity is assumed, making angular acceleration is zero (approximation used in transforming IMU acceleration to COM acceleration)

cc @GiulioRomualdi

[GiulioRomualdi]

Hi @prashanthr05, first of all, thank you for the contribution 😄 .

Is the PR ready to be reviewed?

[prashanthr05]

Hi @prashanthr05, first of all, thank you for the contribution .

Is the PR ready to be reviewed?

A suitable test is pending.

[GiulioRomualdi]

This could be interesting for @G-Cervettini