Maximum Likelihood Attitude and Position Estimation from Pseudo-Range Measurements using Geometric Descent Optimization

This paper addresses the problem of estimating the attitude and the position of a rigid body when the available measurements consist only of pseudo-ranges between a set of body fixed beacons and a set of earth fixed landmarks. To this effect, a maximum likelihood (ML) estimator is formulated as an optimization problem on the parameter space Theta = SE(3) times Ropf^p corresponding to the attitude and position of the rigid body as well as a set of biases present in the pseudo-range equations. Borrowing tools from optimization on Riemannian manifolds, intrinsic gradient and Newton-like algorithms are derived to solve the problem. The rigorous mathematical setup adopted makes the algorithms conceptually simple and elegant; furthermore, the algorithms do not require the artificial normalization procedures that are recurrent in other estimation schemes formulated in Euclidean space. Supported by recent results on performance bounds for estimators on Riemannian manifolds, the intrinsic variance lower bound (IVLB) is derived for the problem at hand. Simulation results are presented to illustrate the estimator performance and to validate the tightness of the IVLB in a wide range of signal to noise ratio scenarios