# Optimal Attitude Determination from Vector Sensors Using Fast Analytical Singular Value Decomposition

Authors: Zhuohua Liu; Wei Liu; Xiangyang Gong; Jin Wu
Journal: Journal of Sensors
 (2018)

---

## Abstract

A novel algorithm is proposed in this paper to solve the optimal attitude determination formulation from vector observation pairs, that is, the Wahba problem. We propose here a fast analytic singular value decomposition (SVD) approach to obtain the optimal attitude matrix. The derivations and mandatory proofs are presented to clarify the theory and support its feasibility. Through simulation experiments, the proposed algorithm is validated. The results show that it maintains the same attitude determination accuracy and robustness with conventional methodologies but significantly reduces the computation time.


---

## Body

## 1. Introduction

In aerial applications, strapdown vector sensors are often required to directly give the rotation matrix between the body frame and inertial frame [1]. With the development of modern global navigation satellite systems (GNSS), GNSS-based attitude determination has been a vital part in spacecraft instrumentation and measurement [2]. This leads to different algorithms of attitude determination on various platforms, for example, remote sensing and marine navigation [3, 4]. In such applications, the attitude determination from vector observation pairs is usually an effective solution [5, 6]. This approach is referred to as the Wahba problem, posed in 1965, which minimizes the following loss function
(1)LR=12∑i=1naiDib−RDir2,in which Dib and Dir are the ith pair of vector observation that was obtained in the body frame and reference frame, respectively [7]. ai is the positive weight of the ith pair with the sum of 1. Here, R is the 3-dimensional direction cosine matrix (DCM) belonging to SO(3) [8]. Initial research on this problem mainly focuses on the exact solution rather than its robustness [9]. For instance, the above loss function can be explicitly given by the augmented rotation equations. Brute force calculation of such a system requires a large matrix memory and its sophisticated generalized inverse. Such solution is not only time and space consuming, but it will also cause nonorthogonality of the DCM. This in fact has a demand on later matrix correction that deduces the optimality of the solution. A robust framework was proposed by Davenport in 1968, who analytically changed the Wahba problem into an eigenvalue problem [9]. The target, that is, DCM, is shifted to an attitude quaternion accordingly. Following this formulation, many famous attitude estimators were then proposed in the next 30 years, including Shuster’s QUaternion ESTimator (QUEST), Markley’s Fast Optimal Attitude Matrix (FOAM), Mortari’s ESOQ, and Wu’s FLAE [10–13].The robust solutions to Wahba’s problem provides a possibility of accurate attitude determination from sensors like the sun sensor, nadir sensor, star tracker, magnetometer, and so on [14]. It can significantly improve the attitude estimation results combined with gyroscopes [15–17]. Apart from Davenport’s method, it is also noticeable that the singular value decomposition (SVD) could be efficient. Markley proposed the SVD-based Wahba solution in 1988 where the DCM is composed of decomposition results from the SVD [18]. The SVD algorithm is proven to be very robust, hence facing some critical cases of sensor combinations, where its accuracy is always maintained. However, conventional SVD computation is very complicated. Could there be a fast analytical SVD algorithm that can boost its execution on embedded platforms?The answer is positive. As a matter of fact, a 3-matrix SVD can be given with much faster speed. And in this paper, we focus on presenting a novel fast SVD algorithm. The derivations are given to show why it is fast. Yet simulations are carried out to compare the proposed algorithm with representative ones on accuracy, robustness, and time consumption, which reflect its superiority in real applications.This paper is structured as follows: Section2 contains our fast SVD theory and its relationship with optimal attitude determination. Section 3 includes the experimental results. Section 4 consists of concluding remarks.


## 2. Our Method

