JOURNAL OF GUIDANCE, CONTROL, AND DYNAMICS

Vol. 27, No. 3, May–June 2004

Optimal-REQUEST Algorithm for Attitude Determination

D. Choukroun,∗ I. Y. Bar-Itzhack,† and Y. Oshman‡

Technion—Israel Institute of Technology, Haifa 32000, Israel

REQUEST is a recursive algorithm for least-squares estimation of the attitude quaternion of a rigid body using
vector measurements. It uses a constant, empirically chosen gain and is, therefore, suboptimal when filtering
propagation noises. The algorithm presented here is an optimized REQUEST procedure, which optimally filters
measurement as well as propagation noises. The special case of zero-mean white noises is considered. The solution
approach is based on state-space modeling of the K-matrix system and uses Kalman-filtering techniques to estimate
the optimal K matrix. Then, the attitude quaternion is extracted from the estimated K matrix. A simulation study
is used to demonstrate the performance of the algorithm.
Downloaded by UNIVERSITY OF TORONTO on February 1, 2015 | http://arc.aiaa.org | DOI: 10.2514/1.10337

Introduction The quaternion of rotation q is another popular attitude repre-

sentation (see Ref. 1, pp. 758 and 759 and Refs. 14 and 15 for
A TTITUDE determination (AD) is a major component of space-
craft operation. The fundamental AD problem is to specify
the orientation of the rigid-body spacecraft axes, expressed by a
extensive surveys of attitude parameterizations). It is a unit vector
in R4 , related to A by
Cartesian coordinate frame B with respect to a given reference
A(q) = (q 2 − eT e)I3 + 2eeT − 2q[e×] (3)
Cartesian coordinate system R. When a physical vector, say the
Earth magnetic field x is observed, a useful measurement model (see Ref. 1, p. 414) where e and q are the vector and the scalar
equation is part, respectively, of q; I3 is the 3 × 3 identity matrix; and the cross-
b = bo + δb = Ar + δb (1) product matrix [e×] is defined by
 
where A is the attitude matrix [also called the DCM matrix (see 0 −e3 e2
[e×] =  e3 −e1 


Ref. 1, p. 411)]; bo and r are the projections of x on B and R, 0 (4)
respectively; and δb is the measurement error. Given two obser- −e2 e1 0
vations, it is possible to estimate A by means of a deterministic
algorithm,2 whereas a single observation is not sufficient to yield an In 1968, Davenport devised a method, known in the literature as the
unambiguous attitude matrix (see Ref. 3, p. 23). q method, for computing the optimal single-frame quaternion q, that
In 1965, Wahba formulated the AD problem from vector obser- is, the quaternion which corresponds to the optimal A of Wahba’s
vations as a least-squares estimation problem4 : problem. As reported in Ref. 16, pp. A.1–A.11, the method is based
Given the two sets of n vectors {r1 , r2 , . . . , rn } and on the following identity:
{b1 , b2 , . . . , bn }, where n ≥ 2, find the proper orthogonal matrix A,
1

n
which brings the first set into the best least-squares coincidence with
the second. That is, find A, which minimizes ai bi − Ari 2 = 1 − qT K q (5)
2 i =1


n
bi − Ari 2 (2) where the K matrix in the right-hand side of Eq. (5) is ob-
i =1 tained as follows. Consider a batch of n simultaneous observations
bi , ri , i = 1, 2, . . . , n, and the corresponding weights ai , whose sum,
subject to the constraints A T A = I3 and det(A) = 1. with no loss of generality, equals one, that is,
This problem, known as the Wahba problem, is a single-frame
attitude determination problem; that is, it assumes that all vector

n

measurements that are processed to estimate the attitude have been ai = 1

obtained at the same attitude. One family of solutions is concerned i =1
with the determination of that optimal matrix A itself (see Refs. 5–9
for earlier solutions and Refs. 10–13 for more recent methods), Define the 3 × 3 matrices B and S, the 3 × 1 column matrix z, and
whereas another family is concerned with the determination of the the scalar σ as
corresponding optimal quaternion. In this paper we treat the latter.




n


B= ai bi riT , S = B + BT
Presented as Paper 2001-4153 at the AIAA 41st Guidance, Navigation, i =1
and Control Conference, Montreal, 6–9 August 2001; received 23 Decem-
ber 2002; revision received 1 November 2003; accepted for publication 12



n


November 2003. Copyright  c 2004 by the authors. Published by the Ameri- z= ai bi × ri , σ = tr(B) (6)
can Institute of Aeronautics and Astronautics, Inc., with permission. Copies i =1
of this paper may be made for personal or internal use, on condition that the
copier pay the $10.00 per-copy fee to the Copyright Clearance Center, Inc., where tr(·) denotes the trace operator. Then, the 4 × 4 symmetric
222 Rosewood Drive, Danvers, MA 01923; include the code 0731-5090/04 matrix K of Eq. (5) is
$10.00 in correspondence with the CCC.
∗ Graduate Student, Faculty of Aerospace Engineering, currently Assistant  
S − σ I3 z
Researcher Associate, University of California at Los Angeles, Department K = (7)
of Mechanical and Aerospace Engineering; [email protected]. Member AIAA. zT σ
† Sophie and William Shamban Professor, Member, Technion’s Asher
Space Research Institute, Faculty of Aerospace Engineering. Fellow AIAA. Note that the trace of K equals zero. It is clear from Eq. (5) that
‡ Associate Professor, Member, Technion’s Asher Space Research Insti- the constrained minimization of Wahba’s cost function is equiv-
tute, Faculty of Aerospace Engineering. Associate Fellow AIAA. alent to the constrained maximization of a quadratic form of q.
418
 CHOUKROUN, BAR-ITZHACK, AND OSHMAN 419

