Total Least Squares for Optimal Pose Estimation

TL;DR

This paper introduces a theoretical framework for pose estimation using total least squares, deriving error bounds and validating the approach through simulations with realistic sensor noise models.

Contribution

It extends previous methods by incorporating more general error correlations and provides a comprehensive covariance analysis for pose estimation.

Findings

Attains Cramér-Rao lower bound under small-angle approximation.

Incorporates full error covariance including correlations.

Validated through Monte Carlo simulations.

Abstract

This work provides a theoretical framework for the pose estimation problem using total least squares for vector observations from landmark features. First, the optimization framework is formulated with observation vectors extracted from point cloud features. Then, error-covariance expressions are derived. The attitude and position solutions obtained via the derived optimization framework are proven to reach the bounds defined by the Cram\'er-Rao lower bound under the small-angle approximation of attitude errors. The measurement data for the simulation of this problem is provided through a series of vector observation scans, and 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. Here, previous derivations are expanded for the pose estimation problem to include more generic correlations in the errors than previous cases involving an isotropic noise assumption. The proposed solution is simulated in a Monte-Carlo framework to validate the error-covariance analysis.