Fixed stabilization, added reachability check to SE2BeleiefSpace