Optimal Pose Estimation and Covariance Analysis with Simultaneous Localization and Mapping Applications

This work provides a theoretical analysis for optimally solving the pose estimation problem using total-leastsquares for vector observations from landmark features, which is central to applications involving simultaneous localization and mapping. First, the optimization process is formulated with observation vectors extracted from pointcloud features. Then, error-covariance expressions are derived. The attitude and position estimates obtained via the derived optimization process are proven to reach the bounds defined by the Cramér–Rao lower bound under the small-angle approximation of attitude errors. A fully populated observation noise-covariance matrix is assumed as the weight in the cost function to cover the most general case of the sensor uncertainty. This includes more generic correlations in the errors than previous cases involving an isotropic noise assumption. The proposed solution is verified using Monte Carlo simulations, a Gazebo simulation in a robotics operating system, and an experiment with an actual light detection and ranging sensor to validate the error-covariance analysis.