Analytical 3D rotation estimation using vector measurements with full variance-covariance matrix

3D rotation estimation using vector measurements is investigated. A general stochastic model is employed in which no specific structure is assumed about the variance covariance matrix of the measurement errors, or in other words, different elements of the same vector, and/or different vectors can have different variances and can be arbitrarily correlated. The rotation matrix is estimated in two steps both of which are analytical. First, a free matrix, not necessarily special orthogonal, is estimated optimally in the weighted least-squares sense. In this step, the special case with only 2 non-collinear vector measurements is also treated. Second, a special orthogonal matrix is estimated which is closest to the previously estimated free matrix in the minimum Frobenius norm sense. Error analysis is performed, and the variance covariance matrix of the rotation estimation error is derived. Simulation is conducted and the statistical efficiency and consistency of the proposed method are validated.