Dot Product Equality Constrained Attitude Determination from Two Vector Observations: Theory and Astronautical Applications

: In this paper, the attitude determination problem from two vector observations is revisited, incorporating the redundant equality constraint obtained by the dot product of vector observations. Analytical solutions to this constrained attitude determination problem are derived. It is found out that the studied two-vector attitude determination problem by Davenport q-method under the dot product constraint has deterministic maximum eigenvalue, which leads to its advantage in error/perturbation analysis and covariance determination. The proposed dot product constrained two-vector attitude solution is applied then to solve several engineering problems. Detailed simulations on spacecrafts attitude determination indicate the efﬁciency of the proposed theory.


Introduction
Attitude determination from vector observations is usually employed in astronautical applications for state estimation and control of spacecrafts [1][2][3]. Commonly, the vector observations are acquired from vector-measurement sensors e.g., Sun sensor, Nadir sensor, magnetometer, star tracker, etc. [4][5][6]. The usage of direct attitude determination from such sensors has the advantage of compensating for biases in rate gyroscopes to cancel the long-endurance drifts inside inertial navigation results [7,8].
In the past several decades, many algorithms for optimal attitude determination have been developed [9,10]. Apart from some intuitive geometric approaches, most of the others are based on the Wahba's problem posed in 1965, giving optimal attitude matrix estimates with normalized vector measurements along with their statistical weights [11]. There have been a lot of Wahba's solvers developed in the past 30 years, including celebrated ones such as the QUaternion ESTimator (QUEST [12]), singular value decomposition (SVD [13]) and Euler-q [14], and recent ones such as the vector-inertia-based method by Patera [15], the Riemannian-manifold-based on by Yang [16], the optimal linear estimator of quaternion (OLEQ [17]), etc. These solver are all general ones facing the multivector cases. Actually, when there are few pairs of vector observations, the Wahba's optimization can also be solved via numerical algorithms like gradient-descent algorithm (GDA [18]), Gauss-Newton algorithm (GNA [19]), Levenberg-Marquardt algorithm (LMA [20]), etc. Such optimization-based solvers are popular in mechatronic navigation tasks with accelerometers and magnetometers only [21].

