Quaternion and Least Square
Quaternion and Least Square
415
that minimizes this nonlinear cost function is obtained and a new method for attitude determination from GPS
by iteration. Note that the measurement, i j in the phase measurements is derived.
Cohens cost function is a scalar value of single By using double differenced carrier phase
differenced carrier phases measured by two GPS measurements between a pair of antennas and a pair of
receivers associated with baseline i and satellite j. This satellites, the user can determine the relative range
type of cost function is only applicable to the specially between the pair of antennas very precisely with a mm-
dedicated GPS antenna array which contains more than level accuracy. Baseline vectors thus obtained from an
three GPS receiver hardware modules connected to one array of three or more GPS antennas and multiple
common oscillator to eliminate the receiver-dependent satellites can be used as observations for estimating the
clock bias error. full three-axis attitude of a rigid body.
The other approach is to determine the attitude It is assumed in this paper that baseline vectors of
parameters from the estimated baseline vectors of an the GPS antenna array are estimated in the WGS-84
antenna array. The motivation behind this approach is to frame ( x, y, z ) and transformed into the local-level
use the well-established result of precise relative frame ( N , E , D ). Either a transformation matrix or a
positioning technique with GPS carrier phase set of orientation parameters defined in the local-level
measurements. Once the baseline vectors are obtained, frame is estimated. The attitude determination
the attitude determination becomes much simpler. In this algorithms are described on the assumption that the
approach the attitude determination problem may be measured directions depend explicitly on the attitude
considered as the problem of finding a proper 3-by-3 alone not on other component such as sensor bias [17,
orthogonal transformation matrix which minimizes the 18]. The procedure for determining attitude from GPS
Wahbas loss function. measurements is summarized in Figure 1.
In this paper, an alternative approach is presented to
determine the four elements of the quaternion directly Wahbas Problem
from the baseline vector measurements instead of The problem of attitude determination from vector
finding 3-by-3 transformation matrix. An instantaneous measurements can be considered as the problem of
least-squares solution method is derived. This alternative determining an orthogonal transformation matrix A in
method extends the Wahbas problem into the nonlinear the following relationship:
least-squares estimation problem with vectorized
measurements and constraint equation. r i = A l i + ei , for i=1,,n (2)
The organization of this paper proceeds as follows.
After a general loss function is described for GPS
where l i denotes the vector representation of the
attitude determination from vector observations, a
simple nonlinear least-squares estimation technique is direction to some observed object in the local-level
introduced for estimating quaternion parameters. frame, r i is the previously defined vector representation
Constraint solutions in terms of quaternion norm are of the corresponding observation in the vehicle body
also described. It is shown that the Wahbas cost frame, ei represents an error vector, and n is the number
function is equivalent to a special case of the cost of vector observations.
function defined in this paper. Simulation results for a Wahba posed the problem of finding the proper
moving vehicle with near-optimal and nonorthogonal orthogonal matrix A that minimizes the loss function
baseline configurations are described. defined as [10]:
In this section, the problem of attitude where i represents a weighting factor. By simple
determination from vector measurements is reviewed
416
matrix manipulations, this Wahbas problem is q 02 + q12 q 22 q 23 2 (q 3 q 0 + q1 q 2 ) 2(q 1q 3 q 0 q 2 )
equivalent to the problem of finding the proper A = 2(q1 q 2 q 3 q 0 )
T
q 02 q 12 + q 22 q32 2(q 0 q 1 + q3 q 2 )
orthogonal matrix A that maximizes the trace of the 2 (q 0 q 2 + q 1q 3 ) 2 (q 2 q3 + q 0 q1 ) q 0 q1 q 2 + q 3
2 2 2 2
matrix product ABT where the matrix B is given by
(6).
n
B = i ri l i
T (4).
Substitution of the transformation matrix A of Eq. (6)
i= 1
into Eq. (2) for any i and with some manipulation yields
another nonlinear observation equation in terms of
Davenport suggested a solution to the Wahbas quaternion parameters as:
quadratic loss function by utilizing quaternion method
[11]. Many practical attitude determination algorithms
l N i = ( q20 + q12 q22 q32 ) rxbi + 2(q3 q0 + q1 q2 )rybi + 2(q1q3 q0 q2 )rzbi
have been developed based on this method. This method
l Ei = 2(q1 q2 q3 q0 )rxbi + ( q02 q12 + q22 q32 )rybi + 2(q0 q1 + q3 q2 )rzbi
by Davenport, called the q-method, directly led to an
eigenvalue equation for quaternion by Keat [12] and the l Di = 2(q0 q2 + q1 q3 ) rxbi + 2(q2 q3 + q0 q1 )rybi + (q02 q12 q22 + q32 ) rzbi
QUaternion ESTimator (QUEST) by Shuster [13]. The (7)
QUEST was derived by using either Gibbs vector or
quaternion. Markley suggested an algorithm based on which can be written in a vector form as:
the Singular Value Decomposition (SVD) method
focusing on theoretical analysis and robust computing l i = hi ( X q ) + w i (8)
[14]. The algorithm requires extra computation for the
singular value decomposition. He also presented two
improved SVD solutions to the Wahbas problem, where X is the vector of unknowns (q0 , q1 , q2 , q3 ) T .
q
known as Fast Optimal Attitude Matrix (FOAM) and Linearization of Eq. (8) about nominal values yields
Slower Optimal Matrix Algorithm (SOMA), without
performing the singular value decomposition [15]. A l i q = H i q X q + v i (9)
more complete survey of other attitude representations is
given in Reference 16.
where liq is the vector of corrections of the
Quaternion-based Least-squares Method
observations, X q represents the corrected unknown
Given a set of reference vectors and a
corresponding set of directional measurements between quaternion vector and H is the matrix of partial
iq
a body axis and these reference vectors, the problem of
derivatives which has the general form as:
three-axis attitude determination can be formulated as a
linear least squares problem with norm constraint on the
hN hN h N h N
solution [19]. The attitude determination algorithm
based on a simple iterative least-squares can easily be hi ( X q ) q 0 q 1 q2 q 3 (10)
h h E h E hE
derived for three element parameters of Euler angle [3]. Hiq = |X = X * = E
X q q q q q 1 q2 q 3
The transformation matrix derived from Euler angles h0 h D h D h D
can take any of the 12 possible forms depend on the D
sequence of rotation angles. Since the attitude matrix q 0 q 1 q2 q 3
q0 q 1
1 x 2 y 3 z
solved by using a quaternion parameterization of the
rotation matrix. In this section, a complete solution hN , h N = 2 (q r b q r b + q r b ) ,
= 2 ( q 2 rxbi + q1 rybi + q 0 rzbi ) i i i
q 2 q3
3 x 0 y 1 z
algorithm for estimating quaternion parameters based on
least-squares with quaternion constraints is developed. hE , hE ,
= 2 (q 3 rxbi + q 0 rybi q 1r zbi ) = 2 (q 2 r xbi q1 r ybi q 0 rzbi )
The solution of the proposed algorithm is equivalent to q 0 q1
the solution of the Wahbas problem in the case hE , hE = 2(q r bi q r bi + q r bi ) ,
= 2 (q1 rxbi + q 2 r ybi + q 3 rzbi )
q 2 q 3
0 x 3 y 2 z
explained later.
hD , hD = 2(q r bi + q r bi q r bi ) ,
= 2( q 2 rxbi + q 1 rybi + q 0 rzbi )
The four elements of quaternion parameters used in q0 q 1
3 x 0 y 1 z
417
a1 a2 a3 a4 especially in the field of attitude determination, where
(11).
H i q = a 4 a3 a2 a1 one often has to solve a linear system with normalization
a 3 a4 a1 a 2 constraint, is to solve the unconstrained linear system
and then to normalize the solution to unity [19]:
The attitude solution can be obtained by ~
X q = X q X q (18).
expanding Eq. (9) to a matrix form and applying a
simple iterative least-squares with the following
correction equation: Cost Function of Least-squares
This section shows that the Wahbas problem is a
X q = (H q Q 1 H q ) 1 H q Q 1 L q
T T (12), special case of our nonlinear least-squares problem. For
a multi-baseline situation, the cost function of nolinear
where least-squares in terms of four elements of quaternion
[
Lq = l 1q
T
l2q
T
lm q ,
T T
] parameters in Eq. (12) is given in a matrix form as:
= [H ], 1
(L h( X ))T W (L h( X ))
T
Hq T
1q H 2Tq H mT q J (X) = (18)
2
[
H c = 2q02 2q12 2q22 2q32 . ] C X = diag{ ATX , A TX , , A TX } ,
The correction vector for the iterative least-squares and ATX is an attitude transformation matrix expressed in
solution and its covariance matrix are then given by [21] terms of unknowns ( q0 , q1 , q 2 , q4 ) as in Eq. (6),
~
[
X q = (H T H )1 H TL + H Tc {H c (H T H )1 H Tc }1{1 Hc ( H T H )1 HL} ] respectively. If we assume that the weighting matrix W
has the form of a diagonal matrix as W=diag{W1 ,
(15) W2 ,,Wm } where Wi is a weighting matrix for individual
~
[
Cov(Xq ) = 2 (H T H )1 (H T H )1 Hc1{Hc (H T H )1 Hc1}1 H c (HT H )1 ] baseline, then the matrix form of cost function in Eq.
(16). (19) can be rewritten in terms of the individual attitude
matrix, ATX as:
The covariance of the attitude estimate in Eq. (16) is a
statistical measure of the estimation errors arising from J (X) =
1 m
2 i=1
( T
) (
l i ATX r i Wi l i ATX r i ) (20).
errors in the reference and observation vectors. The
relationship between constraint least-squares and
unconstrained least-squares is given by If the weighting matrix for each baseline measurement is
a scalar times the identity matrix, i.e., Wi = 1 i2 I , where
~
X q = X q ( H T H ) 1 H cT { H c ( H T H ) 1 H c } 1 ( H c X q 1) (17). I denotes the 3-by-3 identity matrix, the resulting cost
function of Eq. (20) reduces to
The solution of constrained least-squares approach 2
1 m 1
described in Eqs. (15) and (17) provides more accurate J (X) = 2 l i ATX r i
2 i=1
(21).
results than the unconstrained least-squares in Eq. (12)
in general, although the difference is often negligible.
The constrained solution takes considerably more As the result, the cost function of nonlinear least-squares
computations than the non-constained solution. This in attitude determination problem in Eq. (21) is
additional computational time can be reduced by using a equivalent to that of Wahbas problem in Eq. (2). The
simple ad hoc approach. A common ad hoc approach, Wahbas cost function is a special case of nonlinear
418
least-squares problem that a vector measurement has one
same weighting factor, i , for the i-th baseline. If the CONCLUSIONS
baselines do not form an orthonormal basis, then the
attitude solution is suboptimal. In this paper, a new alternative form of attitude
estimation algorithm was proposed for the situation
SIMULATION when the observed vectors are the estimated baselines of
a GPS antenna array. The algorithm was first derived for
Three forms of our new algorithms for estimating an unconstrained nonlinear least-squares solution of the
the quaternion parameters; namely, the unconstrained four elements of quaternion parameters. Two variants of
least-squares (QULS), the constrained least-squares the algorithm with a constraint on quaternion norm were
(QCLS) and the constrained least-squares of ad hoc also given. The second variant of the solution based on
approach (QACLS) proposed by this paper and a an ad hoc constraint approach reduces computation
nonlinear least-squares algorithm for Euler angles (ELS) significantly with little loss in accuracy. It was shown
were coded in double precision MATLAB and executed that the cost function of the proposed least-squares
on a Pentium III 650 MHz computer with Window based attitude estimation algorithms is equivalent to that
Millennium Edition Operating System. In order to of Wahbas problem in a special case. The performance
compare their performance with other existing of the proposed algorithm was compared with existing
algorithms, Eight attitude determination algorithms were attitude determination algorithms via simulation.
also simulated. They are TM, VOM, TRIAD and
QUEST-based methods such as two versions of QUEST,
Table 1. Simulation Test Cases
SVD, FOAM and SOMA. Brief reviews, computational
steps and several simulation results of these attitude Body Frame Vector(m)
determination algorithms are summarized in Reference Case r
T
1
T
r2 (mm)
22. 1 [1, 0, 0] [0, 1, 0] 2
Four test cases were simulated as shown in Table 1. 2 [1, 0, 0] [0, 1, 0] 20
Each test cases were specified by a set of unity baseline 3 [1, 0, 0] [ 1 2 ,1 2 , 0] 2
vectors, r i and standard deviations of carrier phase 4 [1, 0, 0] [ 1 2 ,1 2 , 0] 20
measurements, . The VOM is not applicable to the
situation when r1 is not placed on the longitudinal axis. Table 2. Comparison of RMS Errors
Case 1 Case 2 Case 3 Case 4
Also it can accommodate only two observations; i.e., it yaw 0.101 0.901 0.088 0.979
does not use the additional redundant vector VOM pitch 0.245 2.670 0.191 2.660
measurements. The third measurement vector, roll 0.339 2.175 N/A N/A
r 3 = [0,0,1]T was used only in the TM in our simulation. yaw 0.101 0.901 0.089 0.979
TM pitch 0.262 2.922 0.217 2.867
Other algorithms use only two measurement vectors, r1 roll 1.369 2.198 0.308 3.700
and r 2 . The time history of the true Euler angles in the yaw 1.231 1.433 1.254 1.702
TRIAD pitch 11.305 11.813 11.240 11.988
simulation are shown in Figure 2. The performance of
roll 6.477 6.048 6.505 6.755
each algorithms are compared in terms of the root-mean- QUEST yaw 0.081 0.769 0.069 0.878
squared Euler angle errors as shown in Table 2. The based pitch 0.245 2.681 0.191 2.634
attitude error from TRIAD is relatively large because its methods roll 0.285 2.075 0.275 3.448
accuracy depends on the first choice of reference- yaw 0.081 0.812 0.068 0.877
observation vector pair. The QUEST-based methods QACLS pitch 0.246 2.445 0.190 2.634
such as two versions of QUEST, SVD, FOAM and roll 0.285 2.870 0.274 3.447
Unit : Deg
SOMA which were derived from Davenports q-method
have the same error characteristics as described in
Reference 14. The iteration process in the QUEST for
estimating quaternion caused large computation time to
converge to its global minimum in computing from
characteristic equation, p( ) . QULS, QCLS and QACLS
have the slightly different error characteristics, but not
significantly, mainly due to their different approaches in
dealing with the constraint condition. The QULS is
preferable if small deviations from orthogonality can be
tolerated. The output of ELS shows large errors, as
shown in Figure 3, when the yaw attitude angle
approaches 180 degrees because of the singularity
associated with the Euler expressions. In Figure 4 depict
the performances of the QUEST-based algorithms and Figure 2. True Euler angles
the proposed algorithms. It is shown in the figure that
the proposed algorithms have good performance
comparable to those of the most efficient existing
algorithms.
419
Rotations with Applications, NASA X-546-65-437, Nov.
1965.
[12] Keat, J., Analysis of Least Squares Attitude
Determination Routine DOAOP, CSC/TM-77/6034,
Computer Sciences Corporation, Lanham-Seabrook,
Maryland, Feb. 1977.
[13] Shuster, M.D. and Oh, S.D., Three-Axis Attitude
Determination from Vector Observation, Journal of
Guidance and Control, AIAA81-4003, Vol. 4, No. 1,
1981, pp.70-77.
[14] Markley, F.L., Attitude Determination Using Vector
Observation and the Singular Value Decomposition,
Journal of the Astronautical Science, Vol. 36, No. 3, Jul.-
Sept. 1988, pp.245-258.
[15] Markley, F.L., Attitude Determination Using Vector
Figure 3. Euler Angle Outputs of ELS
Observations: A Fast Optimal Matrix Algorithm, The
Journal of the Astronautical Sciences, Vol. 41, No. 2,
Apr.-Jun. 1993, pp. 261-280.
[16] Shuster, M.D., A Survey of Attitude Representations,
The Joutnal of the Astonautical Science, Vol. 41, No. 4,
Oct.-Dec. 1993, pp. 439-517.
[17] Markley, F.L., Attitude Determination and Parameter
Estimation Using Vector Observations: Theory, The
Journal of the Astronautical Sciences, Vol. 37, No. 1,
Jan.-Mar. 1989, pp. 41-58.
[18] Markley. F.L., Attitude Determination and Parameter
Estimation Using Vector Observations: Applications, The
Journal of the Astronautical Sciences, Vol. 39, No. 3, Jul.-
Figure 4. Comparison of Algorithms Sept. 1991, pp. 367-381.
[19] Gienger, G., Single Axis Attitude Determination by the
Linear Least Squares Method with Norm Constraint, AAS
REFERENCES 93-300, pp. 729-743.
[20] Siouris, G.M., Aerospace Avionics Systems: A Modern
[1] Lerner, G. M., Three-Axis Attitude Determination, Systhesis, Academic Press, Inc.,1993.
Spacecraft Attitude Determination and Control, J. R. [21] Koch, K.R., Parameter Estimation and Hypothesis Testing
Wertz (editor), D. Reidel, Dordrecht, Holland, 1978. in Linear Models, Springer-Verlag, 1987.
[2] Graas, F. V. and Braasch, M., GPS Interferometric [22]Won, J.H., Ko, S.J., Lee, J.S. and Cho, Y.D., Performance
Attitude and Heading Determination: Initial Flight Test Analysis of Attitude Determination Algorithms Based on
Results, Journal of the Institute of Navigation, Vol. 38, Vector Observations from GPS Antenna Array,
No. 4, Winter 1991-1992. Proceedings of 2000 GNSS Workshop, Seoul, Korea, Dec.
[3] El-Mowafy, A., Kinematic Attitude Determination From 2000.
GPS, Ph.D. Dissertation, The University of Calgary,
Calgary, Canada, Dec. 1994.
[4] Lu, G., Development of a GPS Multi-Antenna System for
Attitude Determination, Ph.D. Dissertation, The
University of Calgary, Calgary, Canada, Jan. 1995.
[5] Schleppe, J. B., Development of a Real-time Attitude
System Using a Quaternion Parameterization and Non-
Dedicated GPS Receivers, M. S. Degree Thesis, The
University of Calgary, Calgary, Canada, Jul. 1996.
[6] Cohen, C.E., Attitude Determination Using GPS, Ph.D.
Dissertation, Dept. of Aeronautics and Astronautics,
Stanford Univ., Standford, CA., Dec. 1992.
[7] Crassidis, J.L. and Markley, F.L., New Algorithm for
Attitude Determination Using Global Positioning System
Signals, Journal of Guidance, Control and Dynamics,
Vol. 20, No. 5, Sept.-Oct. 1997, pp. 891-896.
[8] Crassidis, J.L., Lightsey, E.G. and Markley, F.L.,
Efficient and Optimal Attitude Determination Using
Recursive Global Positioning System Signal Operations,
Journal of Guidance, Control and Dynamics, Vol. 22,
No. 2, March-April 1999, pp. 193-201.
[9] Crassidis, J.L., Markley, F.L. and Lightsey, E.G., Global
Positioning System Integer Ambiguity Resolution Without
Attitude Knowledge, Journal of Guidance, Control and
Dynamics, Vol. 22, No. 2, March-April 1999, pp. 212-
218.
[10] Wahba, G., A Least Squares Estimate of Spacecraft
Attitude, SIAM Review, Vol. 7, No. 3, Jul. 1965, p. 409.
[11] Davenport, P. B., A Vector Approach to the Algebra of 420