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

Accepted Manuscript Analytical 3D rotation estimation using vector measurements with full variancecovariance matrix Tianhe Xu, Guobin Chang, Qianxin Wang, Chao Hu PII: DOI: Reference:

S0263-2241(16)30682-0 http://dx.doi.org/10.1016/j.measurement.2016.11.037 MEASUR 4458

To appear in:

Measurement

Received Date: Revised Date: Accepted Date:

6 April 2016 12 November 2016 22 November 2016

Please cite this article as: T. Xu, G. Chang, Q. Wang, C. Hu, Analytical 3D rotation estimation using vector measurements with full variance-covariance matrix, Measurement (2016), doi: http://dx.doi.org/10.1016/ j.measurement.2016.11.037

This is a PDF file of an unedited manuscript that has been accepted for publication. As a service to our customers we are providing this early version of the manuscript. The manuscript will undergo copyediting, typesetting, and review of the resulting proof before it is published in its final form. Please note that during the production process errors may be discovered which could affect the content, and all legal disclaimers that apply to the journal pertain.

Analytical 3D rotation estimation using vector measurements with full variance-covariance matrix Tianhe Xua,c, Guobin Changb,c,*, Qianxin Wangb,c, Chao Hub a b

Institute of Space Science, Shandong University, Weihai 264209, China School of Environmental Science and Spatial Informatics, China University of Mining and

Technology, Xuzhou 221116, China c

State Key Laboratory of Geo-information Engineering, Xi’an Research Institute of Surveying and

Mapping, Xi’an 710054, China * Corresponding author at: School of Environmental Science and Spatial Informatics, China University of Mining and Technology, Xuzhou 221116, China E-mail address: [email protected] (G. Chang).

Abstract: 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 measuremetns 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. Keywords: 3D rotation estimation; weighted least-squares; special orthogonal matrix; vector measurements; error analysis. 1 Introduction 3D rotation estimation is essential to a wide variety of applications, such as satellite attitude estimation [1-4], navigation or guidance [5-9], sensor calibration [10-13], equipment alignment [14-19], computer vision [20-22], photogrammetry [23-25], geodetic coordinate transformation [26-28], plate tectonics [29, 30], etc. The rotation is often represented by a special orthogonal 3×3 matrix [31], though some other representations, e.g., unit quaternion, Euler angles, can also be adopted [32, 33]. 3D vector measurements are often used to estimate the rotation parameter. These vector measurements can be the measured 3D coordinates, the gyro-triad measured angular rates, the accelerometer-triad measured accelerations, the magnetometer measured geomagnetic forces, the sightline direction vectors measured by sun/star trackers, or vectors constructed from other measurements [34]. There are in general two main categories of method to estimate the 3D rotation, i.e., the static and dynamic methods [35]. In the static method, the 3D rotation at an instant or epoch is estimated using the measurements obtained at this instant only, and hence no historical information is incorporated [36]. In the dynamic method, the kinematic and/or dynamic models of the 3D rotation are used to take historical information into account in the current-epoch attitude estimation. The Kalman filter (KF) is widely employed in this type of method [37-39]. In this study, only the static method is studied. Some closed-form or analytical solutions to the rotation estimation problem have been proposed

but usually for measurement error variance covariance matrix (VCM) with some specific structures, see e.g., [2, 26, 27, 40-42]. Rotation estimation with more general covariance structures, e.g., the inhomogeneous/heteroscedastic and/or anisotropic ones, has also been investigated, however often iterative solutions are needed [22]. In this contribution, a closed-form solution to the rotation estimation problem with fully general VCM structure is constructed where the “fully general” means that different elements in the same or different vectors can have different variances and can be arbitrarily correlated. Due to the full generality, any specific structure can be safely treated as a special case of our problem, and hence can be solved using our method. Errors with full VCM have wide practical background. In many situations, the vector measurements are not raw ones which can be reasonably assumed of equal accuracies and uncorrelated, rather, they are calculated from other measurements. In a star tracker, the (unit) vector measurements are calculated using the 2 image coordinates according to the collinearity equation [43]. In GNSS positioning, the 3×1 coordinates vector is calculated using the pseudo ranges or carrier phase signals [44]. In GNSS attitude determination, the already correlated differenced carrier phase measurements [45] may further be transformed to vector measurements [34, 46]. Apparently, un-equal accuracies and correlations emerge inevitably in all these processings of the raw measurements. In the Helmert transformation, it is in fact the relative coordinates with respect to the barycenter that are directly used to estimate the rotation parameters, apparently, the centralization process would also introduce correlations [26]. The analytical solution presented in this contribution, compared to the iterative ones, has its own theoretical and practical benefits, as, e.g., no initial guess is needed, the unique analytical solution must be globally optimal (in its own sense) avoiding the problem of divergence, the computational burden is fixed and predictable though it may not always be less than that of the iterative ones [42]. Though sub-optimal, the proposed method is statistically efficient and consistent which has been validated by a simulation example. Besides the merits in its own, the developed analytical solution in this contribution can provide initial guess to start an iterative algorithm to progressively improve the solution. This is indeed feasible and maybe necessary in certain situations, because there is still space for improving the yet non-optimal or sub-optimal solution. An initial guess of relatively good quality is of significant importance for an iterative algorithm, because it can speed up the convergence, and in some situations, even bring the convergence into being. The remaining of the paper is organized as follows. In Section 2, after formulating the problem, we estimate a free matrix optimally in the weighted least-squares sense, free matrix implies that we ignore the special orthogonal property of the matrix in this stage. The degenerate case with only 2 vector measurement is also treated. Then a special orthogonal matrix is extracted from the estimated free matrix, more specifically, the special orthogonal matrix is chosen to be closest to the free matrix in the Frobenius-norm sense. In Section 3, error analysis of the rotation estimation is performed. Due to the special orthogonal property, there are only 3 freedoms in the 9-element matrix, so a 3×1 vector is adopted to represent the estimation error, the covariance of the vector is derived in detail. In Section 4, a simulation example is designed which validates the statistical efficiency and consistency of the proposed method. 2 Rotation matrix estimation Problem formulation, free matrix estimation, degenerate case treatment, and special orthogonal matrix extraction are presented in the four subsections respectively. 2.1 Problem formulation