Problem Fomulation
Given two normalized vector observation pairs in the body frame b and reference frame r, one can relate them by b 1 = Cr 1 b 2 = Cr 2 (1) where C in the special orthogonal group SO(3) := {C|C T C = I, det(C) = 1} is the direction cosine matrix (DCM); the sensor measurements are given by b 1 = b x,1 , b y,1 , b z,1 T r 1 = r x,1 , r y,1 , r z,1 T , b 2 = b x,2 , b y,2 , b z,2 T r 2 = r x,2 , r y,2 , r z,2 x,1 + b 2 y,1 + b 2 z,1 = 1 r 2 x,1 + r 2 y,1 + r 2 z,1 = 1 b 2 x,2 + b 2 y,2 + b 2 z,2 = 1 r 2 x,2 + r 2 y,2 + r 2 z,2 = 1 In astronautical missions, the spacecraft is always equipped with a Sun sensor, magnetometer, horizon sensor, etc. for attitude determination (see Figure 1). Wahba's problem aims to find out the optimal DCM of Equation (1) by the following least-square optimization. arg min where w is the weight belonging to the first vector observation pair. This optimization can be reduced to finding the optimal eigenvector of the following matrix K [32,33], such that where λ max is the largest eigenvalue of K while other parameters are given by The characteristic polynomial of K can be computed by In fact, many closed-form formulations of this polynomial have been developed. For instance, in ESOQ2, it has been given that [34,35] It is evident that W and K has the similar structure. This completes the proof. Lemma 1. τ 2 = 0 holds for arbitrary two pairs of vector observations.
Proof. The diagonal elements of K matrix can be given by Therefore, it is obvious that the sum of the eigenvalues to K equals to K 1,1 + K 2,2 + K 3,3 + K 4,4 = 0. This is why the coefficient of λ 3 is zero. Note that τ 2 is de facto in the form of [36] Actually, the right part can be represented by In view of B for the two-vector case, we can see that it takes the following form indicating that B is rank-deficient. Therefore, we arrive at which completes the proof.
With Lemma 1, we can see that the characteristic polynomial is essentially a quadratic equation of λ 2 . Then the eigenvalues can be solved immediately via The maximum eigenvalue should be because [36] As the q-method is derived from the Lagrange multiplier, with increasing maximum eigenvalue, the loss function becomes even smaller. The maximum eigenvalue has an upper bound, as proven by Shuster that [12] λ max where w i is the weight of the i-th vector observation pair. Regarding λ max as a function of the vector observations, we can seek its largest point. This is equivalent to figuring out the solution to the following system, which is rather difficult in detail. Let us write out the algebraic form of τ 1 and τ 3 : 2 r x,1 r x,2 + r y,1 r y,2 + r z,1 r z,2 (25a) 2 r x,1 r x,2 + r y,1 r y,2 + r z,1 r z,2 − 8w 3 (w − 1) b x,1 b x,2 + b y,1 b y,2 + b z,1 b z,2 r x,1 r x,2 + r y,1 r y,2 + r z,1 r z,2 − y,1 − 2r x,1 r x,2 r y,1 r y,2 + r 2 y,2 − 2r 2 y,1 r 2 y,2 + r 2 z,1 − r 2 y,2 r 2 z,1 − 2r x,1 r x,2 r z,1 r z,2 − 2r y,1 r y,2 r z,1 r z,2 + r 2 z,2 − r 2 y,1 r 2 z,2 − 2r 2 z,1 r 2 Note that we have the following simplification.
b 2 y,1 + b 2 τ 1 and τ 3 are simplified to Therefore, it is shown that the eigenvalue is actually the function of p and q, with parameters ranging from The figures of the maximum eigenvalue Equation (21) with w = 0.5 are depicted as follows in Figure 2. With different w, the maximum eigenvalues are drawn in Figure 3. From the above figures, we see that the maximum eigenvalue is a convex function with respect to p and q no matter how w changes. Based on this point, the global optimum-seeking procedure is then reduced to solve the following system.
The partial differentiation can be computed by where r is the differentiation independent variables: By which the specific derivatives are obtained by Inserting these equalities into Equation (30) and invoking we obtain at which, we also have leading to Reconsidering the forms of p and q, we find out which can actually be derived from another aspect by This finding reflects that the equality constraint Equation (38) can achieve least value of Wahba's loss function, such that [32] L The occurrence of λ max = 1 has in fact been shown by Markley [26]. However, we need to note that the attitude matrix employed for attitude representation in the work by the authors of [26] is not as flexible as the quaternion used in this paper, as a quaternion only requires normalization rather than orthonormalization for an attitude matrix [37].

