Optimal pose estimation with error-covariance analysis

This paper presents a new formulation for the pose estimation problem, which involves estimating the attitude and position from vector-pair observations. It is shown that the optimal pose estimation problem is derived from a total least squares formulation. In this paper, the most general case where the covariances of the measurement errors may be fully populated matrices is considered. Then, the case where isotropic measurement errors is considered, which is related to Wahba’s problem. In both cases, it is possible to derive a loss function in terms of the unknown attitude only. The position can be determined from the estimated attitude. Error-covariances expressions for the attitude and position are derived for the isotropic-measurement case. Estimates for the vector observations, along with their respective covariances are also derived. Simulation results show that the derived covariance expressions are consistent with Monte Carlo runs.