It is known that the problem of determining the stationary values where m k and m k + 1 are scalar coefficients that keep the largest
of a quadratic form on the unit sphere leads to the solution of an eigenvalue of K k + 1/k + 1 close to one24 ; that is, m k + 1 = ρk m k +ak + 1 ,
eigenvalue problem.17 As a result, the optimal quaternion estimate for k = 0, 1, . . . , and m 0 = a0 . The coefficient ρk is a fading memory
is the eigenvector of K that belongs to its largest positive eigen- factor (Ref. 26, pp. 285–288), which is equal to 1 if k is error free,
value. The highlights of the q method are that it optimally filters and is otherwise set between 0 and 1 according to the propagation
the measurement noises and that it requires neither linearization of noise. (Although REQUEST can handle a batch of new measure-
some equation model nor an initial quaternion. This method has ments, for our purpose we consider only one new measurement.)
become a very popular AD technique and has inspired numerous Treating the propagation noise using the fading memory concept
algorithms. Keat (Ref. 16, pp. A.15–A.34) presented a modified ver- makes REQUEST suboptimal. The aim of the present paper is to
sion of the power method18 for computing the sought eigenvector. introduce an algorithm, called Optimal-REQUEST, which is based
Shuster and Oh19 developed the QUEST (quaternion estimator) for on REQUEST and is able to optimally filter the propagation noise.
this problem. QUEST is a very popular algorithm for a single-frame The remainder of this paper is organized as follows: The problem
(SF) estimation of attitude quaternion. Mortari proposed two new that is solved in the present work is stated in the next section. Then,
SF algorithms, ESOQ20 (estimator of the optimal quaternion) and we lay the foundation necessary for the development of an opti-
ESOQ2.21 However, these algorithms, as any SF attitude estimator, mized REQUEST algorithm. The Optimal-REQUEST algorithm is
are memory-less algorithms, that is, the information contained in presented in the following section. The filter performance is then
measurements of past attitudes is lost. illustrated through a numerical example. Finally, we present our
Methods have been devised in order to relax the requirement for conclusions.
the measurements to be acquired simultaneously. These methods re-
Downloaded by UNIVERSITY OF TORONTO on February 1, 2015 | http://arc.aiaa.org | DOI: 10.2514/1.10337

quire knowing the angular velocity of B with respect to R. Markley22 Problem Statement
developed a batch algorithm using the body kinematics equation in
terms of the attitude matrix A(t). The highlights of this algorithm The choice of ρk is heuristic, making the filter REQUEST sub-
are that it provides an optimal predictor of A(t) and that, in addition, optimal. Moreover ρk is determined by considering the accuracy of
it can estimate a set of constant disturbance parameters, such as gyro the propagation stage only, disregarding the accuracy of the mea-
biases. Recursive estimators, which are more convenient than batch surement.
algorithms, have been developed, that perform sequential estimation We wish to optimize REQUEST by computing an optimal value of
of time-varying attitude,23,24 as well as other parameters.25 the parameter ρk in the update stage of REQUEST. The optimal value
In 1996, a discrete recursive algorithm named REQUEST that of ρk is that which yields an optimal blending of the a priori estimate
propagates and updates the K matrix was introduced.24 This algo- of the K matrix K k + 1/k and its new observation δ K k + 1 . Optimality
rithm is summarized next. It is known (Ref. 1, p. 564) that the body is achieved by minimizing a proxy measure of the uncertainty in the
angular motion can be described in terms of the attitude quaternion a posteriori estimate of the K matrix. [An exact expression of the
by the following difference equation: cost function to be minimized is defined later in Eqs. (57) and (58).]

qk + 1 = k qk (8) Mathematical Model

Solution Approach
where the transition matrix is computed as follows. Assume that
ω k is constant during the time increment
t, then the matrix k is The approach to computing an optimal gain ρk∗ consists of em-
expressed by (Ref. 1, pp. 511, 512) bedding REQUEST in the framework of Kalman filtering. With this
purpose in mind, we modify the formulation of the attitude estima-
k = exp(k
t) (9) tion problem. The central idea is that we would know the value of
the true attitude quaternion at any time had we known the value of
where k is the following skew-symmetric matrix: the true K matrix at any time. The true K matrix is the K matrix
that does not contain any error, neither from vector measurement
 

1 −[ω k ×] ωk noise, nor from propagation noise. Because we cannot know the true
k = (10) K matrix, we propose to estimate it in some optimal way.
2 −ω kT 0
A notation that is consistent with that of REQUEST is used here.
o
Denote by K i/j the true K matrix related to the attitude at ti , which is
The matrix k is a 4 × 4 orthogonal matrix function of the angular
based on the ideal noise-free vector measurements up to t j . The es-
velocity vector of the rotation of B with respect to R, ω k . Denote
timate of this matrix is denoted by K i/j . Similarly, the true K matrix
by K i/j the K matrix at time ti , which is constructed from the mea-
that is based on the noise-free vector measurements acquired at ti is
surements up to time t j . According to the REQUEST algorithm, the
denoted by δ K io . The measured K matrix, computed using the noisy
propagation of the K matrix from tk to tk + 1 is given by
vector measurements acquired at ti , is denoted by δ K i .
K k + 1/k = k K k/k kT (11) We consider the true K matrix as a state-matrix variable and de-
rive the dynamics and measurement equations that describe the state
Given a single observation at time tk + 1 , that is, rk + 1 and bk + 1 , one K -matrix system. The conventional model of a state-space system,
can construct the corresponding incremental K matrix, denoted by which includes the dynamics and measurement equations, is aug-
δ K k + 1 , as follows. First define mented by an additional equation that models a deterministic linear
combination of the noise-free K matrices K ko + 1/k and δ K ko + 1 at


time tk + 1 . As will be shown in the sequel, this combination yields
Bk + 1 = ak + 1 bk + 1 rkT+ 1 , Sk + 1 = Bk + 1 + BkT+ 1 another noise-free K matrix K ko + 1/k + 1 , which is the K matrix re-



lated to the true attitude at tk + 1 and is computed using all the vir-
zk + 1 = ak + 1 bk + 1 × rk + 1 , σk + 1 = tr(Bk + 1 ) (12) tual noise-free vector measurements up to tk + 1 . (We refer to these
vectors as imaginary vectors because in reality only the noisy mea-
then compute δ K k + 1 as surements are available to us.) Note that the structure of the compu-
  tation of K ko + 1/k + 1 fits the structure of the update stage equation in
