Add IMU init and reset function

- This is IMU initialization is required due to the hardware change
- Not Implemented in firmware