In the 3D space we have

wi  Rvi , i  1, 2,

(1)

,n

th

th

where wi is the i 3×1 measurement true vector, vi is the i 3×1 reference vector, R is the 3×3 rotation matrix. wi can be measured with errors, as follows. wi  wi + i

(2)

 w1  w     2      wn 

(3)

 1       2      n 

(4)

r  vec  R

(5)

Substitute (2) into (1), and define

V  v1 v2

vn 

(6)

where vec[R] denotes the vectorization of the matrix R formed by stacking the columns of R into a single column vector. We have

  V T  I3  r  

(7)

where  denotes the Kronecker product, I 3 denotes the 3×3 identity matrix. Assume the combined measurement error vector defined in (4) have a full VCM, as follows.

P  P  cov    cov  

(8)

Note that as a 3n×3n VCM, P should be symmetric and positive semi-definite. This is a rather general assumption. Different elements in the same vector can have different variances, resulting in the so called anisotropy, and different vectors can have different covariances, resulting in the so called inhomogeneity or heteroscedasticity, and any two elements in the same or different measurement vectors can be arbitrarily correlated. The rotation estimation problem is stated as follows, Given the reference vectors vi and the measurement vectors wi , and the VCM of the combined measurement error vector  , i.e., P , a matrix R is estimated subject to the following constraints.

RT R  RRT  I3

(9)

det  R  1

(10)

where det[R] denotes the determinant of R. Constraints on R as in (9) and (10) tell that R is a special orthogonal matrix. 2.2 Free matrix estimation Minimizing the weighted least-squares of the estimated (combined) error vector is a natural object in estimating a parameter, so we define the following loss function, also known as the Mahalanobis

distance [47],

    V T  I3  r  P1   V T  I3  r  T

(11)

Note here, weighted least-squares method does not need any specific assumption on the components of measurement error, for instance, the bias or the noise [48]. However, if it is zero-mean and Gaussianly distributed, the weighted least-squares solution is also the maximum likelihood solution [49]. In fact, in order to estimate r and hence R optimally, a constrained minimization problem should be formulated to incorporate the constraints as in (9) and (10) into the loss function in (11) through the Lagrange multipliers technique [50]. However, due to the general P matrix, we fail to solve this constrained minimization problem analytically. So the rotation matrix is estimated in a sub-optimal manner. More specifically, R is estimated in two steps, both of which are optimal in their own manner. First, a free matrix without any constraints is estimated to minimize (11). Second, a special orthogonal matrix, i.e., satisfying (9) and (10), and being closest to the estimated free matrix in the first step, is estimated. The first step is firstly derived in this subsection and the second is treated latter in the sequel. Note that similar stepwise sub-optimal methods can be found in many engineering problems due to its practical usefulness, see e.g., the coordinate transformation [51, 52], the range-based positioning [53]. According to the first order necessary condition for the minimization of (11), we have   2 V  I3  P1  2 V  I3  P1 V T  I3  r  0 r

(12)

So we have

rˆ  V  I3  P1 V T  I3 

1

V  I3  P1

(13)

Prrˆˆ  cov  rˆ   V  I 3  P1 V T  I 3  

1

(14)