1 Sk + 1 − σk + 1 I3 zk + 1 REQUEST [Eq. (14)]. The reason for this computation will become
δ Kk + 1 = (13)
ak + 1 zkT + 1 σk + 1 clear when defining the estimation errors and deriving their recur-
sive equations. As will be shown later, the updated estimation error
where ak + 1 is the scalar weighting coefficient of the (k + 1) obser- is used to define a special cost function. This cost function will be
vation. The update stage of REQUEST is of the form minimized with respect to the scalar gain ρk , yielding the sought
optimal gain ρk∗ . This gain will be used in the update stage of the
K k + 1/k + 1 = (ρk m k /m k + 1 )K k + 1/k + (ak + 1 /m k + 1 )δ K k + 1 (14) K -matrix estimation process, and, finally, an estimate of the attitude
 420 CHOUKROUN, BAR-ITZHACK, AND OSHMAN

quaternion will be computed as the eigenvector that belongs to the Based on this model we expand the expression for Wko in a Taylor
largest eigenvalue of the updated K -matrix estimate. series about ω ok , and, assuming that k  is small enough, retain
only the first-order term in k . Let B o k/k denote the true B matrix
Measurement Equation at time tk [see Eq. (6)], that is, B o k/k corresponds to the error-free
The measurement equation of the K matrix is derived as follows: vector observations up to tk . Defining B , S , κ , and z as
Consider the measurement equation for a single vector observation
at time tk + 1 , and assume that the reference unit vector rk + 1 is known


B = [k ×]Bk/k
o
, S = B + BT
exactly. Denoting by Ak + 1 the attitude matrix at tk + 1 and by δbk + 1
the noise vector that corrupts the measurement bk + 1 , we have


[z ×] = BT − B , κ = tr(B ) (24)
bk + 1 = Ak + 1 rk + 1 + δbk + 1 (15)
the first-order approximation in k for Wko is
Define the following quantities:
 


S − κ I3 z
Bb = ak + 1 δbk + 1 rkT + 1 , Sb = Bb T + Bb Wk =
t (25)
zT κ



zb = ak + 1 δbk + 1 × rk + 1 , κb = tr(Bb ) (16)
where
t = tk + 1 − tk . The proof of Eq. (25) is detailed in Ref. 27
Then, using these quantities, we can define a 4 × 4 symmetric ma- (pp. 233–235). To summarize, we have derived an approximate
Downloaded by UNIVERSITY OF TORONTO on February 1, 2015 | http://arc.aiaa.org | DOI: 10.2514/1.10337

trix, denoted by Vk + 1 , as follows: propagation equation of the true K matrix, which is

 
1 Sb − κb I3 zb
Vk + 1 = (17) K ko + 1/k = k K k/k
o
kT + Wk (26)
ak + 1 zbT κb

The matrix Vk + 1 is the error in the δ K measurement [see Eqs. (12) where k corresponds to ω k and Wk is an additive matrix noise. The
o
and (13)] at tk + 1 because initial true K matrix is denoted by K 0/0 . The expression for Wk is
provided in Eqs. (24) and (25). From these equations it is realized
δ K k + 1 = δ K ko + 1 + Vk + 1 (18) that Wk contains the noise vector k as well as all of the noise-free
vector pairs (bo , r) up to tk . Note that Wk is random because k is
where, as mentioned before, δ K k + 1 and δ K ko + 1 are, respectively, random. Also note that Wk is linear in the error k . This property will
the noisy and the noise-free K matrices of the new vector avail- become useful when deriving a stochastic model for Wk . Extensive
able at tk + 1 . Thus, δ K k + 1 is computed using the noisy observa- simulations show that the propagation model described by Eq. (26)
tion, bk + 1 , rk + 1 [see Eqs. (12) and (13)], while δ K ko + 1 contains the is a good approximation to the exact propagation model described
imaginary noise-free vector measurements at tk + 1 ; that is, δ K ko + 1 is by Eq. (21) when Wk is used instead of Wko .
expressed using Eqs. (12) and (13) for the noise-free observations
(i.e., δb = 0). Note that Vk + 1 is a linear function of δbk + 1 and rk + 1 ,
Pseudoprocess Equation
and, because δbk + 1 is random, Vk + 1 is also random. The linearity
of Vk + 1 in δbk + 1 will be used to derive a stochastic model for Vk + 1 As mentioned earlier, a supplementary equation is added to the
from the stochastic model of δbk + 1 . K -matrix model given in Eqs. (18) and (26). Motivated by the
REQUEST algorithm [see Eq. (14)], we define a pseudoprocess
as follows:
Process Equation
Let us denote by Bk and R, respectively, the body frame at time

L ok+1 = (1 − αk + 1 )(m k /m k + 1 )K ko + 1/k
tk and the constant reference frame. Let the 3 × 1 vector ω ok denote
the body axes angular rate vector of Bk with respect to R expressed + αk + 1 (δm k + 1 /m k + 1 )δ K ko + 1 (27)
in Bk , and let K 0/0
o
denote the noise-free (true) K matrix at time t0 . If
o
K 0/0 and ω ok are known, the true K matrix can be propagated using where αk + 1 is any real number in the interval [0, 1), the scalars
m k + 1 are recursively computed by
K ko + 1/k = ok K k/k
o
ok T (19)
o m k + 1 = (1 − αk + 1 )m k + αk + 1 δm k + 1 , m 0 = δm 0 (28)
where K k/k is related to the true attitude at tk and contains all of the
noise-free observations up to tk , K ko + 1/k is related to the true attitude
at tk + 1 and contains all of the noise-free observations up to tk , and for k = 0, 1 . . . , and δm k + 1 is the positive weight assigned to δ K k + 1 ,
ok is the 4 × 4 transition matrix corresponding to ω ok [see Eqs. (9) the pseudo K measurement at time k + 1. Because the pseudo K
and (10)]. In practice ω ok is not known but is, rather, measured (or measurement is constructed from a single b measurement, we choose
estimated). The transition matrix k , which is computed from the δm k + 1 = ak + 1 . Note that Eq. (27), which we call pseudoprocess
measured angular rate ω k , contains an error term
k , that is, equation, has a structure that is similar to that of the update stage of
REQUEST, given in Eq. (14). The pseudoprocess equation will be
ok = k −
k (20) central in the development of Optimal-REQUEST. Next we show
that L ok + 1 , an element of the pseudoprocess defined in Eq. (27), is
Using Eq. (20) in Eq. (19) yields a legitimate K matrix.
Proposition: For any value of αk + 1 in the interval [0, 1), the matrix
K ko + 1/k = k K k/k
o
kT + Wko (21) L ok + 1 given in Eq. (27) is a K matrix related to the true attitude at
where time tk + 1 .
Proof: By assumption, the two matrices K ko + 1/k and δ K ko + 1 con-

