Integration of estimator in the control loop + Small changes for PyBullet environement + Figures to test the estimator