which are exactly the results of the classic weighted least-squares theory [54]. Note here that as a nine-parameter vector r is to be estimated, at least 3 non-coplanar vectors have to be measured. This statement has also been pointed out in [55]. However, it is well known that only 2 non-collinear vector measurement can also suffice to determine the rotation uniquely. This reveals a disadvantage of the above method. However, this problem can be fixed as follows. 2.3 The case with only 2 vector measurements The minimum sufficient condition to uniquely determine the rotation is measuring simultaneously 2 non-collinear vectors. This case, due to the fact that it can not be treated by the general method for more than 2 vectors in the previous subsection, is called degenerate. This degenerate case is of both theoretical and practical importance. Fortunately, the above method can be slightly modified to be applied to this case. The key is to construct a third, pseudo vector measurement, according to the vector’s cross product rule. The measured vectors are w1 and w2, and the corresponding reference vectors are v1 and v2. Then we have the following, readily from (2). w1  Rv1  1

(15)

w2  Rv2   2

(16)

According to the cross product rule, we have the following.

w3  w1  w2

  Rv1   1    Rv2   2    Rv1    Rv2    Rv1    2   1   Rv2   1   2

(17)

 R  v1  v2    Rv1    2   Rv2    1   1   2  Rv3   3

with

 3   Rv1    2   Rv2   1

(18)

Note in (17), the approximation results from omitting the second order term of the errors, i.e., 1   2 . With the simultaneous equations (15), (16), and (17), now we are to solve a well determined problem with 9 equations and 9 parameters. It is indeed well determined because the pseudo equation (17) is linearly dependent on both (15) and (16) with r as the argument. However, the method developed in the previous subsection can not be directly applied, because now we are treating a singular VCM of the measurement errors. This singularity is apparent by noting that the pseudo error vector in (18) is a linear combination of the two original error vectors. We can not simply replace the plain inverse of the VCM by its Moore-Penrose pseudo inverse. As can be shown in the sequel, singular VCM actually implies (linear) equality constraints, and the problem should be solved in the framework of constrained least-squares. The same as the previous subsection, stack the 3 vector measurements to get the linear measurement model as in (7), rewrite it here as follows.

  V T  I3  r    Gr  

(19)

Let the singular value decomposition of the VCM be as follows. P  NEN T   N1

 E 0  N1T  T N2   1   T  =N1 E1 N1 0 0    N2 

(20)

where E1 is a diagonal matrix with its diagonal elements being the non-zero singular values, it is easy to conclude that there are 6 non-zero singular values as long as the two original error vectors are linearly independent with each other and are of full rank VCM. Then (19) can be transformed equivalently to the following pseudo measurement model. z  N T   N T Gr  N T   Cr  

(21)

   N T      1    1T   2   N 2  

(22)

     E 0 cov   1    N T cov   N  N T NEN T N  E   1   0 0   2  

(23)

cov  2   0

(24)

Let

Then we have

i.e.,

From (24), it can be shown that  2 has a degenerate distribution as follows, according to [56].

Pr[ 2  0]  1

(25)

where Pr[] denotes the probability of an event. From (25) it means that  2 equals to zero vector almost surely, i.e.,

2  0

(26)

which actually represents a hard, equality constraints. Let  z  N T  z   1    1T   z2   N 2  

(27)

C   N T G  C   1    1T  C 2   N 2 G 

(28)

z1  C1r  1

(29)

z2  C 2 r

(30)

Then we have

and

with no errors. Note that C2 should be of full row rank, otherwise, no feasible estimate would exist or the estimated would be completely determined by (30). The following Lagrangian can be constructed for the constrained least-squares problem.

   z1  C1r  E11  z1  C1r   2 T  z2  C2 r  T

(31)

where λ denotes the Lagrange multiplier corresponding the constraint as in (30). Note that the un-constrained part of (31) is in fact as follows.

   z  C1r  E11  z1  C1r  T

(32)

   Gr  P   Gr  T

where P denotes the Moore-Penrose pseudo inverse. So for singular VCM, the un-constrained loss function has the same form as that for the loss function for non-singular VCM, noting that the plain inverse is only a special case of the Moore-Penrose pseudo inverse for the case of non-singular VCM. However, the case of singular VCM results in a constrained problem in which the loss function as in (32) is only a part of the more rigorous loss function (31), i.e., the Lagrangian. If we use (32) instead of (31), we can only obtain approximate rather than optimal solution. According to the first order necessary condition of a minimizer of (31), we have the following.   2C1T E11  z1  C1r   2C2T   0 r

(33)

Left multiply (33) by C2 and then solve it for λ, then we have the following.

ˆ    C2C2T  C2C1T E11  z1  C1r   J  z1  C1r  1

(34)

Note that C 2 C 2T is indeed invertible because C2 is of full row rank. Substitute (34) into (31), we have

   z1  C1r  E11  z1  C1r   2  z1  C1r  J  z2  C2 r  T

T

(35)

Then we have   2C1T E11  z1  C1r   2C1T J  z2  C 2 r   2C 2T J T  z1  C1r  r

(36)

i.e., Kr   C1T E11C1  C1T J T C 2  C 2T JC1  r   C1T E11  C 2T J  z1  C1T J T z2  Lz1  C1T J T z2

(37)

rˆ  K 1 Lz1  K 1C1T J T z2

