Fixed some problems with the mapping, though I still have problems with obstacles; they are currently lost from the map after some time