iri_poseslam fork to make it work for REEM robot