(38)

Prrˆˆ  K 1 LD1 LT K 1

(39)

So we have

and

Note that K is symmetric. It is apparent that the VCM as in (39) is also singular, however, this singularity brings no difference in the development in the sequel. 2.4 Special orthogonal matrix extraction A matrix is selected to minimize the following loss function [57]

  R  Rˆ



F





T  tr  R  Rˆ R  Rˆ   

(40)

and subject to (9) and (10). In (40), the subscript F denotes the Frobenius norm, tr[] denotes the matrix trace operator, 3×3 matrix Rˆ is restored from rˆ . Expand (40) and use (9), we have

ˆ ˆT    3  2tr  RRˆ T   tr  RR 

(41)

So minimizing (40) is equivalent to maximizing

  tr  RRˆ T 

(42)

Let the singular value decomposition of Rˆ be Rˆ  UDV T

(43) where U and V are both 3×3 orthogonal matrices, D=diag([d1,d2,d3]), d1≥d2≥d3≥0. As long as d2＞0, i.e., rank[ Rˆ ]≥2, the following special orthogonal matrix can maximize (42) according to e.g., [2, 40].

1 0  0   T R  U 0 1 0 V 0 0 det U  det V  =UV T

(44)

1 0 0    U  U 0 1 0  0 0 det U 

(45)

with

1 0 0    V  V 0 1 0  0 0 det V 

(46)

Note that U and V are both special orthogonal matrices. In order to be more self-contained, a proof of (44) is also presented in the appendix. The error analysis of R is performed in the next section. 3 Error analysis Let a small error of R be dR, i.e.,

R  R  dR

(47)

According to the orthogonality of both R and R , we have

I 3   R  dR  R  dR 

T

 RRT  dRRT  RdRT  dRdRT  I 3  dRR  RdR T

(48)

T

where the approximation is correct to first order in dR . (48) implies

dRRT   RdRT    dRRT 

T

(49)

i.e., dRRT is skew-symmetric. Any skew-symmetric map in 3D is equivalent to the cross product with an appropriate vector [20]. Let this vector be θ, we have 3 0

 0 dRR      3   2 T

1

2  1 

(50)

0 

As R  R  dR   I 3    R  1   3   2

3 1 1

(51)

 2  1  R 1 

for small angles, θ1, θ2, and θ3, happen to be the Euler angles transforming the coordinates in the estimated frame to that in the true frame. We will use the 3×1 θ to evaluate the error in R . When using R to transform coordinates of a vector, e.g., b  Ra , the errors in R propagates as follows, db  b  b  Ra  Ra  dRa

(52)

    Ra   Ra   

and Pbb  cov  db   Ra   P  Ra  

T

(53)

From (44), we have

dR  dUV T  UdV T

(54)

Then

   dRRT  dUU T  UdV TVU T  U U dU  dV V  U T

T

(55) T

Similar to (50), we can define

u   U T dU

(56)

v   dV TV

(57)

Substitute (56) and (57) into (55), we have

   dUU T  UdV TVU T

 U U T dU  dV TV  U T

(58)

 U   u    v    U T

So the following can be easily checked [2]

  U u  v 

(59)

Rˆ  UDV T

(60)

From (43) and (45), (46), with 1

1 0 1 0 0  0      D  0 1 0  D 0 1 0  0 0 det V  0 0 det V   d1 0  0     0 d2 0   0 0 d3 det U  det V 

1

(61)

Note that as det[U]=±1 and det[V]=±1, we have 1/det[U]=det[U] and 1/det[V]=det[V] which have been used in (61). So from (60) we have dRˆ  dUDV T  UdDV T  UDdV T So from (56), (57), and (62), we have ˆ  U T dUD  dD  DdV TV U T dRV   u  D  dD  D v 

(62)

(63)

Define ˆ   U T dRV ˆ    U T dRV   D  u   dD  v  D   u  D  dD  D v    D  u  + v    u  + v  D T

from which the following can be easily checked

(64)

 d 2  d3 det U  det V  0 0       0 d1  d3 det U  det V  0  u  v  0 0 d1  d 2   D u  v 

(65)

So from (59) and (65), we have

  UD1

(66)

Let

e1

e2 e3   I3

(67)

and we have e3T   e2       e1T   e3   e2T   e1    e T U T dRV ˆ  3  ˆ   e1T U T dRV  ˆ  e T U T dRV  2

  

  

e2   T T ˆ  e3 U dRVe2   T   ˆ e3    e1T U T dRVe 3    T T ˆ T e U dRVe1  e1   2 

T

ˆ ˆ  e2T U T dRVe  e3T U T dRVe  3 2  T T   T T  ˆ ˆ   e3 U dRVe 1    e1 U dRVe3   T T ˆ   T T ˆ  e1 U dRVe2   e2 U dRVe1   e3T V T    e2T U T    e2T V T    e3T U T         e1T V T    e3T U T   drˆ   e3T V T    e1T U T   drˆ      e2T V T    e1T U T    e1T V T    e2T U T         S1

