fixed ConcurrentModificationException in garbage collector when trying to remove elements while iterating over the list
fixed handling of prior poses when reading robot pose information