The introduced matrix theory in this paper is mainly referenced from [19]. Given an arbitrary nonsingular real squared matrix A, the matrix B=ATA is real symmetry. One can compute A’s singular values with B’s eigenvalues λ1≥λ2≥λ3>0 with σi=λi, i=1,2,3. The Householder transform and QR factorization are usually adopted for SVD. However, in real applications, they are proven to be time-consuming. Here, we define that matrix A is decomposed by
(2)A=UDV,where D=diagσ1,σ2,σ3 and U and V are 3 × 3 orthonormal matrices. That is to say, the factorization process of A can be significantly simplified if the preliminary decomposition of B is completed. Actually, in scientific computations, such technique is very common which uses the QR decomposition or Jacobi method imposed on B.Now we propose the analytic eigenvalue decomposition ofB. The characteristic polynomial of B is given by
(3)fλ=detλI−B=λ−Β11−B12−B13−B12λ−Β22−B23−B13−B23λ−Β33=λ−Β11λ−B22λ−B33−B232+B12−B12λ−B33−B13B23−B13B12B23+λ−B22B13=λ3−B11+B22+B33λ2+B11B22+B11B33+B22B33−B232−B122−B132λ−B11B22B33−B11B232−B122B33+2B12B13B23−B132B22=λ3+aλ2+bλ+c,where
(4)a=−B11+B22+B33,b=B11B22+B11B33+B22B33−B232−B122−B132,c=−B11B22B33−B11B232−B122B33+2B12B13B23−B132B22,with Bij denoting the element of B in the ith row and jth column. Letting λ=x−a/3, fλ=0 can be converted to
(5)x3+px+q=0,where
(6)p=3b−a23,q=2a3−9ab+27c27.The discriminant is established by(7)Δ=p33+q22.As far as the symbol of the discriminant is concerned, there are three specific situations:(1)
Δ>0: there are one real and a pair of conjugate complex roots.
(8)x1=k1+k2,x2,x3=−12x1±3j2k1−k2,where
(9)k1=12−4q+8Δ3,k2=12−4q−8Δ3,in which j denotes the unitary imaginary number.
(2)
Δ=0: there are three real roots in which two of them are equal to each other.
(10)x1=−4q3orx1=−signq23−3p,x2=x3=−x12.
Especially whenp=q=0, the roots are zeros.
(3)
Δ<0: there are three different real roots.
(11)x1=23−3pcosθ3,x2,x3=23−3pcosθ±2π3,where
(12)θ=arccos9q2p−3p.Based on the three conditions and inserting the roots intoλ=x−a/3, B’s eigenvalues can be obtained. Here we assume that they are sorted in descending order described as before. Note that B is really symmetric. Then its eigenvalues are real numbers, that is, the roots of (5) are real as well. Hence only the abovementioned cases (2) and (3) would be adopted. Apart from this, according to the Vieta theorem [20], the eigenvalues are associated with
(13)λ1+λ2+λ3=−a,λ1λ2+λ2λ3+λ3λ1=bλ1λ2λ3=−c.Since the eigenvalues are assumed to be real, their maximum condition number could be−a, while the minimum one is that of −c/b. Now we define the condition number of the characteristic polynomial equation fλ=0 as
(14)condfλ=abc.Before computations, it is suggested that the users check the condition number first. If the condition number is too large, that means the numerical stability of the calculated results are not satisfactory. In such circumstance, we may resort to a more robust way, for example, the Jacobi method.Based on the matrix theory, the orthogonal eigenvectors corresponding to the above-computed eigenvalues can be determined as follows:(1)
The eigenvalues have a multiplicity of 3λ1=λ2=λ3. Three orthonormal eigenvectors can be immediately established by
(15)v1=100,v2=010,v3=001.
(2)
The eigenvalues have a multiplicity of 2λ1=λ2>λ3orλ1>λ2=λ3. The eigenvector associated with the eigenvalue λ2 meets
(16)λ2I−Bv2=0.
LettingC=λ2I−C, we have rankC=1, that is, the three row vectors of R are linearly correlated. Assuming that the maximum element of R is Rij, it is obtained that
(17)Civ=Ci1Ci2Ci3v=0,where Ti is the ith row vector of the matrix C. If j=1 or j=2, the eigenvector can be directly established by
(18)v2=Ci2−Ci10T.
And ifj=3, we have
(19)v2=0−Ci3Ci2T.
Whenλ1=λ2>λ3, the eigenvectors are composed by
(20)v1=Ci×v2,v3=v1×v2,while when λ1>λ2=λ3, the eigenvectors are composed by
(21)v3=Ci×v2,v1=v2×v3.
(3)
The eigenvalues are all different: the eigenvector associated with the eigenvalueλ1 meets
(22)λ1I−Bv2=0.LettingC=λ1I−B, we have rankC=2, which describes that the three row vectors of C should be on the same plane, with the preliminary definition that the zero vector can be on an arbitrary plane. Note that at least two of them are not parallel with each other. Here we define
(23)vt1=C1×C2,vt2=C2×C3,vt3=C1×C3.Theoretically,vt1,vt2,vt3 are parallel. Every one of them can be the eigenvector only if it is not a zero vector. In engineering practice, the item with the maximum norm is usually chosen for computing the eigenvector v1 belonging to the eigenvalue λ1. Using the same technique, the eigenvector v2 can be computed. While for v3, it can be given by v3=v1×v2.Finally the eigenvectorsv1,v2,v3 are normalized. Letting
(24)V=v1v1v2v2v3v3,one can write
(25)BV=VΛ,in which
(26)Λ=diagλ1λ2λ3.InsertingB=ATA and Λ=D2 into (26), we obtain
(27)ATAV=VD2,which is transformed into
(28)A=AT−1VDDVT=UDVT.ExtractingU=AT−1VD=AATA−1VD=AVD2VT−1VD=AVD−1, we can easily verify that U is an orthogonal matrix, namely, UTU=AT−1VDTAVD−1=DVTA−1AVD−1=I. Now, we have finished the presentation of the proposed SVD method.The proposed SVD method can be applied to productions in which the computation resources are highly restricted. Relating to the attitude determination, we hereinafter introduce two propositions.Proposition 1.
For the 3 × 3 nonsingular matrix A, if its SVD is A=UDVT, C=UVT is one of the closest orthonormal matrices to A, such that
(29)argminCJC=argminCC−A2=argminC∑i=13∑j=13Cij−Aij2.Proof 1.
(46) is equivalent to
(30)argminCJC=argminCtrC−ATC−A.
With the consideration ofCTC=I, introducing Lagrange multipliers, the Lagrangian is developed by
(31)Jo=trC−ATC−A+ΘCTC−I,where Θ is the symmetric Lagrange multiplication matrix. Using differential rules of matrix
(32)∂∂XtrATX=∂∂XtrXTA=AT,∂∂XtrAXTX=2AX.
The above Lagrangian is optimized by(33)∂Jo∂C=∂∂CtrCTC−CTA−ATC+ATA+ΘCTC−Θ=∂∂CtrI+ΘCTC−CTA−ATC+ATA−Θ=2I+ΘCT−AT=0,producing
(34)A=CI+Θ.
Then we have(35)ATA=CI+ΘTCI+Θ=I+ΘCTCI+Θ=I+Θ2,which arrives at
(36)I+Θ=±ATA1/2,
Inserting (36) into (34), C is determined by
(37)C=±AATA−1/2.
WhenA approximates an attitude matrix, that is, detA≈+1, we choose the positive symbol, which gives
(38)C=AATA−1/2=UDVTUDVTTUDVT−1/2=UDVTVDDVT−1/2=UDVTUDVTUDVT−1/2=UDVTUDVT−1/2=UVT.If we rewrite the SVD into the following form(39)A=UDVT=UVT·VDVT=CS,where C=UVT and S=VDVT are a symmetric positive definite matrix, that is, detS>0, then (37) represents the orthonormal symmetric decomposition.Proposition 2.
For the 3 × 3 nonsingular matrix A, if its SVD is A=UDVT, the choice C=UVT makes the following function maximum:
(40)JC=trCAT=max.Proof 2.
(38) is equivalent to
(41)JC=trCAT=trCUDVTT=trCVDUT=trUTCV⋅D=trC∗D,where
(42)C∗=UTCV.
Then we have(43)JC=trC∗D=trC11∗C12∗C13∗C21∗C22∗C23∗C31∗C32∗C33∗σ1000σ2000σ3=C11∗σ1+C22∗σ2+C33∗σ3≤σ1+σ2+σ3.
Sinceσ1≥σ2≥σ3>0, the above “equal” holds if and only if C11∗=C22∗=C33∗=1, which corresponds to C∗=I.
Finally (42) arrives at
(44)C=UVT.It has been proposed by Arun et al. in 1987 [21] and Markley in 1988 [18], respectively, that for equal-weight and normal-weight cases the optimal DCM can be computed with
(45)R=VRMRURT,MR=diag1,1,detURdetVR,with the given
(46)BR=∑i=1naiDibDirT=URDRVRT.The Wahba problem can be shifted to maximizing [22]
(47)argminRRT=I,detR=+1JR=argminRRT=I,detR=+1trRBRT.According to the above propositions, the optimal DCM is calculated by(48)R=URTVR.The proposed fast SVD is now summarized in the MATLAB coding (see Algorithm1).Algorithm 1: MATLAB code of the proposed fast SVD algorithm.
1:
% Optimal Attitude Determination Algorithm Using Fast Singular Value Decomposition
% author: liuzhuohua@bupt.edu.cn; jin_wu_uestc@hotmail.com
function [u1,u2, u3,   …
d1, d2, d3,  …
v1, v2, v3] = svd3_fast (A,   …
A11, A12, A13,   …
A21, A22, A23,   …
A31, A32, A33)
B11 = A11 ∗ A11 + A21 ∗ A21 + A31 ∗ A31;
B12 = A11 ∗ A12 + A21 ∗ A22 + A31 ∗ A32;
B13 = A11 ∗ A13 + A21 ∗ A23 + A31 ∗ A33;
B22 = A12 ∗ A12 + A22 ∗ A22 + A32 ∗ A32;
B23 = A12 ∗ A13 + A22 ∗ A23 + A32 ∗ A33;
B33 = A13 ∗ A13 + A23 ∗ A23 + A33 ∗ A33;
B12B23 = B12 ∗ B23;
B13B22 = B13 ∗ B22;
B13B12 = B13 ∗ B12;
B11B23 = B11 ∗ B23;
B12B12 = B12 ∗ B12;
B11B22 = B11 ∗ B22;
a = −(B11 + B22 + B33);
b =     B11B22 + B11 ∗ B33 + B22 ∗ B33 − B23ˆ2 − B12B12 − B13ˆ2;
c = −(B11B22 ∗ B33 − B11B23 ∗ B23 − B12B12 ∗ B33 + 2 ∗ B12B23 ∗ B13 − B13 ∗ B13B22);
a3 = a/3;
p = b − a ∗ a3;
q = (2 ∗ aˆ3–9 ∗ a ∗ b + 27 ∗ c)/27;
p2 = p ∗ p;
T = −q ∗sqrt(−27 ∗ p)/2/p2;
theta3 =acos(T)/3;
pi23 = 2.094395102393195;
x0 = 2 ∗sqrt(−p/3);
x1 = x0 ∗cos(theta3);
x2 = x0 ∗cos(theta3 − pi23);
x3 = x0 ∗cos(theta3 + pi23);
rs1 = x1 − a3;
rs2 = x2 − a3;
rs3 = x3 − a3;
if rs1 < rs2, tmp = rs2; rs2 = rs1; rs1 = tmp; end
if rs1 < rs3, tmp = rs3; rs3 = rs1; rs1 = tmp; end
if rs2 < rs3, tmp = rs2; rs2 = rs3; rs3 = tmp; end
v1 = [B12B23 − B13B22 + rs1 ∗ B13
B13B12 − B11B23 + rs1 ∗ B23
(rs1 − B11) ∗ (rs1 − B22) − B12B12];
v2 = [B12B23 − B13B22 + rs2 ∗ B13
B13B12 − B11B23 + rs2 ∗ B23
(rs2 − B11) ∗ (rs2 − B22) − B12B12];
v1 = v1/norm(v1);
v2 = v2/norm(v2);
v3 = [v1 (2) ∗ v2 (3) − v1 (3) ∗ v2 (2);
v1 (3) ∗ v2 (1) − v1 (1) ∗ v2 (3);
v1 (1) ∗ v2 (2) − v1 (2) ∗ v2 (1)];
d1 =sqrt(rs1);
d2 =sqrt(rs2);
d3 =sqrt(rs3);
u1 = A ∗ v1/d1;
u2 = A ∗ v2/d2;
u3 = A ∗ v3/d3;
end