S2

(68)

S3  drˆ T

with S1  Ve3   Ue2   Ve2   Ue3  S2  Ve1   Ue3   Ve3   Ue1 

(69)

S3  Ve2   Ue1   Ve1   Ue2 

So  S1T Prrˆˆ S1  P   S2T Prrˆˆ S1  S3T Prrˆˆ S1 

S1T Prrˆˆ S 2 S2T Prrˆˆ S 2 S3T Prrˆˆ S2

S1T Prrˆˆ S3   S 2T Prrˆˆ S3  S3T Prrˆˆ S3 

(70)

From (66), we have P  UD1 P D1U T

4 An illustrating example

(71)

Ten 3D vector measurements are used to estimate the rotation matrix. The true rotation matrix is generated randomly as follows, a rotation angle ϑ0 is generated randomly according to the [0 2π] uniform distribution, elements of a 3D vector are generated randomly according to the standard normal distribution, then this vector is normalized to be  , then the true rotation matrix is given as R  I3  sin0    1  cos 0     

(72)

Each element of the ten (true) reference 3D vectors vi is generated randomly according to the [1 10] uniform distribution, and accordingly the true measurement 3D vector can be calculated as wi=Rvi. The combined measurement error vector, i.e., the 30×1  , is generated randomly according to a (0,

2P) normal distribution, where varies from 1 to 0.01 with a −0.01 step, and P is generated as follows, the diagonal elements are set as ones, the off-diagonal elements are generated randomly according to a [−0.1 0.1] uniform distribution. For an estimated rotation matrix R , the skew-symmetric matrix of the error vector is given as

  

T 1 R  R  RT  R  R  R      2

(73)

The merit of this formulation compared to that in (50) is that the matrix is skew-symmetric exactly rather than approximately, no matter what R and R are. It is straightforward to extract the error vector  from (73). The elements of  , i.e., θ1, θ2, and θ3, respectively, and its associate (±) standard deviation, i.e., the square-root of the corresponding diagonal elements of P are plotted against different  in Fig. 1, 2, and 3, respectively. Note the rotation of the angles in θ around the axes defined by the estimated rotation is nothing but the one that rotates the vectors in the estimated frame to those in the true frame. As this error vector is referenced with respective to the (estimated) target (often the body) frame, it is also called a local error [1]. Two main observations can be made from these figures, first the estimation error approach zeros as the accuracy of the measurement increase which validates the statistical efficiency of the proposed method, second, for any measurement accuracy, the estimation error in magnitude is approximately equal to the associated standard deviation, which validates the statistical efficiency between the estimates and their variances.

0.1 error ＋sd －sd

1 (rad)

0.05

0

-0.05

-0.1 1

0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0  (m)

Fig. 1 The estimation error and the variance of the first element of the error vector θ versus different measurement accuracies

0.15 error ＋sd －sd

0.1

2 (rad)

0.05 0 -0.05 -0.1 -0.15 1

0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1  (m)

0

Fig. 2 The estimation error and the variance of the second element of the error vector θ versus different measurement accuracies

0.2 error ＋sd －sd

0.15

3 (rad)

0.1 0.05 0 -0.05 -0.1 -0.15 -0.2 1

0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0  (m)

Fig. 3 The estimation error and the variance of the third element of the error vector θ versus different measurement accuracies 5 Conclusion The general 3D rotation estimation using vector measurements has been studied, here the “general” means that the covariance of the (combined) measurement error vector can be arbitrary given its symmetry and positive semi-definiteness as a covariance matrix. A closed solution has been derived. This solution is obtained in two steps, first a free matrix not necessarily special orthogonal is estimated optimally in the weighted least-squares sense, second, a special orthogonal matrix closest to the free matrix in the Frobenius-norm sense is extract. This solution can only be sub-optimal though both the two steps are optimal in their own sense. Error analysis of the problem is also performed, the covariance of the estimation error vector is derived. The statistical efficiency and consistency of the proposed method have been validated in an illustrating example. As the proposed method is somewhat fully general, measurement errors with any specific covariance structure can be safely treated as a special case of our study, and hence can be solved using our method, though some other methods, which may be globally optimal, may exist for these cases. Acknowledgements Two anonymous reviewers are greatly acknowledged for their valuable comments which improved the manuscript significantly. We would like to dedicate this work to our teacher Prof. Yuanxi Yang on his 60th birthday. This work is supported by the National Key Research and Development Program of China (2016YFB0501701), and the National Natural Science Foundation of China (41574013, 41404001, 41404033, 41576105). Appendix: Proof of (44).

We should solve a constrained maximizing problem to estimate R, i.e., maximizing (42) under the inherent constraints among the elements of R. So the following Lagrangian is constructed

  trace  RRˆ T   trace  L  RT R  I     det  R  1

(74)

