Integrate odometry position even with very small intervals

The small time interval is not a problem when we integrate. It is only a problem for velocity estimation. I propose we only skip this last part when/if the time interval is very small.