Added step detector and Kalman compensation