where the symmetric matrix L and the scalar μ are the multipliers corresponding to the orthogonal and determinant constraints. We have the following first-order necessary condition with respect to R to maximize (74),  ˆ  R  2 RL   R  0 R

(75)

M  2 L   I3

(76)

Let Then (75) can be rearranged as

RM  Rˆ It is apparent that M in symmetric, so transpose both sides of (77), we have MRT  Rˆ T

(77)

(78) Left multiply the left and right sides of (77) with the left and right sides of (78) respectively, and with (43) in mind, we have (79) M 2  VD2V T 2 3 2 As M and M are commutative, i.e., M M=MM =M , so M and M can be diagonalized by the same 2

2

orthogonal matrix, see [58], p64, theorem 1.3.21. So from (32), we have (80) M =VNV T =VDWV T where W is diagonal whose diagonal elements can only be +1 or −1. From (42) and (77), we have

  trace  RRˆ T   trace  RMRT   trace  M 

(81)

where the last equation holds according to the invariance of the trace under the similarity matrix transformation. So from (80) and (81), we have

  trace VDWV T   trace  DW  =d1w1  d2 w2  d3 w3

(82)

where dj and wj are the diagonal elements of D and W respectively, and further d1≥d2≥d3≥0, and wj=+1 or −1. In the following, four different cases are analyzed separately. Case 1, if rank[ Rˆ ]=3, implying that Rˆ is full-rank, we have d3>0, and hence det[D]>0. In order to maximize (82), we should first let w1=w2=1 with w3 to be determined. From (80) and (77), we have respectively det  M  =  det V  det  D det W  =det  D det W  =w3det  D

(83)

det  M  =det  RT  det  Rˆ  =det  Rˆ  =det  D det U  det V 

(84)

2

So we have w3=det[U]det[V]. From (77), we have

R  HM 1  UDV TVWD1V T  UDWD1V T  UWV T

(85)

1 0  0   W = 0 1 0  0 0 det U  det V 

(86)

with

Case 2, if rank[ Rˆ ]=2, implying that d1≥d2>0, and d3=0. Similarly, in order to minimize (82), we should first let w1=w2=1, while w3 can not be determined for now because it does not affect (82) for this case. As d3=0, and w1=w2=1, it is apparent that DW=D. So from (77) and (80), we have

RM  RVDWV T  RVDV T  H  UDV T So U T RVD  D . Define the orthogonal matrix

Q  q1 q2

q3   U T RV

(87)

(88)

so QD=D, i.e., q1d1   d1

0 0 , q2 d2  0 d2

0

T

T

(89)

So q1  1 0 0 , q2  0 1 0 T

T

(90)

From (88), we have det[Q]=det[U]det[R]det[V]=det[U]det[V]. So for the orthogonal matrix Q and from (90) we must have q3  0 0 det U  det V 

T

(91)

It is found that Q=W, so from (88), we again have (85) with (86). Case 3, if rank[ Rˆ ]=1, implying that d1>0, and d2=d3=0. In order to maximize (82), we should first let w1=1, while w2 and w3 can not be determined for now because neither affects (82) for this case. From (80), rank[M]=rank[ Rˆ ]=1, so there are infinitely many special orthogonal matrix R that can maximize (82). Case 4, if rank[ Rˆ ]=0, implying that d1=d2=d3=0. There is no maximum value of (82), or in other words, any 3×3 matrix can maximize (82). So when rank[ Rˆ ]≥2, i.e., case 1 or 2, can the existence of the unique least-squares solution to the problem be ensured, and the solution is (85) with (86). Let the DCM estimated in this appendix be denoted as R , (44) is proved from (85) and (86).

References [1] M.D. Shuster, S. Oh, Three-axis attitude determination from vector observations, Journal of Guidance, Control, and Dynamics, 4 (1981) 70-77. [2] F.L. Markley, Attitude determination using vector observations and the singular value decomposition, The Journal of the Astronautical Sciences, 36 (1988) 245-258. [3] X. Tong, Y. Xu, Z. Ye, S. Liu, X. Tang, L. Li, H. Xie, J. Xie, Attitude oscillation detection of the ZY-3 satellite by using multispectral parallax images, IEEE Transactions on Geoscience and Remote Sensing, 53 (2015) 3522-3534. [4] M. Kiani, S.H. Pourtakdoust, A.A. Sheikhy, Consistent calibration of magnetometers for nonlinear attitude determination, Measurement, 73 (2015) 180-190. [5] G. Chang, Loosely Coupled INS/GPS Integration with Constant Lever Arm using Marginal Unscented Kalman Filter, The Journal of Navigation, 67 (2014) 419-436. [6] W. Liu, X. Ma, Z. Jia, Y. Zhang, Z. Shang, X. Li, Position and attitude measurement of high-speed isolates for hypersonic facilities, Measurement, 62 (2015). [7] Y. Wu, J. Wang, D. Hu, A New Technique for INS GNSS Attitude and Parameter Estimation Using

