Step counting algorithm