obstacle avoidance updates

* updated ultrasonic filter
* refactored avoidance flow
* update GPS points