Refactorized IMU code