Embodiments of the invention disclose a system and a method for determining a pose of a probe relative to an object by probing the object with the probe, comprising steps of: determining a probability of the pose using Rao-Blackwellized particle filtering, wherein a probability of a location of the pose is represented by a location of each particle, and a probability of an orientation of the pose is represented by a Gaussian distribution over orientation of each particle conditioned on the location of the particle, wherein the determining is performed for each subsequent probing until the probability of the pose concentrates around a particular pose; and estimating the pose of the probe relative to the object based on the particular pose.