Quaternion Solution
As the loss function is zero, we can regard the optimization as a properly solved one. That is, at this time, the information from each sensor has been maximumly extracted to reduce the loss function value; and, geometrically, every sensor has fully contributed to all those angles that they can observe. This means the optimal quaternion is finally computed via generating the following row-echelon form The symbolic solutions to this function can be obtained by b y,1 r x,1 r x,2 r z,2 + b x,1 b z,2 r x,2 r y,1 − b z,2 r x,1 r y,2 − b y,2 r x,2 r z,1 + b y,2 r x,1 r z,2 + b z,1 r y,2 (b x,2 r x,1 + r x,2 r x,1 + r z,1 r z,2 ) + r y,1 −b x,2 r x,2 + r 2 y,2 − 1 + b z,2 r y,2 r 2 y,1 − b y,2 r y,2 r y,1 r z,1 − b y,1 r y,2 r y,1 r z,2 + b z,2 r y,1 r z,1 r z,2 − b y,1 r z,1 r 2 z,2 − b z,2 r y,2 + b y,1 r z,1 − b y,2 r 2 z,1 r z,2 + b y,2 r z,2 (43b) β = b z,2 r x,2 r 2 y,1 − b z,2 r x,1 r y,2 r y,1 − b z,1 r x,2 r y,2 r y,1 + b x,2 r y,2 r y,1 r z,1 + b x,1 r y,2 r y,1 r z,2 + b z,1 r x,1 r 2 y,2 + b y,2 −b z,1 r x,2 r y,1 + b x,1 r y,1 r z,2 + b z,1 r x,1 r y,2 − b x,1 r y,2 r z,1 + b y,1 b z,2 r x,2 r y,1 − b x,2 r y,1 r z,2 − b z,2 r x,1 r y,2 + b x,2 r y,2 r z, b y,2 r x,2 r 2 z,1 + b y,1 r x,1 r 2 z,2 + b y,2 b z,1 r x,2 r z,1 − b y,1 b z,2 r x,2 r z,1 − b y,2 b z,1 r x,1 r z,2 + b y,1 b z,2 r x,1 r z,2 − b y,2 r x,1 r z,1 r z,2 − b y,1 r x,2 r z,1 r z,2 + b x,2 r y,1 r z,2 (b z,1 + r z,1 ) + r y,2 −b z,1 r z,1 + r 2 y,1 − 1 + r x,1 r x,2 r y,1 + b x,1 r y,2 (r z,1 b z,2 + r z,1 r z,2 + r x,1 r x,2 ) + r y,1 −b z,2 r z,2 + r 2 y,2 − 1 + b y,2 r x,2 r 2 y,1 − b y,2 r x,1 r y,2 r y,1 − b y,1 r x,2 r y,2 r y,1 + b y,1 r x,1 r 2 The final quaternion solution is obtained by We can see that the obtained quaternion is weight-free. A well-known two-vector quaternion solution is given by Markley [25], such that Markley's solution has the same accuracy as QUEST. However, if we put the proposed equality p = q into this solution, it is difficult to draw the conclusion that the quaternion is independent of w. The is because Markley's solution relies on the normalized cross products b 3 and r 3 . It is the nonlinearity of the normalization that makes the further simplification harder. The above results can also be computed by introducing the Lagrange multiplier [38] and in combination with the GDA [18]; as, in such an optimization, there will be two constraints: (1) quaternion unity and (2) dot product equality, and because the solving process will belong to the type of interior-point solvers for nonlinear programming [39], deriving the closed-form solution for such a problem seems much harder than the presented approach.

Error and Covariance Anlysis
Equation (1) is an ideal model for two-vector attitude determination without noises. The noise-corrupted model is expressed as follows where e 1 , e 2 , ε 1 , and ε 2 are independent white Gaussian noise items with covariances of Σ b 1 , Σ b 2 , Σ r 1 , and Σ r 2 , respectively. Assume that for the true value K we have a corresponding true quaternion q with eigenvalue 1, such that Kq = q With noise perturbation, K is interfered with by δK, also generating an eigenvector perturbation δq, say (K + δK) (q + δq) = q + δq Expanding it yields Kq + δKq + Kδq + δKδq = q + δq As we also have det(K + δK − I) = 0, the quaternion perturbation is derived by which produces the following quaternion difference [40], The above error analysis is the standard approach for the Wahba's solution using Davenport q-method. This error-analysis approach is presented to cope with the restriction of the unknown eigenvalues associated with the Davenport q-method. However, such results are also related to the selection of the weight w. Apparently, seen from Solution Equation (43), one may easily see that the obtained quaternion is free of w. This type of quaternion owns closed forms without high-order or highly nonlinear terms. Therefore for the proposed method, the error analysis can be performed easily using δq = 1 with δ 1 and δα, δβ, δγ, and δN can be computed directly from Equation (43). Then, the covariance of the derived quaternion using first-order approximation is also straightforward, given by where the Jacobian J is

