Time interval correction for step detection algorithm
I managed to solve the problem of the algorithm by using a different approach. Algorithm is not clear on what is happening with the initial values or how to reliably capture a set of peak/valley candidates and how to deal with them.

Therefore, we decided to use only magnitude part of the paper, which radically simplifies the whole algorithm, and in fact works great ( assuming you are getting reasonable sensor readings).