Online Optimization, IEEE Transactions on Signal Processing, 62 (2014) 2642-2655. [8] D.-J. Jwo, C.-F. Yang, C.-H. Chuang, T.-Y. Lee, Performance enhancement for ultra-tight GPS/INS integration using a fuzzy adaptive strong tracking unscented Kalman filter, Nonlinear Dynamics, 73 (2013) 377-395. [9] D.-J. Jwo, C.-F. Yang, C.-H. Chuang, K.-C. Lin, A Novel Design for the Ultra-Tightly Coupled GPS/INS Navigation System, The Journal of navigation, 65 (2012) 717-747. [10] J. Bjorke, Computation of calibration parameters for multibeam echo sounders using the least squares method, IEEE Journal of Oceanic Engineering, 30 (2005) 818-831. [11] X. Wei, G. Zhang, Q. Fan, J. Jiang, J. Li, Star sensor calibration based on integrated modelling with intrinsic and extrinsic parameters, Measurement, 55 (2014) 117-125. [12] Y. Wu, W. Shi, On Calibration of Three-axis Magnetometer, IEEE Sensors Journal, 15 (2015) 6424-6431. [13] K. Kanatani, Calibration of ultra-wide fisheye lens cameras by eigenvalue minimization, IEEE Transactions on Pattern Analysis and Machine Intelligence, 35 (2013) 813-822. [14] X. Liu, X. Xu, Y. Liu, L. Wang, A fast and high-accuracy transfer alignment method between M/S INS for ship based on iterative calculation, Measurement, 51 (2014) 297-309. [15] M. Wu, Y. Wu, X. Hu, D. Hu, Optimization-based alignment for inertial navigation systems Theory and algorithm, Aerospace science and technology, 15 (2011) 1-17. [16] P.M.G. Silson, Coarse Alignment of a Ship's Strapdown Inertial Attitude Reference System Using Velocity Loci, IEEE Transactions on Instrumentation and measurement, 60 (2011) 1930-1941. [17] G. Chang, Fast two-position initial alignment for SINS using velocity plus angular rate measurements, Advances in Space Research, 56 (2015) 1331-1342. [18] J. Ali, M.R.U.B. Mirza, Initial orientation of inertial navigation system realized through nonlinear modeling and filtering, Measurement, 44 (2011) 793-801. [19] A. Acharya, S. Sadhu, T. Ghoshal, Improved self-alignment scheme for SINS using augmented measurement, Aerospace science and technology, 15 (2011) 125-128. [20] L. Dorst, First order error propagation of the procrustes method for 3D attitude estimation, IEEE Transactions on Pattern Analysis and Machine Intelligence, 27 (2005) 221-229. [21] K. Kanatani, H. Niitsuma, Optimal computation of 3-D similarity: Gauss–Newton vs. Gauss–Helmert, Computational Statistics & Data Analysis, 56 (2012) 4470-4483. [22] K. Kanatani, H. Niitsuma, Optimal Computation of 3-D Similarity from Space Data with Inhomogeneous Noise Distributions, Memoirs of the Faculty of Engineering, Okayama University, 46 (2012) 1-9. [23] K. Kraus, Photogrammetry: Geometry from images and laser scans, de Gruyter, Berlin, 2007. [24] J.-Y. Han, C.-S. Chen, C.-T. Lo, Time-variant registration of point clouds acquired by a mobile mapping system, IEEE Geoscience and Remote Sensing Letters, 11 (2014) 196-199. [25] Y. Wang, Y. Wang, K. Wu, H. Yang, H. Zhang, A dual quaternion-based, closed-form pairwise registration algorithm for point clouds, ISPRS Journal of Photogrammetry & Remote Sensing, 94 (2014) 63-69. [26] G. Chang, On least-squares solution to 3D similarity transformation problem under Gauss-Helmert model, Journal of Geodesy, 89 (2015) 573-576. [27] E.W. Grafarend, J.L. Awange, Nonlinear analysis of the three-dimensional datum transformation [conformal group C7(3)], Journal of Geodesy, 77 (2003) 66-76. [28] H. Zeng, Analytical algorithm of weighted 3D datum transformation using the constraint of

orthonormal matrix, Earth, Planets and Space, 67 (2015) 105. [29] T. Chang, J. Stock, P. Molnar, The rotation group in plate tectonics and the representation of uncertainties of plate reconstructions, Geophysical journal international, 101 (1990) 649-661. [30] T. Soler, J.-Y. Han, On rotation of frames and physical vectors an exercise based on plate tectonics theory, GPS Solutions, (2016) 10.1007/s10291-10016-10521-10295. [31] M.D. Shuster, A Survey of Attitude Representations., The Journal of the Astronautical Sciences, 41 (1993) 439-517. [32] F.L. Markley, J.L. Crassidis, Fundamentals of Spacecraft Attitude Determination and Control, Springer, New York, 2014. [33] Q. Wang, G. Chang, T. Xu, Y. Zou, Representation of the rotation parameter estimation errors in the