Applications: Attitude Determination from Horizon Sensor and Another Generalized Sensor
The derived theory can be applied to those problems in which some of the reference vectors are hard to be determined. In this section, we choose the system configuration with a horizon sensor and another generalized sensor for two-vector attitude determination. The horizon sensor can be either an infrared Nadir detector [41] or a camera-based horizon finder [42]. It estimates the horizontal Euler angles of the mounting object by tracking Nadir or analyzes the geometry of the camera-captured horizon. Also, we need to note that in near-ground static applications, accelerometers can be used for reconstruction of roll and pitch information and can also be treated as a kind of horizon sensor.
There are many other types of sensors for a horizon sensor to accompany. For instance, the attitude determination by means of accelerometer and magnetometer can be used for pedestrian navigation. The infrared Nadir detector together with Sun sensor or magnetometer can also provide accurate enough attitude determination results. Take the accelerometer-magnetometer case for example, the magnetic reference vector is usually time-varying with the position of the rigid body. This is because the Earth geomagnetic field can be described by the following potential [43].
where A denotes the distance of the object to the Earth center; R e is the Earth radius; θ and ϕ are co-latitude and longitude, respectively; P m n is the associated Legendre function of degree m and order n; and g m n and h m n are Gaussian coefficients from global satellite geomagnetic measurements and are released in annual IGRF models. Then, the magnetic vector in the geodetic frame is the function of A, θ, and ϕ. The most common way to obtain such reference information is by looking up the table or directly computing the vector by spheric harmonic functions. However, when there is no source of position measurements (such as low-cost indoor smart wearables), it is very hard to obtain such information. A compromising way is to use the magnetic north rather than the true north provided by IGRF. Assuming that the accelerometer and magnetometer have their normalized vector measurements pairs as follows, we can directly construct the magnetic reference vector by whereas another component can be determined by where κ is the local dip angle [44]. From the derived results shown in last section, the quaternion calculated now only obtains roll, pitch from accelerometer, and yaw from magnetometer, respectively. The magnetometer does not influence the estimation of other two angles and neither does the accelerometer [28]. A record of accelerometer/magnetometer data along with reference angles are logged from the 3DM-GX3-25 attitude and heading reference system (AHRS). This AHRS module is attached to the data collection computer via the serial port with baudrate of 921600. The sampling frequency is set to 500 Hz. The motion has been generated using a human-operated inertial stabilized gimbal so that the motion in all axes will be produced. The motivation of using the accelerometer and magnetometer as an experimental object is that we would like to theoretically explain why attitude estimation can be conducted in a reference-free manner as presented in previous literatures [21,28,[45][46][47]. Using MATLAB r2016b, we evaluate the attitude determination results in Figure 4. The related loss functions are plotted in Figure 5. The results in Figure 5 magnify the nonintuitive errors presented in Figure 4.  In this experiment, the weights between accelerometer and magnetometer are both 0.5. The local magnetic reference vector of FLAE is set to M r = (0.9800, 0, −0.1989) T in the Northern Hemisphere. It is noticed from the figure that the roll and pitch angle determination has more observable differences than that of the proposed method between the true angles. This is because the roll and pitch angles used herein are not fully determined by the accelerometer but influenced by magnetometer as well. For the conventional Wahba's problem, the two sensors both contribute to the observabilities of three Euler angles roll, pitch, and yaw. However, using the proposed method, we can separate these observabilities. Apart from the errors in the local magnetic reference vector during movement, the AHRS has been somewhat distorted by outer unknown electromagnetic disturbances. In such occasion, the non-yaw angles would contain evident differences with the reference values. If the proposed method behaves, the angles fit those from the reference instrument very well. The internal reason has been introduced before i.e., the roll and pitch angles are fully measured from the accelerometer while the yaw is only determined by the magnetometer. The loss function results also reflect this point. Coinciding with previous findings, the loss function values from the proposed method vary at the basic scale of 1 × 10 −27 which is tiny enough to be considered as zero.
While for the more general case in which there are one horizon sensor and another sensor, using the results in Equation (43), we can give the analytical solution to such attitude determination problem by where and h b and h r are vector observation pair for the horizon sensor in b and r frames, respectively, whereas g b and g r represent the same meaning for another generalized sensor. Now, we simulate a mission for a satellite. Four sensors, including a star tracker, a horizon sensor, a Sun sensor, and a magnetometer, are employed (see Figure 6). Only the horizon sensor and Sun sensor are employed to compute the attitude in this section; the other sensors are used for generating high-resolution attitude reference information. All of the sensors presented in this simulation study are transformed into the J2000 Earth-Centered Interval (ECI) frame for attitude computation. The Sun vector can be computed straightforward according to historical astronomy ephemeris [48]. Given the Julian time T JD [49]: where · · · is the downward floor operator, thus the normalized Sun vector g r can be determined by where M sun denotes the Sun mean anomaly, γ ecliptic is the ecliptic longitude of the Sun, and e sun stands for the eccentricity of the Sun orbit. The satellite motion is simulated by creating an orbit around the Earth with eccentricity of 0.15 and semimajor axis of 9966.14 km. The satellite attitude is generated with Nadir alignment subject to orbit normal constraint of 3 deg. The spacecraft attitude has been simulated through the following equation of angular momentum, Iω +ḣ + ω × (Iω + h) = u where I denotes the inertia of spacecraft, ω is the angular rate, and h stands for the angular momentum driven by the control input u. The attitude model has been constructed via the 4-th Runge-Kutta method, and the control method used herein is a Propotional-Derivative (PD)-type control algorithm. The star tracker in this simulation points exactly to the Hipparcos 2 24608 star. The outer geomagnetic environment has the perturbation based on the Olson-Pfitzer field. The IGRF model for the magnetometer measurement in the local geodetic frame is expressed as follows.
(g m n cos mϕ + h m n sin mϕ) P m n (cos θ) The Sun sensor not always work, as there may be an area of shade due to the Earth's positioning during flight that prohibits Sun detection. The simulation is run for 1 single day from 1 March 2019 04:00:00.000 to 2 March 2019 04:00:00.000. The available solar luminance is indicated in Figure 7. The Sun sensor has accuracy of 20 arcsec and for horizon sensor that would be 1.5 arcmin. The sensor noises are simulated to be additive and subject to the Gaussian distribution. When the spacecraft is in the Earth shade area, we do not compute the attitude since only one vector observation pair does not have enough observability for three-axis attitude determination. The raw measurements of the four attitude sensors are presented in Figure 8 where the measured values of Sun sensor, star tracker, and the horizon sensor are presented in the normalized form. Using the representative QUEST solver for conventional Wahba's problem, we show the comparison with the proposed method. Figures 9 and 10 show the results from QUEST and the proposed algorithm, respectively.  The results show that both methods are efficient for attitude determination. However, since the proposed method separates the effects of the horizon sensor and Sun sensor, the attitude error covariance has a smaller scale than QUEST. The quaternion error responses of QUEST and the proposed algorithm are shown in Figure 11. In Figure 11, the 3σ bounds are also presented using the square roots of the diagonal elements of the computed quaternion error covariances. Corresponding Euler angle errors are shown in Figure 12. The QUEST has the root-mean-squared error (RMSE) attitude accuracy of 5.12 deg on roll, 2.34 deg on pitch, and 4.06 deg on yaw. The proposed method has advantages on roll and pitch and has RMSE attitude accuracy of 2.93 deg on roll, 1.01 deg on pitch, and 3.82 deg on yaw. The proposed method can make the attitude propagation from sensors independently and thus when emergency occurs the engineers may be also easier to separate the faults and will find out the sensor failures much faster; that is, if the horizon sensor has exceptions, only roll and pitch will be affected, whereas the Sun sensor encounters faults only if yaw can be influenced.

Conclusions
In this paper, we revisit the problem of two-vector attitude determination, which is based on the theory of Markley [25]. A novel dot product equality constraint is proposed to further support the conventional optimization. The quaternion solutions and covariance analysis are also obtained for implementation on chips. By virtue of the equality constraint, the derived quaternion result can separate sensor effects for independent attitude angle observabilities. The derived quaternion solution is not as neat as that provided in the work by the authors of [26], but it provides another perspective without matrix orthonormalization. An application on the attitude determination from the horizon sensor and another generalized sensor illustrates that the proposed method is effective and accurate. It has also been tested smaller covariance scales in the presented simulation study for spacecraft attitude determination. Future works should be devoted to invoke more vector observation pairs for generalized attitude determination tasks. Also, according to the independent angle separation functionality of the proposed work, it may be used for flexible sensor calibration in further studies.