Added imu, wrong pose but just for testing