Fixed imu pose, still to be tested on real robot