tain error-free vector observations; therefore, one of their eigen-
Wko = −k K k/k
o

kT −
kT K k/k
o
kT +
k K k/k
o

kT (22)
vectors is the true attitude quaternion qk + 1 , and it belongs to their
The expression for Wko in Eq. (22) is exact but not useful because maximal eigenvalue, which can be made equal to one by proper
we cannot infer from it any statistical property of Wko . To determine scaling. Thus,
a useful approximation to Wko , we assume that the error in ω k is
additive. Denoting this error by k , we have K ko + 1/k qk + 1 = qk + 1 (29)

ω k = ω ok + k (23) δ K ko + 1 qk + 1 = qk + 1 (30)
 CHOUKROUN, BAR-ITZHACK, AND OSHMAN 421

Using Eqs. (27–30), we find that As an example, consider the real 2 × 2 symmetric matrix of zero-
 mean processes x11 , x12 , and x22 , given by
L ok + 1 qk + 1 = (1 − αk + 1 )(m k /m k + 1 )K ko + 1/k
 
x11 x12
+ αk + 1 (δm k + 1 /m k + 1 )δ K ko + 1 qk + 1 X= (38)
x12 x22
= (1 − αk + 1 )(m k /m k + 1 )qk + 1 + αk + 1 (δm k + 1 /m k + 1 )qk + 1
The corresponding PX X matrix has the following form:
= {[(1 − αk + 1 )m k + αk + 1 δm k + 1 ]/m k + 1 }qk + 1 = qk + 1 (31)  
From Eq. (31) we conclude that qk + 1 is an eigenvector of L ok + 1
2
E x11 + E x12
2
E[x11 x12 ] + E[x12 x22 ]
PX X =   (39)
associated with the eigenvalue 1. Consequently, the matrix L ok+1 is E[x11 x12 ] + E[x12 x22 ] 2
E x12 + E x22
2
a legitimate K matrix. 2
As stated in the proposition, the matrix L ok + 1 is related to the true
attitude at time tk + 1 ; this matrix is indeed a correct K matrix at Discussion
time tk + 1 containing all of the noise-free vector measurements up PX X is a symmetric positive semidefinite matrix. The variances
to time tk + 1 . Adopting a consistent notation, we denote this matrix of each element of X are on the main diagonal of PX X , whereas the
by K ko + 1/k + 1 . The pseudoprocess equation is thus rewritten as off-diagonal elements of PX X contain only cross-covariance terms.
Consider the 4 × 1 zero-mean vector vecX , defined as
K ko + 1/k + 1 = (1 − αk + 1 )(m k /m k + 1 )K ko + 1/k
Downloaded by UNIVERSITY OF TORONTO on February 1, 2015 | http://arc.aiaa.org | DOI: 10.2514/1.10337

vecX = [x11 x12 x12 x22 ]T (40)
+ αk + 1 (δm k + 1 /m k + 1 )δ K ko + 1 (32)
It should be emphasized that Eq. (32) was developed to match the and construct its 4 × 4 covariance matrix
structure of the update stage of REQUEST. As will be seen later, this
matching is crucial for defining the estimation errors, designing a

cov(vecX ) = E[vecX (vecX )T ] (41)
cost function and, finally, minimizing this cost function with respect
to the gain. The pseudoprocess of Eq. (32), and the process and
Then, the trace of the matrix PX X is identical to the trace of the
measurement equations [Eqs. (26) and (18)], which are
covariance matrix in Eq. (41), that is
K ko + 1/k = k K k/k
o
kT + Wk (33)
tr(PX X ) ≡ tr[cov(vecX )] (42)
δ K k + 1 = δ K ko + 1 + Vk + 1 (34)
We realize that although the matrix PX X is not a covariance matrix,
o
(with an initial condition K 0/0 ), form the model for the K -matrix it has a desired feature in a compact convenient form. Thus, it will
system. be used as a measure of uncertainty for the matrix error processes
that are considered in this paper.
Stochastic Models Using the expressions for Wk and Vk in Eqs. (25) and (17), re-
The purpose of this subsection is to describe the stochastic models spectively, it can be shown that these processes are zero-mean un-
of the system noise matrices Wk and Vk . To derive the stochastic correlated white-noise processes. Thus,
models for Wk and Vk , we need the stochastic models for k and   
δbk . As mentioned earlier, only basic models are considered in this E Wk WkT+ i = O4 , E Vk VkT+ i = O4 , E Wk VkT+ i = O4
work; thus, the vector process k is modeled as a zero-mean white-
noise vector process whose components are identically distributed (43)
with variance ηk , that is,
 for i = 0. Denote by Qk and Rk , respectively, the P matrices of Wk
E[k ] = 0, E k kT + i = ηk I3 δk,k + i (35) and Vk , that is,
 
for k = 1, 2, . . . , and δk,k + i is the Kronecker delta function. As-

Qk = E Wk WkT ,


Rk = E Vk VkT (44)
suming that the unit vector measurements bk are axisymmetrically
distributed about their true value, we employ a unit vector error then, explicit expressions for Qk and Rk can be derived using the
model19 that provides approximate but quite accurate expressions assumptions we made on k and δbk . In the computation of Qk ,
for the mean and the covariance of δbk . The first and second mo- one must address the issue of the dependence of the process noise
ments of this model are matrix Wk on the noise-free matrix B o k/k [Eqs. (24) and (25)], which
 is unknown. To overcome this difficulty, B o k/k is replaced by its best
