Method and system for registering an object with a probe using entropy-based motion selection and Rao-Blackwellized particle filtering

A system for registering a probe with an object by probing the object with the probe at multiple poses, wherein each pose of the probe includes a location and an orientation, comprising the steps of

• means for representing a probability distribution of a current location of the probe by a set of particles, wherein each particle has a location and has a weight corresponding to a probability of the particle, and for representing a probability distribution of a current orientation of the probe by a Gaussian distribution for each particle conditioned on the location of the particle;
• means for choosing a set of candidate motions;
• means for determining, for each candidate motion, an expected uncertainty based on the set of particles;
• means for selecting the candidate motion with a least expected uncertainty as a next motion of the probe;
• a robot arm configured to move the probe according to the next motion;
• and
• means for updating the set of particles using the next pose of the probe, wherein the registration is in a Rao-Blackwellized particle filtering (RBPF) framework.