Improved the tracking. Now it is just done at obstacle level.