E[δbk ] = 0, E δbk δbkT + i = µk I3 − bk bkT + i δk,k + i (36)
available estimate Bk/k . The latter is computed from the estimated
for k = 1, 2, . . . , where µk is the variance of the component of bk K matrix K k/k using the definition of the K matrix given in Eqs. (6)
along a direction normal to E[bk ]. Furthermore, it is assumed that and (7). The detailed computation of Qk and Rk is provided in
δbk and k are mutually uncorrelated. The exact expressions for the Ref. 27 (pp. 235–240).
two moments can be found in the appendix of Ref. 11.
Optimal-REQUEST
Measure of Uncertainty The algorithm derivation in this section follows an approach that
The goal of the following analysis is to present a model for the was used in a direct derivation of the Kalman–Bucy filter for the case
uncertainty that Wk and Vk introduce into the K -matrix system of of vector processes.28 A similar derivation of the discrete Kalman
Eqs. (33) and (34), respectively. For scalar and vector processes, filter can be found in the literature (e.g., see Ref. 26, pp. 105–110).
the covariance is a measure of the uncertainty associated with the The approach consists of three steps:
process error. In the case of a matrix process, like the one presented 1) The update of the estimate is formulated as a linear combination
in Eqs. (33) and (34), we introduce a special measure of uncertainty of the predicted estimate and the new observation.
as follows. 2) The a posteriori and a priori estimates are forced to be unbiased.
Definition 1: For the zero-mean general matrix process X , a mea- 3) The optimal filter gain is computed by minimizing the variance
sure of uncertainty PX X is defined as of the a posteriori estimation error.
In the third step, instead of the variance, we will use the proxy


PX X = E[X X T ] (37) measure of uncertainty introduced in the preceding section.
 422 CHOUKROUN, BAR-ITZHACK, AND OSHMAN

Measurement Update Stage Before computing the expectations of the expressions in Eq. (53),
The proposed update stage is a slightly modified version of we consider the following expectation, E[
K k + 1/k VkT+ 1 ]. From
REQUEST, namely, unlike the REQUEST update formula of Eq. (36) δb is a zero-mean white-noise process; therefore [see
Eq. (14), here the updated estimate K k + 1/k + 1 is chosen to be a Eqs. (15–17)], V is a zero-mean white-noise process too. More-
convex combination of the predicted estimate K k + 1/k and the new over, the random variable
K k + 1/k depends on the sequences {Wi }
observation δ K k + 1 , that is, and {Vi }, where i takes the integer values from 1 to k only. Therefore
K k + 1/k + 1 = (1 − ρk + 1 )(m k /m k + 1 )K k + 1/k
K k + 1 /k and Vk + 1 are uncorrelated; thus,

+ ρk + 1 (δm k + 1 /m k + 1 )δ K k + 1 (45) E
K k + 1/k VkT+ 1 = O4 (54)
where δm k + 1 is a positive scalar weight and m k + 1 is computed Taking the transpose of Eq. (54) yields a similar result. Taking the
recursively by expectation of both sides of Eq. (53), and only retaining the nonzero
m k + 1 = (1 − ρk + 1 )m k + ρk + 1 δm k + 1 (46) terms, yields

for k = 0, 1 . . . , and m 0 = δm 0 . The scalar ρk + 1 ∈ [0, 1) has the role Pk + 1/k + 1 = [(1 − ρk + 1 )(m k /m k + 1 )]2 E
K k + 1/k
K kT+ 1/k
of a gain in Eq. (45). (We exclude the case where ρk + 1 = 1 for
practical reasons because, in that case, the filter would be memory-

+ [ρk + 1 (δm k + 1 /m k + 1 )]2 E Vk + 1 VkT+ 1 (55)
less.) Because of the linearity of Eq. (45) and the fact that ρk + 1 is a
scalar, the properties of symmetry and of zero trace of the estimated One identifies the matrices Pk + 1/k and Rk + 1 in the first and second
K matrix are preserved. This is a matter of importance because the terms of the right-hand side of Eq. (55); thus, we can write
Downloaded by UNIVERSITY OF TORONTO on February 1, 2015 | http://arc.aiaa.org | DOI: 10.2514/1.10337

computation of the attitude quaternion, which uses the q method,

Pk + 1/k + 1 = [(1 − ρk + 1 )(m k /m k + 1 )]2 Pk + 1/k
depends on these properties.
The estimation errors of the filter are defined by
+ [ρk + 1 (δm k + 1 /m k + 1 )]2 Rk + 1 (56)



K k + 1/k = K ko + 1/k − K k + 1/k (47a) Equation (56) expresses the uncertainty update in the K -matrix esti-
mation process for any ρk + 1 , when a new measurement is acquired.



K k + 1/k + 1 = K ko + 1/k + 1 − K k + 1/k + 1 (47b)
Optimal Gain
where
K k + 1/k and
K k + 1/k + 1 denote, respectively, the a priori When a new observation is processed, we would like the estima-
and the a posteriori estimation errors. It is assumed that the a priori tion uncertainty to decrease as much as possible. From the earlier
estimate K k + 1/k is unbiased, that is, E[
K k + 1/k ] = 0. This assump- discussion of the properties of the P matrix, we saw that its trace was
tion will be justified when developing the prediction stage. a suitable measure of the uncertainty. Thus, we define the following
The expression for K ko + 1/k + 1 is known from Eq. (32), where cost function:
αk + 1 is arbitrary in [0, 1). For a reason that will become clear in

the ensuing, we choose αk + 1 = ρk + 1 , where ρk + 1 is that which was

Jk + 1 = tr E
K k + 1/k + 1
K kT+ 1/k + 1 = tr(Pk + 1/k + 1 ) (57)
introduced in Eq. (45). Consequently, Eq. (32) becomes
K ko + 1/k + 1 = (1 − ρk + 1 )(m k /m k + 1 )K ko + 1/k Then, the design problem of the filter gain ρk + 1 reduces to solving
the following minimization problem with respect to ρk + 1 :
+ ρk + 1 (δm k + 1 /m k + 1 )δ K ko + 1 (48)
min Jk + 1 (58a)
Subtracting Eq. (45) from Eq. (48) and making use of the definitions ρk + 1 ∈ [0,1)
in Eqs. (47) yields the following relation between the a priori and
the a posteriori errors: where

