Fixed twist, debug, added initial IMU data