## 3. Simulation Results

In this subsection, we conduct two simulations to verify the accuracy and efficiency of the proposed fast SVD method.


### 3.1. Static Simulation

The vector observation pairs are simulated with(49)Dib=RtrueDir+εi,i=1,2,…n,in which the noise items εi with different indexes are independent from each other and subject to Gaussian distribution. The reference DCM Rtrue here is chosen as
(50)Rtrue=0.236440.18118−0.95460−0.105270.981450.160200.965920.062610.25113,which corresponds to the Euler angles of roll: −14°, pitch: 75°, yaw: 24°. Here the standard deviation of noise is defined as 0.01 for each axis.With this model, 10,000 groups of 3 pairs of vector observations are simulated for testing. The FLAE, QUEST, and Markley’s SVD method are employed for comparison. For FLAE, we here use the Newton iteration to compute the eigenvalues of Davenport matrixK. While for QUEST, we use both Newton iteration and direct factorization with the Eigen math library. The Eigen library is also adopted for calculation of Markley’s SVD method. We use the MATLAB software to evaluate the results, which are shown in Figure 1. We can see from the figure that the tested results of representative algorithms and the proposed method are all the same. Actually, since we have not broken the conventional framework of Wahba’s problem, the final accuracy are all the same. However, as is known, SVD has been recognized as a robust computation tool. The numerical stability of SVD is far beyond the eigenvalue factorization.Figure 1
Static test results.By taking more cases into account, detailed time consumption results are tested. The designed cases from Markley are employed (see Table1, [5]). The results are categorized as follows (Tables 2–4). The presented materials show that the proposed algorithm has the same attitude accuracy with the compared representative ones.Table 1
Test cases.
Case
Reference vectors
Noise standard deviations
1
D1r=1,0,0T,D2r=0,1,0T,D3r=0,0,1T
σ1=10−6,σ2=10−6,σ3=10−6
2
D1r=1,0,0T,D2r=0,1,0T
σ1=10−6,σ2=10−6
3
D1r=1,0,0T,D2r=0,1,0T,D3r=0,0,1T
σ1=0.01,σ2=0.01,σ3=0.01
4
D1r=1,0,0T,D2r=0,1,0T
σ1=0.01,σ2=0.01
5
D1r=0.6,0.8,0T,D2r=0.8,−0.6,0T
σ1=10−6,σ2=0.01
6
D1r=1,0,0T,D2r=1,0.01,0T,D3r=1,0,0.01T
σ1=10−6,σ2=10−6,σ3=10−6
7
D1r=1,0,0TD2r=1,0.01,0T
σ1=10−6,σ2=10−6
8
D1r=1,0,0T,D2r=1,0.01,0T,D3r=1,0,0.01T
σ1=0.01,σ2=0.01,σ3=0.01
9
D1r=1,0,0T,D2r=1,0.01,0T
σ1=0.01,σ2=0.01
10
D1r=1,0,0T,D2r=0.96,0.28,0T,D3r=0.96,0,0.28T
σ1=10−6,σ2=0.01,σ3=0.01
11
D1r=1,0,0T,D2r=0.96,0.28,0T
σ1=10−6,σ2=0.01
12
D1r=1,0,0T,D2r=0.96,0.28,0T
σ1=0.01,σ2=10−6Table 2
Roll RMSE (deg).
Case
SVD Eigen
Fast SVD
QUEST Eigen
QUEST Newton
FLAE Newton
1
4.3120 × 10−05
4.3120 × 10−05
4.3120 × 10−05
4.3120 × 10−05
4.3120 × 10−05
2
6.0836 × 10−05
6.0836 × 10−05
6.0836 × 10−05
6.0836 × 10−05
6.0836 × 10−05
3
4.3242 × 10−01
4.3242 × 10−01
4.3242 × 10−01
4.3242 × 10−01
4.3242 × 10−01
4
5.9031 × 10−01
5.9031 × 10−01
5.9031 × 10−01
5.9031 × 10−01
5.9031 × 10−01
5
4.4283 × 10−01
4.4283 × 10−01
4.4283 × 10−01
5.0414 × 10−01
2.4250 × 10+01
6
4.9071 × 10−03
4.9071 × 10−03
4.9071 × 10−03
4.9071 × 10−03
4.9071 × 10−03
7
8.0575 × 10−03
8.0575 × 10−03
8.0575 × 10−03
8.0575 × 10−03
8.0575 × 10−03
8
5.9527 × 10+01
5.9527 × 10+01
5.9527 × 10+01
5.9527 × 10+01
5.9527 × 10+01
9
7.7087 × 10+01
7.7087 × 10+01
7.7087 × 10+01
7.7087 × 10+01
7.7087 × 10+01
10
1.4617 × 10+00
1.4617 × 10+00
1.4617 × 10+00
4.7431 × 10+01
1.5650 × 10+00
11
2.0270 × 10+00
2.0270 × 10+00
2.0270 × 10+00
4.6515 × 10+01
2.9349 × 10+00
12
2.0594 × 10+00
2.0594 × 10+00
2.0594 × 10+00
3.6679 × 10+01
1.7381 × 10+01Table 3
Pitch RMSE (deg).
Case
SVD Eigen
Fast SVD
QUEST Eigen
QUEST Newton
FLAE Newton
1
4.0938 × 10−05
4.0938 × 10−05
4.0938 × 10−05
4.0938 × 10−05
4.0938 × 10−05
2
5.3539 × 10−05
5.3539 × 10−05
5.3539 × 10−05
5.3539 × 10−05
5.3539 × 10−05
3
4.0487 × 10−01
4.0487 × 10−01
4.0487 × 10−01
4.0487 × 10−01
4.0487 × 10−01
4
5.3909 × 10−01
5.3909 × 10−01
5.3909 × 10−01
5.3909 × 10−01
5.3909 × 10−01
5
4.0024 × 10−01
4.0024 × 10−01
4.0024 × 10−01
4.5559 × 10−01
1.2596 × 10+01
6
4.0103 × 10−05
4.0103 × 10−05
4.0103 × 10−05
4.0103 × 10−05
4.0103 × 10−05
7
5.2893 × 10−05
5.2893 × 10−05
5.2893 × 10−05
5.2893 × 10−05
5.2893 × 10−05
8
3.6948 × 10−01
3.6948 × 10−01
3.6948 × 10−01
3.6948 × 10−01
3.6948 × 10−01
9
4.5164 × 10−01
4.5164 × 10−01
4.5164 × 10−01
4.5164 × 10−01
4.5164 × 10−01
10
5.6876 × 10−05
5.6876 × 10−05
5.6876 × 10−05
5.6877 × 10−05
5.6876 × 10−05
11
5.7268 × 10−05
5.7268 × 10−05
5.7268 × 10−05
5.7270 × 10−05
5.7265 × 10−05
12
4.8652 × 10−01
4.8652 × 10−01
4.8652 × 10−01
7.9615 × 10+00
3.9358 × 10+00Table 4
Yaw RMSE (deg).
Case
SVD Eigen
Fast SVD
QUEST Eigen
QUEST Newton
FLAE Newton
1
4.3411 × 10−05
4.3411 × 10−05
4.3411 × 10−05
4.3411 × 10−05
4.3411 × 10−05
2
4.8769 × 10−05
4.8769 × 10−05
4.8769 × 10−05
4.8769 × 10−05
4.8769 × 10−05
3
4.3535 × 10−01
4.3535 × 10−01
4.3535 × 10−01
4.3535 × 10−01
4.3535 × 10−01
4
4.8474 × 10−01
4.8474 × 10−01
4.8474 × 10−01
4.8474 × 10−01
4.8474 × 10−01
5
2.5750 × 10−01
2.5750 × 10−01
2.5750 × 10−01
2.9318 × 10−01
1.7425 × 10+01
6
3.6295 × 10−05
3.6295 × 10−05
3.6295 × 10−05
3.6295 × 10−05
3.6295 × 10−05
7
4.8501 × 10−05
4.8501 × 10−05
4.8501 × 10−05
4.8501 × 10−05
4.8501 × 10−05
8
3.9564 × 10−01
3.9564 × 10−01
3.9564 × 10−01
3.9564 × 10−01
3.9564 × 10−01
9
5.0110 × 10−01
5.0110 × 10−01
5.0110 × 10−01
5.0110 × 10−01
5.0110 × 10−01
10
6.1477 × 10−05
6.1477 × 10−05
6.1477 × 10−05
6.1480 × 10−05
6.1482 × 10−05
11
6.1345 × 10−05
6.1345 × 10−05
6.1345 × 10−05
6.1343 × 10−05
6.1348 × 10−05
12
3.1358 × 10−01
3.1358 × 10−01
3.1358 × 10−01
5.1110 × 10+00
2.8665 × 10+00


