Important fix.Also added line to set gyro drifting velocity to zero.