K k + 1/k + 1 = (1 − ρk + 1 )(m k /m k + 1 )
K k + 1/k

Jk + 1 = tr(Pk + 1/k + 1 ) (58b)
+ ρk + 1 (δm k + 1 /m k + 1 )Vk + 1 (49)
Inserting Eq. (56) into the expression for Jk + 1 in Eq. (57) yields,
where Vk + 1 is the measurement error defined in Eq. (17). Note after some manipulation,
that the expression in Eq. (49) is based on the choice αk + 1 = ρk + 1 .
Taking the expectation of both sides of Eq. (49) yields Jk + 1 (ρk + 1 ) = [(1 − ρk + 1 )(m k /m k + 1 )]2 tr(Pk + 1/k )
E[
K k + 1/k + 1 ] = (1 − ρk + 1 )(m k /m k + 1 )E[
K k + 1/k ]
+ [ρk + 1 (δm k + 1 /m k + 1 )]2 tr(Rk + 1 ) (59)
+ ρk + 1 (δm k + 1 /m k + 1 )E[Vk + 1 ] (50)
The first-order necessary condition for an extremum of Jk + 1 is
Using the assumptions that the measurement error Vk + 1 and the  2  2
a priori estimation error
K k + 1/k are zero mean, one finds that dJk + 1 mk δm k + 1
the a posteriori estimation error
K k + 1/k + 1 is zero mean too, as =2 tr(Pk + 1/k ) + tr(Rk + 1 ) ρk + 1
dρk + 1 mk + 1 mk + 1
required.
The P matrices corresponding to both estimation errors are de-  2
fined as follows: mk
 −2 tr(Pk + 1/k ) = 0 (60)

mk + 1
Pk + 1/k = E
K k + 1/k
K kT+ 1/k (51)
 resulting in the following condition for ρk∗ + 1 to yield a stationary


Pk + 1/k + 1 = E
K k + 1/k + 1
K kT+ 1/k + 1 (52) point for Jk + 1

Using Eq. (49), we compute the following product: m 2k tr(Pk + 1/k )

ρk∗ + 1 = (61)

K k + 1/k + 1
K kT+ 1/k + 1 = [(1 − ρk + 1 )(m k /m k + 1 )]2 m 2k tr(Pk + 1/k ) + δm 2k + 1 tr(Rk + 1 )
 Using the sufficiency condition for this point to be a minimum, it can
×
K k + 1/k
K kT+ 1/k − (1 − ρk + 1 )ρk + 1 m k δm k + 1 m 2k + 1 be verified that the cost function Jk + 1 indeed reaches a minimum
at ρk∗ + 1 . Note that, in the case of a scalar process, Eq. (61) yields
×
K k + 1/k VkT+ 1 + Vk + 1
K kT+ 1/k the exact expression for the Kalman-filter gain. Even in the gen-
eral case the filter gain ρk∗ + 1 still has the feature of a Kalman-filter
+ [ρk + 1 (δm k + 1 /m k + 1 )]2 Vk + 1 VkT+ 1 (53) gain; namely, for a high uncertainty in the a priori estimate, relative
 CHOUKROUN, BAR-ITZHACK, AND OSHMAN 423

to the uncertainty in the measurement, the gain is close to 1 and where the matrix Rk + 1 is computed according to Eqs. (16), (17),
weighs more favorably the new measurement in the update stage and (44). The optimal quaternion q̂k + 1/k + 1 is the eigenvector of
described in Eq. (45). On the other hand, for a high uncertainty in K k + 1/k + 1 , which belongs to its maximal eigenvalue.
the measurement, relative to the uncertainty in the a priori estimate,
the gain is close to 0, and the filter assigns a low weight to the new Simulation Study
measurement.
An extensive Monte Carlo (MC) simulation study was performed
Prediction Stage in order to test the attitude estimator in the presence of process and
measurement noises. Different single vector observations were ac-
We require that the predicted K k + 1/k be linear in K k/k and that it
quired at each sampling time. The body coordinate system B was
produces an unbiased predicted estimate. These requirements yield
assumed to be fixed with respect to an inertial coordinate system R.
the prediction formula of the REQUEST algorithm [Eq. (11)]
The vector observation noise was a zero-mean Gaussian white noise
K k + 1/k = k K k/k kT (62) with an angular standard deviation of one degree, which is a typi-
cal accuracy obtained using magnetometers. Three body-mounted
Using the K -matrix process of Eq. (26), the prediction in Eq. (62), gyroscopes measured the angular velocity of B with respect to R.
and the definitions of the estimation errors given in Eqs. (47) yields Because the system B did not rotate with respect to system R, the
the error propagation equation nominal body rates were zero; hence, the gyro measurements con-

K k + 1/k = k
K k/k kT + Wk (63) tained only gyro noises. The gyro noises were Gaussian zero-mean
white noises with a standard deviation of 0.2 deg/h in each axis.
It is easy to see that E[
K k + 1/k ] = 0, which justifies our initial
Downloaded by UNIVERSITY OF TORONTO on February 1, 2015 | http://arc.aiaa.org | DOI: 10.2514/1.10337

