Adding the position tracking

This adds position tracking capabilities AND obstacle avoidance and
stuff