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

TL;DR

This paper presents a theoretical framework for optimal pose estimation in SLAM using total least squares, deriving error-covariance expressions and demonstrating that estimates reach the Cramér-Rao bound under small-angle assumptions.

Contribution

It introduces a comprehensive covariance analysis for pose estimation using vector observations with a general noise covariance matrix, extending previous isotropic noise models.

Findings

Estimates reach the Cramér-Rao lower bound under small-angle approximation.

Derived error-covariance expressions for pose estimates.

Validated the theoretical analysis with Monte Carlo simulations and real LIDAR data.

Abstract

This work provides a theoretical analysis for optimally solving the pose estimation problem using total least squares 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 point-cloud 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\'er-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 and an experiment with an actual LIDAR to validate the error-covariance analysis.