Opimal Estimation of a Rigid Body Pose

A typical movement analysis problem is the estimation, at every time instant, of the position vector k and of the orientation matrix R describing the pose of a segment, supposed rigid, based on the position measurement of N points embedded with the segment. The orthonormal matrix R and the k-vector have to satisfy the following co-ordinate transformation pi,true=R ai,true + k where pi,true and ai,true are the error-free i-th point co-ordinates in the fixed and moving frame, respectively. Defining as l=[a1T p1 T,…,aN T pN T] T the vector of observations affected by additive white noise, as the optimal estimate of l, and as Q-1 a suitable weight matrix, in this paper, the optimal estimate of R, k, (and l) is obtained considering both pi and ai as noisy observations by iteratively minimization, in the least squares sense, of the following functional = (  l )TQ-1(  l ), looking for the solution that satisfies the co-ordinate transformation. At every iteration, adjustment is performed both on parameters (R and k) and on observations. Results are compared with those obtained by Singular Value Decomposition (SVD) [1] and by the method proposed in [2].