### 3.2. Dynamic Simulation

In this subsection, the attitude is simulated using an attitude and heading reference system (AHRS) on which a MicroStrain 3DM-GX3-25 module is attached to the platform firmly. With a gimbal with angle encoders, the platform is rotated and the raw sensor data along with the reference angles are logged with 500 Hz. The reference angles are converted to DCMs that are inserted into (49) to simulate vector observations. With the abovementioned compared algorithms, the results are evaluated and plotted in Figure 2. The overall tests show that the proposed fast SVD method is not different compared with the representative methods. The superiority of the proposed method is going to be introduced in Subsection 3.3.Figure 2
Dynamic test results.


### 3.3. Computation Complexity

Several algorithms are tested with simulated pairs of vector observations. Here, in each round, there are 5 pairs of vector observations. The execution time consumption from different sources are collected with the internal timer of the MATLAB software and is shown in Figure3. It is obvious that the proposed fast SVD is the best one compared with the other ones. This is because the proposed algorithm seeks the intelligent way by taking the simplest analytical form of SVD of the 3 × 3 matrix.Figure 3
Time consumption results.The low time consumption of the proposed method can significantly increase the CPU schedule of the attitude determination system especially on embedded platforms. The reserved time can be used for emergency backup to enhance the overall stability [23].


