Fix error using interpolated values instead of real values.
Allow small changes of direction without resetting history