By the way, after you get it working, maybe it's best to work with time_frame instead of time step because that will give you the exact elapsed time from the previous frame, not a smoothed time. I think it may prevent some jitter if the fremerate suddenly changes.

Anyway, this is just dry talk. Like the equation, I did not test it.

Let me know how it works for you.