## 3.1. Static Simulation

The vector observation pairs are simulated with(49)Dib=RtrueDir+εi,i=1,2,…n,in which the noise items εi with different indexes are independent from each other and subject to Gaussian distribution. The reference DCM Rtrue here is chosen as
(50)Rtrue=0.236440.18118−0.95460−0.105270.981450.160200.965920.062610.25113,which corresponds to the Euler angles of roll: −14°, pitch: 75°, yaw: 24°. Here the standard deviation of noise is defined as 0.01 for each axis.With this model, 10,000 groups of 3 pairs of vector observations are simulated for testing. The FLAE, QUEST, and Markley’s SVD method are employed for comparison. For FLAE, we here use the Newton iteration to compute the eigenvalues of Davenport matrixK. While for QUEST, we use both Newton iteration and direct factorization with the Eigen math library. The Eigen library is also adopted for calculation of Markley’s SVD method. We use the MATLAB software to evaluate the results, which are shown in Figure 1. We can see from the figure that the tested results of representative algorithms and the proposed method are all the same. Actually, since we have not broken the conventional framework of Wahba’s problem, the final accuracy are all the same. However, as is known, SVD has been recognized as a robust computation tool. The numerical stability of SVD is far beyond the eigenvalue factorization.Figure 1
Static test results.By taking more cases into account, detailed time consumption results are tested. The designed cases from Markley are employed (see Table1, [5]). The results are categorized as follows (Tables 2–4). The presented materials show that the proposed algorithm has the same attitude accuracy with the compared representative ones.Table 1
Test cases.
Case
Reference vectors
Noise standard deviations
1
D1r=1,0,0T,D2r=0,1,0T,D3r=0,0,1T
σ1=10−6,σ2=10−6,σ3=10−6
2
D1r=1,0,0T,D2r=0,1,0T
σ1=10−6,σ2=10−6
3
D1r=1,0,0T,D2r=0,1,0T,D3r=0,0,1T
σ1=0.01,σ2=0.01,σ3=0.01
4
D1r=1,0,0T,D2r=0,1,0T
σ1=0.01,σ2=0.01
5
D1r=0.6,0.8,0T,D2r=0.8,−0.6,0T
σ1=10−6,σ2=0.01
6
D1r=1,0,0T,D2r=1,0.01,0T,D3r=1,0,0.01T
σ1=10−6,σ2=10−6,σ3=10−6
7
D1r=1,0,0TD2r=1,0.01,0T
σ1=10−6,σ2=10−6
8
D1r=1,0,0T,D2r=1,0.01,0T,D3r=1,0,0.01T
σ1=0.01,σ2=0.01,σ3=0.01
9
D1r=1,0,0T,D2r=1,0.01,0T
σ1=0.01,σ2=0.01
10
D1r=1,0,0T,D2r=0.96,0.28,0T,D3r=0.96,0,0.28T
σ1=10−6,σ2=0.01,σ3=0.01
11
D1r=1,0,0T,D2r=0.96,0.28,0T
σ1=10−6,σ2=0.01
12
D1r=1,0,0T,D2r=0.96,0.28,0T
σ1=0.01,σ2=10−6Table 2
Roll RMSE (deg).
Case
SVD Eigen
Fast SVD
QUEST Eigen
QUEST Newton
FLAE Newton
1
4.3120 × 10−05
4.3120 × 10−05
4.3120 × 10−05
4.3120 × 10−05
4.3120 × 10−05
2
6.0836 × 10−05
6.0836 × 10−05
6.0836 × 10−05
6.0836 × 10−05
6.0836 × 10−05
3
4.3242 × 10−01
4.3242 × 10−01
4.3242 × 10−01
4.3242 × 10−01
4.3242 × 10−01
4
5.9031 × 10−01
5.9031 × 10−01
5.9031 × 10−01
5.9031 × 10−01
5.9031 × 10−01
5
4.4283 × 10−01
4.4283 × 10−01
4.4283 × 10−01
5.0414 × 10−01
2.4250 × 10+01
6
4.9071 × 10−03
4.9071 × 10−03
4.9071 × 10−03
4.9071 × 10−03
4.9071 × 10−03
7
8.0575 × 10−03
8.0575 × 10−03
8.0575 × 10−03
8.0575 × 10−03
8.0575 × 10−03
8
5.9527 × 10+01
5.9527 × 10+01
5.9527 × 10+01
5.9527 × 10+01
5.9527 × 10+01
9
7.7087 × 10+01
7.7087 × 10+01
7.7087 × 10+01
7.7087 × 10+01
7.7087 × 10+01
10
1.4617 × 10+00
1.4617 × 10+00
1.4617 × 10+00
4.7431 × 10+01
1.5650 × 10+00
11
2.0270 × 10+00
2.0270 × 10+00
2.0270 × 10+00
4.6515 × 10+01
2.9349 × 10+00
12
2.0594 × 10+00
2.0594 × 10+00
2.0594 × 10+00
3.6679 × 10+01
1.7381 × 10+01Table 3
Pitch RMSE (deg).
Case
SVD Eigen
Fast SVD
QUEST Eigen
QUEST Newton
FLAE Newton
1
4.0938 × 10−05
4.0938 × 10−05
4.0938 × 10−05
4.0938 × 10−05
4.0938 × 10−05
2
5.3539 × 10−05
5.3539 × 10−05
5.3539 × 10−05
5.3539 × 10−05
5.3539 × 10−05
3
4.0487 × 10−01
4.0487 × 10−01
4.0487 × 10−01
4.0487 × 10−01
4.0487 × 10−01
4
5.3909 × 10−01
5.3909 × 10−01
5.3909 × 10−01
5.3909 × 10−01
5.3909 × 10−01
5
4.0024 × 10−01
4.0024 × 10−01
4.0024 × 10−01
4.5559 × 10−01
1.2596 × 10+01
6
4.0103 × 10−05
4.0103 × 10−05
4.0103 × 10−05
4.0103 × 10−05
4.0103 × 10−05
7
5.2893 × 10−05
5.2893 × 10−05
5.2893 × 10−05
5.2893 × 10−05
5.2893 × 10−05
8
3.6948 × 10−01
3.6948 × 10−01
3.6948 × 10−01
3.6948 × 10−01
3.6948 × 10−01
9
4.5164 × 10−01
4.5164 × 10−01
4.5164 × 10−01
4.5164 × 10−01
4.5164 × 10−01
10
5.6876 × 10−05
5.6876 × 10−05
5.6876 × 10−05
5.6877 × 10−05
5.6876 × 10−05
11
5.7268 × 10−05
5.7268 × 10−05
5.7268 × 10−05
5.7270 × 10−05
5.7265 × 10−05
12
4.8652 × 10−01
4.8652 × 10−01
4.8652 × 10−01
7.9615 × 10+00
3.9358 × 10+00Table 4
Yaw RMSE (deg).
Case
SVD Eigen
Fast SVD
QUEST Eigen
QUEST Newton
FLAE Newton
1
4.3411 × 10−05
4.3411 × 10−05
4.3411 × 10−05
4.3411 × 10−05
4.3411 × 10−05
2
4.8769 × 10−05
4.8769 × 10−05
4.8769 × 10−05
4.8769 × 10−05
4.8769 × 10−05
3
4.3535 × 10−01
4.3535 × 10−01
4.3535 × 10−01
4.3535 × 10−01
4.3535 × 10−01
4
4.8474 × 10−01
4.8474 × 10−01
4.8474 × 10−01
4.8474 × 10−01
4.8474 × 10−01
5
2.5750 × 10−01
2.5750 × 10−01
2.5750 × 10−01
2.9318 × 10−01
1.7425 × 10+01
6
3.6295 × 10−05
3.6295 × 10−05
3.6295 × 10−05
3.6295 × 10−05
3.6295 × 10−05
7
4.8501 × 10−05
4.8501 × 10−05
4.8501 × 10−05
4.8501 × 10−05
4.8501 × 10−05
8
3.9564 × 10−01
3.9564 × 10−01
3.9564 × 10−01
3.9564 × 10−01
3.9564 × 10−01
9
5.0110 × 10−01
5.0110 × 10−01
5.0110 × 10−01
5.0110 × 10−01
5.0110 × 10−01
10
6.1477 × 10−05
6.1477 × 10−05
6.1477 × 10−05
6.1480 × 10−05
6.1482 × 10−05
11
6.1345 × 10−05
6.1345 × 10−05
6.1345 × 10−05
6.1343 × 10−05
6.1348 × 10−05
12
3.1358 × 10−01
3.1358 × 10−01
3.1358 × 10−01
5.1110 × 10+00
2.8665 × 10+00