Helmert

transformation

model,

Survey

Review,

Online

first

(2016)

doi:

10.1080/00396265.00392016.01234806. [34] G. Chang, T. Xu, Q. Wang, Baseline configuration for GNSS attitude determination with an analytical least-squares solution, Measurement Science and Technology, 27 (2016) 125105. [35] J.L. Crassidis, F.L. Markley, Y. Cheng, Survey of nonlinear attitude estimation methods, Journal of Guidance, Control, and Dynamics, 30 (2007) 12-28. [36] G. Chang, T. Xu, Q. Wang, Error Analysis of Davenport's q Method, Automatica, 75 (2016) 217-220. [37] E.J. Lefferts, F.L. Markley, M.D. Shuster, Kalman filtering for spacecraft attitude estimation, Journal of guidance, control, and dynamics, 5 (1982) 417-429. [38] I.Y. Bar-Itzhack, Y. Oshman, Attitude determination from vector observations: quaternion estimation, IEEE Transactions on Aerospace and Electronic Systems, 21 (1985) 128-136. [39] M. Kiani, S.H. Pourtakdoust, Adaptive Square-Root Cubature-Quadrature Kalman Particle Filter for satellite attitude determination using vector observations, Acta Astronautica, 105 (2014) 109-116. [40] S. Umeyama, Least-squares estimation of transformation parameters between two point patterns, IEEE Transactions on Pattern Analysis and Machine Intelligence, 13 (1991) 376-380. [41] J. Keat, Analysis of least-squares attitude determination routine (DOAOP),

Computer Sciences

Corporation, 1977, pp. CSC/TM-77/6034. [42] Y. Yang, Z. Zhou, An analytic solution to Wahba's problem, Aerospace science and technology, 30 (2013) 46-49. [43] Y. Cheng, J.L. Crassidis, F.L. Markley, Attitude estimation for large field-of-view sensors, The Journal of the Astronautical Sciences, 54 (2006) 433-448. [44] A. Leick, L. Rapoport, D. Tatarnikov, GPS Satellite Survey, 4th Edition, Wiley, New Jersey, 2015. [45] P.A. Roncagliolo, J. Garcia, P.I. Mercader, D.R. Fuhrmann, C.H. Muravchik, Maximum-likelihood attitude estimation using GPS signals, Digital Signal Processing, 17 (2007) 1089-1100. [46] J.L. Crassidis, F.L. Markley, New Algorithm for Attitude Determination Using Global Positioning System Signals, Journal of Guidance, Control, and Dynamics, 20 (1997) 891-896. [47] G. Chang, Robust Kalman filtering based on Mahalanobis distance as outlier judging criterion, Journal of Geodesy, 88 (2014) 391-401. [48] X. Ye, X. Xiao, J. Shi, M. Ling, The new concepts of measurement error theory, Measurement, 83 (2016) 96-105. [49] S.M. Kay, Fundamentals of statistical signal processing, Volume I: estimation theory, Pearson Education, New York, 2013.

[50] J. Nocedal, S.J. Wright, Numerical Optimization, Second Edition, Springer, New York, 2006. [51] J.-Y. Han, Noniterative Approach for Solving the Indirect Problems of Linear Reference Frame Transformations, Journal of Surveying Engineering, 136 (2010) 150-156. [52] J.-Y. Han, B.H.W. van Gelder, Stepwise Parameter Estimations in a Time-Variant Similarity Transformation, Journal of Surveying Engineering, 132 (2006) 141-148. [53] J. Yan, C.C.J.M. Tiberius, G.J.M. Janssen, P.J.G. Teunissen, G. Bellusci, Review of range-based positioning algorithms, IEEE Aerospace and Electronic Systems Magazine, (2013) 2-27. [54] P.J.G. Teunissen, Adjustment Theory: An Introduction, Delft University Press, Delft, 2000. [55] F.L. Markley, I.Y. Bar-Itzhack, Unconstrained optimal transformation matrix, IEEE Transactions on Aerospace and Electronic Systems, 34 (1998) 338-340. [56] J.R. Magnus, H. Neudecker, Matrix Differential Calculus With Applications In Statistics And Econometrics, Third Edition, John Wiley and Sons, New York, 2007. [57] F.L. Markley, Y. Cheng, J.L. Crassidis, Y. Oshman, Averaging quaternions, Journal of guidance, control, and dynamics, 30 (2007) 1193-1197. [58] R.A. Horn, C.R. Johnson, Matrix Analysis, 2nd Edition, Cambridge University Press, New York, 2013.

Highlights

1, The method can be applied to vector measurements with full variance-covariance matrix. 2, The rotation matrix is estimated in two steps both of which are optimal in their own. 3, The solution is analytical. 4, Special case with only 2 non-collinear vector measurements is treated. 5, Error analysis of the solution is provided.