Relocalization algorithm