On the closed-form covariance estimation for the pIC algorithm

A number of existing methods for estimating the displacement of a robotic vehicle incorporates scan matching algorithms. There are many different scan matching algorithms but only few of them base their estimation on a probabilistic framework. To correctly integrate the scan matching estimate in a Simultaneous Localization And Mapping (SLAM) algorithm under Gaussian assumption, it is needed to know not only the displacement estimation but also its covariance. Robust methods of estimating the covariance are based on the analysis of the cost function being minimized and are independent of the minimization algorithm. In this paper, we study those methods and properly apply them in a closed form to the Probabilistic Iterative Correspondence (pIC) scan matching algorithm. The pIC is the base algorithm used in a previous work where the authors had proposed a pose-based algorithm to solve the full SLAM problem for an Autonomous Underwater Vehicle (AUV), navigating in an unknown and possibly unstructured underwater environment. This approach is verified with Monte Carlo simulations and numerical calculations.