made connection matrix generation more efficient. fixed a bug where robot pose was being reset when the map was updated.