The noise models in the system and in the filter were identical. The
assumption that the a priori estimate of K k + 1/k is unbiased. Using sample rate was 10 Hz, both in the measured directions and in the
the latter expression for
K k + 1/k , the stochastic models of the noise, gyro measurements. Each simulation lasted 6000 s. We ran 100 runs
and the orthogonality property of the k matrix, the propagation with different seeds and averaged the results at each time point to
equation for the P matrix is obtained as follows: obtain the ensemble averages.
The results are summarized in Figs. 1 and 2 and Table 1. Figure 1a
Pk + 1/k = k Pk/k kT + Qk (64) presents the time history of δφ, the MC mean of δφ, the angular es-
timation error. The angle δφ is defined as the angle of the small rota-
Equation (64) is similar to the covariance propagation equation in
tion that brings the estimated body frame B̂ onto the true body frame
the Kalman-filter algorithm.
B. This angle is obtained as follows: First, the quaternion of the ro-
To conclude, similarly to the Kalman filter, the algorithm com-
tation from B̂ to B, denoted by δq, is evaluated, then the rotation
prises two parallel channels. One channel is for the computation
angle δφ is computed from δq, the scalar component of δq, using the
of the signal estimate, which here is the K -matrix estimate, and
known relation (Ref. 1, p. 414) δφ = 2 arccos(δq). The error reaches
the other one is for the uncertainty propagation of the estimation
a steady state of approximately 0.06 deg. Figure 1b shows the time
process, which in the Kalman filter is expressed by the covariance
history of the MC mean of ρ ∗ , the optimal filter gain. The average
matrix, and is needed for the computation of the optimal filter gain.
gain decreases exponentially from 0.5 down to 0.001. As expected,
the Optimal-REQUEST algorithm behaves like a Kalman filter; it
Algorithm Summary
initially weighs more the new observations in the estimate, and,
The Optimal-REQUEST algorithm presented in this section can as the number of processed measurements grows, it progressively
be summarized as follows: turns to be a pure predictor of the estimate, that is, it weighs less
Initialization: the incoming measurements. The attitude estimated by the Optimal-
K 0/0 = δ K 0 (65) REQUEST filter converges successfully to the true attitude.
Figure 2 shows the performance of Optimal-REQUEST com-
where δ K 0 is computed using the first vector measurement according pared to that of various cases of REQUEST where the gain ρ is
to Eqs. (12) and (13). chosen as a constant. We chose three different values for ρ, namely,
P0/0 = R0 (66) 0.1, 0.01, and 0.001, which, as seen in Fig. 1b are typical values
in the span of the optimal gain ρ ∗ . In this simulation the sampling
m 0 = δm 0 (67) frequency Fs was chosen equal to 0.5 Hz. Figure 2 depicts the vari-
ations of the MC mean of δφ for each case. For ρ = 0.1, the filter
where δm 0 is a positive weighting factor. weighs relatively much the measurements, so that after a quick tran-
Time update: sient the error remains on a relatively high steady state (0.45 deg)
and shows random variations. On the other hand, a very low gain
K k + 1/k = k K k/k kT (68) (ρ = 0.001) yields a smooth but very slow convergence of the error,
which reaches the value of 0.05 deg in steady state. As seen in Fig. 2,
Pk + 1/k = k Pk/k kT + Qk (69) the optimal gain ρ ∗ of Optimal-REQUEST yields a lower bound for
where the matrix Qk is computed according to Eqs. (24), (25), and the various MC means of δφ. This is true during the transient as well
(44). as in the steady state where Optimal-REQUEST yields an error of
Measurement update: 0.03 deg.
Optimal-REQUEST was also tested using various √ sampling rates
m 2k tr(Pk + 1/k ) Fs , standard deviations of the √vector observations µ, and standard
ρk∗ + 1 = (70) deviations of the gyro output η. Table 1 presents the Monte Carlo
m 2k tr(Pk + 1/k ) + δm 2k + 1 tr(Rk + 1 )
means of δφ at the final time δφ(t f ) for the various cases. For the sake
of comparison, the same number of observations—here, 2000—are
m k + 1 = 1 − ρk∗ + 1 m k + ρk∗ + 1 δm k + 1 (71)
mk δm k + 1 Table 1 Monte Carlo means of δφ at final time
K k + 1/k + 1 = 1 − ρk∗ + 1 K k + 1/k + ρk∗ + 1 δ Kk + 1
mk + 1 mk + 1
√ √ δφ at t f √ δφ at t f √ δφ at t f
(72) Fs , Hz µ, deg η = 0.01, deg/h η = 360, deg/h η = 3600, deg/h
 2  2 10 1 0.03 0.15 0.78
mk δm k + 1
Pk + 1/k + 1 = 1 − ρk∗ + 1 Pk + 1/k + ρk∗ + 1 Rk + 1 10 5 0.15 0.55 1.99
mk + 1 mk + 1 0.5 1 0.04 0.39 3.25
0.5 5 0.24 1.18 7.79
(73)
 424 CHOUKROUN, BAR-ITZHACK, AND OSHMAN

a) Time history of the MC-mean of δφ

Downloaded by UNIVERSITY OF TORONTO on February 1, 2015 | http://arc.aiaa.org | DOI: 10.2514/1.10337

b) Time history of the MC-mean of ρ∗

Fig. 1 Performance of Optimal-REQUEST averaged over 100-runs Monte Carlo simulation.

Fig. 2 Comparison of Optimal-REQUEST and REQUEST.

 CHOUKROUN, BAR-ITZHACK, AND OSHMAN 425

processed in each case. As seen in Table 1, the values of δφ(t f ) 5 Farrel, J. L., and Stuelpnagel, J. C., “A Least Squares Estimate of Space-

√ filter parameters, that is, δφ(t f )

are consistent with those craft Attitude,” SIAM Review, Vol. 8, No. 3, 1966, pp. 384–386.
√ of the 6 Wessner, R. H., “A Least Squares Estimate of Spacecraft Attitude,” SIAM
increases with 1/Fs , µ, and η. Notice that δφ(t f ) converges to
some final value even in the worst simulated case. Review, Vol. 8, No. 3, 1966, pp. 384–386.
7 Velman, J. R., “A Least Squares Estimate of Spacecraft Attitude,” SIAM
Review, Vol. 8, No. 3, 1966, pp. 384–386.
Conclusions 8 Brock, J. E., “A Least Squares Estimate of Spacecraft Attitude,” SIAM

