Add possibility to use a Kalman filter instead of a complementary filter (still need tuning) + Rework complementary filter to take into account the lever arm between the center of the base and the IMU