Improve the interface of the observer bey enabling to get measurements and inputs
Huge bug fixing, the flexibility NOW turns around the contact position
Add an assert to check measurement vector input.
Put kinematics algorithms in a specific namespace
Add state and homogeneous matrix inversion and update the namespace using