[fix] Fix GetLastStates

SLAM crashed in GetLastStates function if going back in time to some previous frame.

Fix it by giving a limit of time for interpolation and make it work in reverse order.