IMU implemented. Tested out of the robot. some tests on real environment should be performed