Accurate Covariance Estimation For Pose Data From Iterative Closest Point Algorithm

One of the fundamental problems of robotics and navigation is the estimation of relative pose of an external object with respect to the observer. A common method for computing the relative pose is the Iterative Closest Point (ICP) algorithm, where a reference point cloud of a known object is registered against a sensed point cloud to determine relative pose. To use this computed pose information in down-stream processing algorithms, it is necessary to estimate the uncertainty of the ICP output, typically represented as a covariance matrix. In this paper we introduce a novel method for estimating uncertainty from sensed data. Using a visual simulation of an automated aerial refueling (AAR) task, we demonstrate significantly more accurate uncertainty estimates using our proposed approach than a naive "Jacobian" method.