In this paper a new recursive optimal estimator for estimating Review, Vol. 8, No. 3, 1966, pp. 384–386.
9 Brock, J. E., “Optimal Matrices Describing Linear Systems,” AIAA Jour-
the quaternion of rotation from vector measurements is presented.
Named Optimal-REQUEST, this algorithm is an extension of the nal, Vol. 6, No. 7, 1968, pp. 1292–1296.
10 Markley, F. L., “Attitude Determination Using Vector Observations:
REQUEST algorithm, which is based on the q method.
a Fast Optimal Matrix Algorithm,” Journal of the Astronautical Sciences,
The proposed filter is derived using a unique variance-like Vol. 41, No. 2, 1993, pp. 261–280.
performance criterion, which gives a stochastic basis to the esti- 11 Markley, F. L., “Attitude Determination Using Vector Observations and
mation process. Like a Kalman filter, the proposed algorithm opti- the Singular Value Decomposition,” Journal of the Astronautical Sciences,
mally filters both measurement and process noises; thus, it covers Vol. 36, No. 3, 1988, pp. 245–258.
a deficiency of REQUEST, where the process noise is filtered in 12 Bar-Itzhack, I. Y., “Polar Decomposition for Attitude Determination

an empirical manner. The cost to achieve that performance is that from Vector Observations,” Flight Mechanics/Estimation Theory Sympo-
the computation of the optimal gain involves 4 × 4 matrix equa- sium, NASA Conf. Pub. 3186, NASA, 1992, pp. 243–257.
13 Bar-Itzhack, I. Y., and Markley, F. L., “Unconstrained Optimal Transfor-
tions, which increases the computational burden of the filter. The
mation Matrix,” IEEE Transactions on Aerospace and Electronic Systems,
special case of a zero-mean white-noise process is considered here.
Downloaded by UNIVERSITY OF TORONTO on February 1, 2015 | http://arc.aiaa.org | DOI: 10.2514/1.10337

Vol. 34, No. 1, 1998, pp. 338–340.

When severe modeling errors, like unknown constant gyro biases, 14 Shuster, M. D., “A Survey of Attitude Representations,” Journal of the
are present, adaptive filtering theory can be used to adapt online the Astronautical Sciences, Vol. 41, No. 4, 1993, pp. 439–517.
covariance of the filter process noise to the bias level, rendering the 15 Markley, F. L., “Spacecraft Attitude Determination Methods,” 40th Is-
overall estimation scheme robust to the modeling uncertainty. rael Annual Conference on Aerospace Sciences, edited by Y. Oshman, Vol.
Optimal-REQUEST also retains all of the features of REQUEST; 40, Technion—Israel Inst. of Technology, Haifa, Israel, 2000.
16 Keat, J., “Analysis of Least Squares Attitude Determination Routine
that is, it is a time-varying recursive attitude quaternion estimator,
the quaternion unit-norm property is preserved, and the attitude DOAOP,” Computer Sciences Corp., CSC/TM-77/6034, Silver Spring, MD,
is updated even when a single vector observation is processed at Feb. 1977.
17 Bellman, R., Introduction to Matrix Analysis, McGraw–Hill, New-York,
each sampling time. The efficiency of the new filter is demonstrated
1960, pp. 5, 6.
through Monte Carlo simulations. A comparison test was performed 18 Golub, G. H., and Van Loan, C. F., Matrix Computations, Johns Hopkins
on Optimal-REQUEST and several versions of REQUEST where, as University Press, Baltimore, MD, 1983, p. 405.
the algorithm is set, for each version, the fading memory factor was 19 Shuster, M. D., and Oh, S. D., “Three-Axis Attitude Determination from
held constant during the whole simulation period. The simulation Vector Observations,” Journal of Guidance and Control, Vol. 4, No. 1, 1981,
results show that Optimal-REQUEST yields the lowest Monte Carlo pp. 70–77.
20 Mortari, D., “A Closed-Form Solution to the Wahba Problem,” Journal
mean of the angular estimation error during the transient period as
well as at steady state, clearly demonstrating the superiority of the of the Astronautical Sciences, Vol. 45, No. 2, 1997, pp. 195–204.
21 Mortari, D., “Second Estimator of the Optimal Quaternion,” Journal of
new algorithm over the old one.
Guidance, Control, and Dynamics, Vol. 23, No. 5, 2000, pp. 885–888.
22 Markley, F. L., “Attitude Determination Using Vector Observations:
Acknowledgments Theory,” Journal of the Astronautical Sciences, Vol. 37, No. 1, 1989,
This work was partially supported by NASA Grant NAG5-8770. pp. 41–58.
We wish to thank T. Stengle and R. Harman of NASA Goddard 23 Shuster, M. D., “A Simple Kalman Filter and Smoother for Space-

Space Flight Center for this support. A part of this work was per- craft Attitude,” Journal of the Astronautical Sciences, Vol. 37, No. 1, 1989,
formed while the second author was holding a National Research pp. 89–106.
24 Bar-Itzhack, I. Y., “REQUEST: A Recursive QUEST Algorithm for
Council Research Associateship Award at NASA Goddard Space
Sequential Attitude Determination,” Journal of Guidance, Control, and Dy-
Flight Center.
namics, Vol. 19, No. 5, 1996, pp. 1034–1038.
25 Psiaki, M. L., “Attitude Determination Filtering via Extended Quater-
References nion Estimation,” Journal of Guidance, Control, and Dynamics, Vol. 23,
1 Wertz, J. R. (ed.), Spacecraft Attitude Determination and Control, No. 2, 2000, pp. 206–214.
D. Reidel, Dordrecht, The Netherlands, 1984. 26 Gelb, A. (ed.), Applied Optimal Estimation, MIT Press, Cambridge,
2 Black, H. D., “A Passive System for Determining the Attitude of a Satel- MA, 1988.
lite,” AIAA Journal, Vol. 2, No. 7, 1964, pp. 1350, 1351. 27 Choukroun, D., “Novel Methods for Attitude Estimation from Vec-
3 Davenport, P., “Attitude Determination and Sensor Alignment via tor Observations,” Ph.D. Dissertation, Faculty of Aerospace Engineering,
Weighted Least Squares Affine Transformations,” NASA, X-514-71-312, Technion—Israel Inst. of Technology, Haifa, Israel, May 2003.
Aug. 1971. 28 Athans, M., and Tse, E., “A Direct Derivation of the Optimal Linear
4 Wahba, G., “A Least Squares Estimate of Spacecraft Attitude,” SIAM Filter Using the Maximum Principle,” IEEE Transactions on Automatic Con-
Review, Vol. 7, No. 3, 1965, p. 409. trol, Vol. AC-12, No. 6, 1967, pp. 690–698.