## 3.2. Dynamic Simulation

In this subsection, the attitude is simulated using an attitude and heading reference system (AHRS) on which a MicroStrain 3DM-GX3-25 module is attached to the platform firmly. With a gimbal with angle encoders, the platform is rotated and the raw sensor data along with the reference angles are logged with 500 Hz. The reference angles are converted to DCMs that are inserted into (49) to simulate vector observations. With the abovementioned compared algorithms, the results are evaluated and plotted in Figure 2. The overall tests show that the proposed fast SVD method is not different compared with the representative methods. The superiority of the proposed method is going to be introduced in Subsection 3.3.Figure 2
Dynamic test results.


## 3.3. Computation Complexity

Several algorithms are tested with simulated pairs of vector observations. Here, in each round, there are 5 pairs of vector observations. The execution time consumption from different sources are collected with the internal timer of the MATLAB software and is shown in Figure3. It is obvious that the proposed fast SVD is the best one compared with the other ones. This is because the proposed algorithm seeks the intelligent way by taking the simplest analytical form of SVD of the 3 × 3 matrix.Figure 3
Time consumption results.The low time consumption of the proposed method can significantly increase the CPU schedule of the attitude determination system especially on embedded platforms. The reserved time can be used for emergency backup to enhance the overall stability [23].


## 4. Conclusion

In this paper, an analytical singular value decomposition (SVD) of a 3 × 3 matrix is proposed. The algorithm is based on the SVD of a real symmetric matrix and the roots to its characteristic polynomial. Detailed derivations of roots to this cubic equation and further compositions of eigenvectors are introduced. With the presented theory, any 3 × 3 real matrix can be factorized with much faster speed. This is then applied to the optimal attitude determination problem. Simulations are carried out which contains static and dynamic performance tests. The attitude accuracy results indicate that the proposed fast SVD approach has the same accuracy with conventional ones. However, since the computations are significantly reduced, the execution time of the algorithm is much more superior to other methods. It is believed that this algorithm may be introduced in embedded attitude determination systems in the future. However, when dealing with some critical cases, the proposed method may not be usable as the divisions may face NaN values.



---

*Source: 6308530-2018-06-27.xml*