partial attitude and rate gyro bias estimation
TRANSCRIPT
This article was downloaded by: [Institutional Subscription Access]On: 18 July 2011, At: 14:28Publisher: Taylor & FrancisInforma Ltd Registered in England and Wales Registered Number: 1072954 Registered office: Mortimer House,37-41 Mortimer Street, London W1T 3JH, UK
International Journal of ControlPublication details, including instructions for authors and subscription information:http://www.tandfonline.com/loi/tcon20
Partial attitude and rate gyro bias estimation:observability analysis, filter design, and performanceevaluationPedro Batista a , Carlos Silvestre a & Paulo Oliveira aa Instituto Superior Técnico/Institute for Systems and Robotics, Av. Rovisco Pais, 1, Lisboa,Portugal
Available online: 11 Jul 2011
To cite this article: Pedro Batista, Carlos Silvestre & Paulo Oliveira (2011): Partial attitude and rate gyro bias estimation:observability analysis, filter design, and performance evaluation, International Journal of Control, 84:5, 895-903
To link to this article: http://dx.doi.org/10.1080/00207179.2011.582652
PLEASE SCROLL DOWN FOR ARTICLE
Full terms and conditions of use: http://www.tandfonline.com/page/terms-and-conditions
This article may be used for research, teaching and private study purposes. Any substantial or systematicreproduction, re-distribution, re-selling, loan, sub-licensing, systematic supply or distribution in any form toanyone is expressly forbidden.
The publisher does not give any warranty express or implied or make any representation that the contentswill be complete or accurate or up to date. The accuracy of any instructions, formulae and drug doses shouldbe independently verified with primary sources. The publisher shall not be liable for any loss, actions, claims,proceedings, demand or costs or damages whatsoever or howsoever caused arising directly or indirectly inconnection with or arising out of the use of this material.
International Journal of ControlVol. 84, No. 5, May 2011, 895–903
Partial attitude and rate gyro bias estimation: observability analysis, filter design, and
performance evaluation
Pedro Batista*, Carlos Silvestre and Paulo Oliveira
Instituto Superior Tecnico/Institute for Systems and Robotics, Av. Rovisco Pais, 1, Lisboa, Portugal
(Received 29 March 2011; final version received 17 April 2011)
This article presents the analysis, design, and performance evaluation of a novel filter for partial attitude and rategyro bias estimation. In addition to a single body-fixed vector observation of a constant reference vector ininertial coordinates, biased angular velocity measurements are available for filtering purposes, which occursdirectly in the space of the vector observations. Appropriate observability conditions are derived and a filter withglobally asymptotically stable (GAS) error dynamics is proposed. Simulation and experimental results aredetailed, with ground truth data for comparison purposes, that illustrate the achievable performance of theenvisioned solutions.
Keywords: attitude estimation; observability analysis; filter design
1. Introduction
Attitude estimation has been a hot topic of research inthe past decades, playing a key role in the developmentof navigation systems for autonomous vehicles andmobile platforms. In traditional solutions there exist,at least, two vector observations, in body-fixed coor-dinates, of known constant reference vectors in inertialcoordinates, where at least two of them are non-parallel. Extended Kalman Filters (EKFs) and variantshave been widely used, see Farrell (1970) andBar-Itzhack and Oshman (1985), for instance. Inspite of the good performance achieved by EKF andEKF-like solutions, divergence due to the linearisationof the system dynamics, see Crassidis, Markley, andCheng (2007), has led the scientific community topursue different solutions, in particular nonlinearobservers such as those presented in Rehbinder andGhosh (2003), Thienel and Sanner (2003), Metni,Pflimlin, Hamel, and Soueres (2006), Tayebi,McGilvray, Roberts, and Moallem (2007), Mahony,Hamel, and Pflimlin (2008), Sanyal, Lee, Leok, andMcClamroch (2008), Batista, Silvestre, and Oliveira(2009), Vasconcelos, Cunha, Silvestre, and Oliveira(2010), and references therein. The reader is referred toCrassidis et al. (2007) for a survey on this topic.
It is well known that, when there is a single vectorobservation, in body-fixed coordinates, of a singleconstant reference vector, in inertial coordinates, it isimpossible to recover the whole attitude of theplatform. However, it is still possible to partiallyrecover some of the attitude information and, in
addition, estimate the rate gyro bias. This last pointis particularly important as, without bias compensa-tion, the error of open-loop integration of inertialsensors diverges, see Savage (1998a,b). By partiallyestimating the attitude and fully estimating the rategyro bias, it is possible to aid inertial navigationsystems, reducing the divergence of attitude estimation,in open-loop, to the effect of the integration of sensor-noise, but eliminating the overwhelming negative effectof rate gyro bias. In addition, for some applications,the entire attitude is not required. As an example, forstabilisation of quadrotors, the roll and pitch anglesare quite important, but the yaw angle, with respect toan inertial frame, is not always required, particularly invision-based approaches.
This article presents the analysis, design, andperformance evaluation of a novel filter for partialattitude and rate gyro bias estimation. In addition to asingle body-fixed vector observation of a constantreference vector in inertial coordinates, biased angularvelocity measurements are available for filtering pur-poses, which occurs directly in the space of the vectorobservations. An exact transformation of the systemdynamics is at the core of the proposed design thatallows to regard the system as linear time-varying forobserver design purposes, even though it still is, in fact,nonlinear. Powerful results for linear systems areemployed and appropriate observability conditionsare derived, ending with the design of a Kalman filterwith globally asymptotically stable (GAS) errordynamics. Preliminary work by the authors can be
*Corresponding author. Email: [email protected]
ISSN 0020–7179 print/ISSN 1366–5820 online
� 2011 Taylor & Francis
DOI: 10.1080/00207179.2011.582652
http://www.informaworld.com
Dow
nloa
ded
by [
Inst
itutio
nal S
ubsc
ript
ion
Acc
ess]
at 1
4:28
18
July
201
1
found in Batista et al. (2009), where the theoreticalframework was first presented. This article elaboratesin greater detail on the observability of the system andpresents defining simulation and experimental resultsto evaluate the performance of the proposed solution.
This article is organised as follows. Section 2introduces the problem at hand, while the observabilityanalysis is detailed in Section 3. The filter design isbriefly addressed in Section 4 and simulation resultsare presented in Section 5. Experimental results arediscussed in Section 6 and, finally, Section 7 presentsthe main conclusions and results of this article.
1.1 Notation
Throughout this article the symbol 0 denotes a matrix(or vector) of zeros and I an identity matrix, both ofappropriate dimensions. A block diagonal matrix isrepresented as diag(A1, . . . ,An). For x2R
3 and y2R3,
x� y represents the cross product. Finally, the Diracdelta function is denoted by �(t).
2. Problem statement
Let {I } denote a local inertial frame, {B} the body-fixed frame and R(t)2SO(3) the rotation matrix from{B} to {I }. The attitude kinematics are given by
_RðtÞ ¼ RðtÞS xðtÞ½ �,
where x(t)2R3 is the angular velocity of {B},
expressed in {B}, and S(x) is the skew-symmetricmatrix such that S(x)y¼ x� y. It is assumed that theangular velocity is a bounded continuous differentiablesignal, with bounded derivative.
Suppose that measurements y(t)2R3 are available,
in body-fixed coordinates, of a known constant vectorin inertial coordinates, i.e.
Iy ¼ RðtÞyðtÞ:
Further consider rate gyro measurements xm(t)2R3
corrupted with bias bx2R3, i.e.
xmðtÞ ¼ xðtÞ þ bxðtÞ:
The problem considered here is that of obtainingfiltered estimates of y(t) and estimating the rate gyrobias bx. In addition, partial attitude reconstruction iscarried out using Iy and the filtered estimates of y(t).
3. Observability analysis
It is well known that, with a single vector observationof a constant vector in inertial coordinates, it isimpossible to recover the attitude. Indeed, one degree
of freedom is unobservable, which is readily verified as
yðtÞ ¼ RTðtÞIy ¼ RTðtÞRToIy for all rotation matrices Ro
about the inertial constant vector Iy, while RoR(t)
satisfies the corresponding differential equation
d
dtRoRðtÞ½ � ¼ RoRðtÞ½ �S xðtÞ½ �:
Nevertheless, partial attitude reconstruction can still be
performed and, in some cases, it is also possible to
estimate the rate gyro bias. This is discussed in this
section.Considering a sensor-based approach, with the
incorporation of the derivative of the vector observa-
tions in the system dynamics, gives
_x1ðtÞ ¼ �S xmðtÞ½ �x1ðtÞ þ S x2ðtÞ½ �x1ðtÞ_x2ðtÞ ¼ 0
yðtÞ ¼ x1ðtÞ
8<: ð1Þ
where x1(t)¼ y(t) and x2(t)¼ bx(t) are the system states
and y(t) is the system output. Using the cross product
property x� y¼�y� x and the relation y(t)¼x1(t)
allows to rewrite (1) as
_x1ðtÞ ¼ �S xmðtÞ½ �x1ðtÞ � S yðtÞ½ �x2ðtÞ
_x2ðtÞ ¼ 0
yðtÞ ¼ x1ðtÞ
8>><>>:
or, in compact form,
_xðtÞ ¼ A yðtÞ,xmðtÞð ÞxðtÞyðtÞ ¼ CxðtÞ
�ð2Þ
where xðtÞ ¼ xT1 ðtÞ xT2 ðtÞ
� �T2 R
6 is the system state,
A yðtÞ,xmðtÞð Þ ¼�S xmðtÞ½ � �S yðtÞ½ �
0 0
� �2 R
6�6,
and C ¼ I 0� �
2 R3�6. For simplicity of notation, the
dependence of A(y(t),xm(t)) on y(t) and xm(t) is
omitted in the sequel and the system matrix is simply
denoted by A(t).Although (2) is a nonlinear system, it can be
regarded as linear time-varying because the output
y(t) is available for observer design purposes. Before
presenting the main results of this section, the follow-
ing lemma is introduced.
Lemma 3.1: Consider the nonlinear system
_xðtÞ ¼ A t, uðtÞ, yðtÞð ÞxðtÞ þ BðtÞuðtÞyðtÞ ¼ CðtÞxðtÞ:
�ð3Þ
If the observability Gramian W(t0, tf) associated with
the pair (A(t, u(t), y(t)),C(t)) on I :¼ [t0, tf] is invertible
then the nonlinear system (3) is observable in the sense
that, given the system input {u(t), t2I} and the system
896 P. Batista et al.
Dow
nloa
ded
by [
Inst
itutio
nal S
ubsc
ript
ion
Acc
ess]
at 1
4:28
18
July
201
1
output {y(t), t2I}, the initial condition x(t0) is uniquely
defined.
Proof: Given the system input {u(t), t2I} and the
system output { y(t), t2I}, it is possible to compute the
transition matrix associated with the system matrix
A(t, u(t), y(t))
/ t, t0ð Þ ¼ Iþ
Z t
t0
A �1, u �1ð Þ, y �1ð Þð Þd�1
þ
Z t
t0
A �1, u �1ð Þ, y �1ð Þð ÞZ �1
t0
A �2, u �2ð Þ, y �2ð Þð Þd�2d�1 þ � � �
on I , which clearly satisfies / t0, t0ð Þ ¼ I and
@/ t, t0ð Þ
@t¼ A t, uðtÞ, yðtÞð Þ/ t, t0ð Þ:
Therefore, it is also possible to compute the observ-
ability Gramian
W t0, tf� �
¼
Z tf
t0
/T t, t0ð ÞCTðtÞCðtÞ/ t, t0ð Þdt:
Now, note that it is possible to write the evolution of
the state, given the system input and output (which
allow to compute the transition matrix), as
xðtÞ ¼ / t, t0ð Þx0 þ
Z t
t0
/ t, �ð ÞB �ð Þu �ð Þd�, ð4Þ
where x0 is the initial condition. This is easily verified
as with t¼ t0 in (4) gives x(t0)¼ x0 and taking the time
derivative of (4) gives _x tð Þ ¼ Aðt, uðtÞ, yðtÞÞxðtÞ þBðtÞuðtÞ. The remainder of the proof follows as in
classic theory. The output of the system can be written,
from (4), as
yðtÞ ¼ CðtÞ/ t, t0ð Þx0 þ CðtÞZ t
t0
/ t, �ð ÞB �ð Þu �ð Þd�: ð5Þ
Multiplying (5) on both sides by /T t, t0ð ÞCTðtÞ andintegrating on I yields
W t0, tf� �
x0 ¼
Z tf
t0
/T t, t0ð ÞCTðtÞyðtÞdt
�
Z tf
t0
/T t, t0ð ÞCTðtÞCðtÞZ t
t0
/ t, �ð ÞB �ð Þu �ð Þd�dt: ð6Þ
All quantities in (6) but the initial condition are known
given the system input and output and therefore it
corresponds to a linear algebraic equation on x0. If the
observability GramianW(t0, tf) is invertible, then x0 is
uniquely defined, which concludes the proof. œ
Remark 1: Note that, even though the evolution
of the state given in (4) seems to resemble the
response of a linear system, the response of the
system does not correspond to the superposition of
the free response (due to the initial conditions) and the
forced response (due to system input). This is so
because the transition matrix in (4) depends explicitly
on the system input. For observability purposes this
is not a problem because both the input and output
are available.
Remark 2: Note that Lemma 3.1 presents only a
sufficient condition for the observability of the non-
linear system (3). For linear systems the present
condition is also necessary. However, the fact that
the underlying system is inherently nonlinear precludes
that conclusion in this case.
The following result [Proposition 4.2, Batista,
Silvestre, and Oliveira (2011)] is useful in the sequel.
Proposition 3.2: Let f(t) : [t0, tf ]�R!Rn be a con-
tinuous and i-times continuously differentiable function
on I :¼ [t0, tf ], T :¼ tf� t04 0, and such that f t0ð Þ ¼_f t0ð Þ ¼ � � � ¼ f ði�1Þ t0ð Þ ¼ 0: Further assume that
kf(iþ1)(t)k�C for all t2I . If there exist �40 and
t12I such that kf (i)(t1)k��, then there exist 05��T
and �4 0 such that kf(t0þ �)k��.
The following theorem addresses the observability
of the nonlinear system (2).
Theorem 3.3: The nonlinear system (2) is observ-
able on I :¼ [t0, tf ], in the sense that given
{xm(t), t2I} and {y(t), t2I} the initial condition x(t0)
is uniquely defined, if and only if for every unit vector
d2R3, it is possible to choose t*2I such that
ky(t*)� dk4 0.
Proof: The proof of sufficiency follows resorting to
Lemma 3.1. The transition matrix associated with A(t)
is given by
/ t, t0ð Þ ¼RT
mðtÞ �RTmðtÞ
R tt0Rmð�ÞS yð�Þ½ �d�
0 I
" #,
where Rm(t)2SO(3) is such that _RmðtÞ ¼ RmðtÞS xmðtÞ½ �,
with Rm(t0)¼ I. This can be easily verified as
r(t0, t0)¼ I and
@/ t, t0ð Þ
@t¼ AðtÞ/ t, t0ð Þ:
Let d ¼ dT1 dT2
� �T2 R
6 be a unit vector, with d1, d22R3.
Then, it follows that
dTW t0, tf� �
d ¼
Z tf
t0
f �, t0ð Þ 2d�,
International Journal of Control 897
Dow
nloa
ded
by [
Inst
itutio
nal S
ubsc
ript
ion
Acc
ess]
at 1
4:28
18
July
201
1
where W(t0, tf) denotes the observability Gramian
associated with the pair (A(t),C) on [t0, tf ] and
f �, t0ð Þ ¼ d1 �
Z �
t0
Rmð�ÞS yð�Þ½ �d2d� 2 R3:
The first derivative of f(�, t0) with respect to � is
given by
d
d�f �, t0ð Þ ¼ �Rmð�ÞS yð�Þ½ �d2
and it is straightforward to show that the second
derivative with respect to � is norm-bounded under the
assumption of bounded angular velocities x(t). Now,
note that if d1 6¼ 0 then kf(t0, t0)k¼kd1k¼ �14 0.
On the other hand, if d1¼ 0, it must be kd2k¼ 1,
f (t0, t0)¼ 0, and dd� f �, t0ð Þ ¼ yð�Þ � d2
: Now, under
the hypothesis of the theorem, for all kd2k¼ 1 it is
possible to choose t*2I such that k dd� fð�, t0Þj�¼tk
� �2 4 0: But that means, using Proposition 3.2, that
if kd2k¼ 1, there exist t22I such that kf (t2, t0) k¼
�34 0. Thus, so far it is possible to conclude that, for
all kdk¼ 1, if is possible to choose t32I such that
kf(t3, t0)k. This, in turn, allows to conclude, using
Proposition 3.2 again, that dTW(t0, tf )d4 0 for all
kdk¼ 1, which means that the observability Gramian
W(t0, tf ) is positive definite. As such, it follows, from
Lemma 3.1, that under the hypothesis of the theorem
the nonlinear system (2) is observable on I .The proof of necessity follows by simply showing
that, if the hypothesis of the theorem is not verified, then
there exist at least two initial conditions that explain the
system output. Suppose that there exists a unit vector
d2R3 such that y(t)� d¼ 0 for all t2I or, equivalently,
y(t)¼ y0 is a constant vector. Let x1(t0)¼ y0 and
x2(t0)¼ x20 be the initial state of the nonlinear system
(2). As the output is constant, it follows that
�S xmðtÞ½ �yðtÞ � S yðtÞ½ �x20 ¼ 0: ð7Þ
Now, note that, considering different initial conditions
x1 t0ð Þ ¼ y0x2 t0ð Þ ¼ x20 þ �y0
,
�ð8Þ
where � 2R\{0}, the same output is verified for all
t2I , i.e. y(t)¼ y0 is the output of the nonlinear system
(2) with initial conditions (8). This can be readily
verified: using (7) and y(t)� y0¼ y0� y0¼ 0 gives
_x1ðtÞ ¼ �S xmðtÞ½ �yðtÞ � S yðtÞ½ �x20 � �S yðtÞ½ �y0 ¼ 0
for all t2I , which means that x1(t)¼ x1(t0)¼ y0 and as
such y(t)¼x1(t)¼ y0. But this concludes the proof as it
was shown that, if the hypothesis of the theorem does
not hold, then the initial state is not uniquely
defined. œ
In general, stronger forms of observability arerequired in order to design observers (or filters) withglobally asymptotically stable error dynamics.The following theorem provides a sufficient conditionon uniform complete observability of the pair (A(t),C),a result that will be useful for the design of observers(filters) with globally asymptotically stable errordynamics.
Theorem 3.4: The pair (A(t),C) is uniformly comple-tely observable if and only if there exist scalars �4 0and �4 0 such that, for all t� t0 and all unit vectorsd2R
3, it is possible to choose t*2 [t, tþ �] such thatky(t*)� dk� �.
Proof: The pair (A(t),C) is uniformly completelyobservable if and only if there exist scalars �14 0,�24 0, and �*4 0 such that
�1 5 dTW t, tþ �ð Þd5�2 ð9Þ
for all t� t0 and all unit vectors d2R6, where
W(t, tþ �*) denotes the observability Gramian associ-ated with the pair (A(t),C) on [t, tþ �*]. As all entriesof the system matrices A(t) and C are continuous andbounded, it follows that, given �*4 0, it is alwayspossible to choose �24 0 such that the right-hand sideof (9) is verified for all t� t0 and all unit vectors d2R
6.Therefore, in the remainder of the proof, only theleft-hand side of (9) is considered.
In order to prove sufficiency, suppose that thereexist scalars �4 0 and �4 0 such that, for all t� t0 andall unit vectors d02 2 R
3, it is possible to chooset*2 [t, tþ �] such that y tð Þ � d02
� �: Let �*¼ �.As in Theorem 3.3, it is possible to write
dTW t, tþ �ð Þd ¼
Z tþ�
t
f �, tð Þ 2d�,
where d ¼ dT1 dT2� �T
, d12R3, d22R
3 and
f �, tð Þ ¼ d1 �
Z �
t
Rmð�ÞS yð�Þ½ �d2d� 2 R3:
Suppose that d1 6¼ 0. Then, it is clear that there existsc14 0 such that kf(t, t)k¼kd1k¼ c1 for all t� t0.On the other hand, if d1¼ 0, then it must be thatkd2k¼ 1, f(t, t)¼ 0 and
d
d�f �, tð Þ
¼ yð�Þ � d2
:But under the hypothesis of the theorem, it is possibleto choose t*2 [t, tþ �] such that
d
d�f �, tð Þ
�¼t
� �
for all t� t0 and kd2k¼ 1. Hence, resorting toProposition 3.2, it is possible to conclude that there
898 P. Batista et al.
Dow
nloa
ded
by [
Inst
itutio
nal S
ubsc
ript
ion
Acc
ess]
at 1
4:28
18
July
201
1
exists a scalar c240 such that, for all t� t0 and kd2k¼ 1,it is possible to choose t0 2 [t, tþ �] such thatkf (t0, t)k� c2. Thus, it has been established that thereexists a scalar c4 0 such that, for all t� t0 and kdk¼ 1,it is possible to choose ti2 [t, tþ �] such thatkf (ti, t)k� c. But this, in turn, allows to conclude,using Proposition 3.2 again, that there exists a sca-lar �140 such that, for all t� t0 and kdk¼ 1,dTW(t, tþ �)d4�1. This concludes the first part ofthe proof.
In order to establish necessity, it will be shown thatif the hypothesis does not hold, then the pair (A(t),C) isnot uniformly completely observable. Suppose thenthat, for all scalars �01 4 0 and �4 0, there exists t*� t0and a unit vector d22R
3 such that, for all t2 [t*, t*þ �],it is true that yðtÞ � d2
5�01. Let d ¼ 0 dT2� �T
2 R6.
Then, it is straightforward to conclude that
dTW t, t þ �ð Þd¼
Z tþ�
t
f �, tð Þ 2d�
¼
Z tþ�
t
Z �
tRmð�ÞS yð�Þ½ �d2d�
2d�
�
Z tþ�
t
Z �
tRmð�ÞS yð�Þ½ �d2 2d�d�
�
Z tþ�
t
Z �
tyð�Þ � d2 2d�d�
� y � tð Þ½ � � d2 2Z tþ�
t
Z �
t1d�d�
� y � tð Þ½ � � d2 2�2
2,
where �(t*)2 ]t*, t*þ �[. As yðtÞ � d2 5�01 for all
t2 [t*, t*þ �], it follows that dTW(t*, t*þ �)d5�1,with �1 :¼ �01�
2=2, which means that the pair (A(t),C)is not uniformly completely observable, thereforeconcluding the proof. œ
Remark 3: Theorem 3.3 has a clear physical interpre-tation. In simple words, the nonlinear system (2) isobservable on [t0, tf ] if and only if the vector observa-tion, in body-fixed coordinates, is not a constant vectoron that time interval. Likewise, Theorem 3.4 corre-sponds to the same condition but imposing also apersistency of excitation requirement, meaning that thepair (A(t),C) is uniformly completely observable if andonly if there exists a certain time interval length, �, suchthat there is a minimum change of the vector observa-tion on all time intervals [t, tþ �], t� t0.
4. Filter design
This section presents an estimation solution for thenonlinear system (2). As (2) can be regarded as linear
time-varying, even though it is a nonlinear system, it ispossible to implement the well-known Kalman filterto estimate the system state. This is so because allquantities are available to compute A(t), while C is aconstant matrix. Its stability is readily characterisedfrom Theorem 3.4, see Jazwinski (1970).
Considering additive system disturbances andsensor noise, the system dynamics are given by
_xðtÞ ¼ AðtÞxðtÞ þ wðtÞyðtÞ ¼ CxðtÞ þ nðtÞ
,
�ð10Þ
where w(t)2R6 and n(t)2R
3 are uncorrelated zero-mean white Gaussian processes, with E[w(t)wT
(t� �)]¼��(�), and E[n(t)nT(t� �)]¼?�(�),. 0,? 0. Note that, for filter design purposes, both w(t)and n(t) could have been modelled as the outputs ofstable linear time invariant filters, which could beeasily employed to model, e.g. coloured noise. In thisarticle, and for the sake of clarity of presentation, thesimplest white Gaussian noise version is presented.
The Kalman filter equations for the systemdynamics (10) are standard, see Kalman and Bucy(1961) and Gelb (1974). The state estimate evolvesaccording to
_x tð Þ ¼ AðtÞx tð Þ þ K tð Þ y tð Þ � Cx tð Þ½ �
while the covariance matrix propagation isdescribed by
_P tð Þ ¼ A tð ÞP tð Þ þ P tð ÞA tð Þ þ .� P tð ÞCT?�1CP tð Þ:
The Kalman gain matrix is given by K(t)¼P(t)CT?�1.
The filter error dynamics are globally asymptoti-cally stable under the conditions of Theorem 3.4 as thepair (A(t), C) is uniformly completely observable, see,e.g. Kalman and Bucy (1961), Jazwinski (1970), andAnderson (1971).
Remark 4: Note that it is not possible to claim thatthe Kalman filter is the optimal solution because, inreality, there exists multiplicative noise. An alternativedesign to the Kalman filter is theH1 filter, consideringthat w(t) and n(t) are finite energy signals.
5. Simulation results
In order to evaluate the performance of the proposedsolutions, simulations were first carried out consider-ing that the vector observations are given by anaccelerometer which, for low frequencies, is dominatedby the acceleration of gravity, as discussed in Mahonyet al. (2008). A rate gyro unit provides angular velocitymeasurements corrupted by bias, which was set tobx¼ [2 �3 1]T/180 rad s�1 in the simulations.
International Journal of Control 899
Dow
nloa
ded
by [
Inst
itutio
nal S
ubsc
ript
ion
Acc
ess]
at 1
4:28
18
July
201
1
Sensor noise was added both to the accelerometer andrate gyro measurements. Additive zero-mean whiteGaussian processes were considered, with standarddeviation of 0.05m s�2 for the acceleration measure-ments and 0.05/180 rad s�1 for the rate gyro mea-surements. With nothing else but gravity readings, itis only possible to partially recover the attitude.In particular, two Euler angles may be recovered, theroll and pitch of a roll, pitch, and yaw Euler anglerepresentation of the attitude. These angles corre-spond, in a different terminology, to the bank andelevation angles of air vehicles. The evolutionof the Euler angles during the simulation is
depicted in Figure 1. Note that the trajectory describedby the platform is such that uniform complete observ-ability is attained, according to Theorem 3.4.
In the simulations, the initial estimate of the gravitywas set close to the true value, as it is always possibleto initialize the filter according to the first vectorobservation. The rate gyro bias initial estimate was setto zero. The filter matrices were chosen as.¼ diag(0.05I, 0.01I) and ?¼ 0.05I. The evolution ofthe filter errors is depicted in Figure 2, while theevolution of the Euler angles error computed from thefiltered estimates is shown in Figure 3. The evolutionof the error of the Euler angles computed directly from
0 20 40 60 80 100 120 140 160 180
−0.2
−0.1
0
0.1
0.2
0.3
−0.3
t (s)
Err
or a
ccel
erat
ion
of g
ravi
ty
(m/s
2 )
xyz
0 20 40 60 80 100 120 140 160 180−4
−2
0
2
4
t (s)
Err
or g
yro
bias
(°/
s) xyz
Figure 2. Evolution of the Kalman filter error.
0 20 40 60 80 100 120 140 160 180−30
−20
−10
0
10
20
30
t (s)
Eul
er a
ngle
s (°
)
RollPitch
Figure 1. Evolution of the Euler angles.
900 P. Batista et al.
Dow
nloa
ded
by [
Inst
itutio
nal S
ubsc
ript
ion
Acc
ess]
at 1
4:28
18
July
201
1
the vector observation is also depicted for comparisonpurposes. The results are quite good considering thelow cost characteristics of the sensor suite and the factthat a single vector observation is available to estimatethe rate gyro bias. The filtering effect on the vectorobservations is also quite evident.
6. Experimental results
In order to evaluate the performance in real worldapplications, an experimental setup was developedresorting to a high precision motion rate table, Model2103HT from Ideal Aerosmith. This table outputs, in afixed-frequency profile, the angular position of thetable with a resolution of 0.00025�, considered as aground truth signal for performance evaluation. Theangular velocity and acceleration readings are obtainedfrom an Inertial Measurement Unit (IMU), modelNANO IMU NA02-0150F50, from MEMSENSE,which outputs data at a rate of 150Hz. The worstcase standard deviation values provided by the man-ufacturer of the IMU are 0.008m/s2 for the acceler-ometers and 0.95�/s for the rate gyros. Theseperformance specifications are typical of low costunits. Figure 4 displays the motion rate table with theexperimental setup mounted on the top.
The initial estimates of the filter were set in asimilar fashion to those in the simulations: the estimateof the gravity was set close to the true value, accordingto the first vector observation, while the initial rategyro bias estimate was set to zero. The systemdisturbances intensity matrix was chosen as .¼ 0.5diag(0.008I, 2� 10�8I), while the output noise intensity
matrix was chosen as ?¼ 0.008I. No particular
emphasis was given on the tuning process as the
resulting performance with these simple parameters is
very good. In practice, the spectral contents of the
sensors noise may be experimentally approximated and
frequency weights adjusted to improve the perfor-
mance of the filter, see the examples provided in
Batista, Silvestre, and Oliveira (2010). Moreover,
correlation between the system disturbances w and
the sensor noise n may also be considered.The initial convergence of the filter error is depicted
in Figure 5, as well as the steady-state error. Note that
0 20 40 60 80 100 120 140 160 180−1
−0.5
0
0.5
1
t (s)
RollPitch
0 20 40 60 80 100 120 140 160 180−2
−1
0
1
2
t (s)
Err
or E
uler
ang
les
from
m
easu
rem
ents
(°)
Err
or E
uler
ang
les
from
fil
ter
(°)
RollPitch
Figure 3. Evolution of the error of the Euler angles computed from the Kalman filter estimates.
Figure 4. Experimental setup.
International Journal of Control 901
Dow
nloa
ded
by [
Inst
itutio
nal S
ubsc
ript
ion
Acc
ess]
at 1
4:28
18
July
201
1
the convergence rate is still very fast and the filter copeswell with the low specifications of the sensor suite. Theinitial convergence of the rate gyros biases estimates isshown in Figure 6. Clearly, the filter copes well theslowly time-varying nature of the rate gyro biases.
7. Conclusions
This article presented the design, analysis, and perfor-mance evaluation of a novel filter for partial attitudedetermination, in addition to rate gyro bias estimation,based on single vector observations. The observabilityof the system was thoroughly assessed and conditions
with physical meaning derived. The resulting filter errordynamics are globally asymptotically stable. Simulationand experimental results were also detailed, with groundtruth data for comparison purposes, that illustrate theachievable performance of the envisioned solutions.
Acknowledgements
This work was partially supported by Fundacao para aCiencia e a Tecnologia (ISR/IST pluriannual funding)through the PIDDAC Program funds, by the projectPTDC/MAR/64546/2006 OBSERVFLY of the FCT, andby the EU project TRIDENT of the EC-FP7.
0 0.2 0.4 0.6 0.8 1−6
−4
−2
0
2
4
6
t (s)
Err
or a
ccel
erat
ion
of g
ravi
ty (
m/s
2 )
xyz
200 400 600−0.1
−0.08
−0.06
−0.04
−0.02
0
0.02
0.04
0.06
0.08
0.1
t (s)
Err
or a
ccel
erat
ion
of g
ravi
ty (
m/s
2 )
xyz
Figure 5. Evolution of the acceleration of gravity error: initial convergence and detailed evolution.
0 0.2 0.4 0.6 0.8 1−40
−30
−20
−10
0
10
20
30
40
t (s)
Gyr
os b
ias
estim
ate
(°/s
)
xyz
200 400 600−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
t (s)
Gyr
os b
ias
estim
ate
(°/s
)
xyz
Figure 6. Evolution of the bias estimate: initial convergence and detailed evolution.
902 P. Batista et al.
Dow
nloa
ded
by [
Inst
itutio
nal S
ubsc
ript
ion
Acc
ess]
at 1
4:28
18
July
201
1
References
Anderson, B. (1971), ‘Stability Properties of Kalman–Bucy
Filters’, Journal of Franklin Institute, 291, 137–144.Bar-Itzhack, I., and Oshman, Y. (1985), ‘AttitudeDetermination from Vector Observations: Quaternion
Estimation’, IEEE Transactions on Aerospace andElectronic Systems, 321, 128–136.
Batista, P., Silvestre, C., and Oliveira, P. (2009),
‘Sensor-based Complementary Globally AsymptoticallyStable Filters for Attitude Estimation’, in Proceedings ofthe 48th IEEE Conference on Decision and Control, China:Shanghai, pp. 7563–7568.
Batista, P., Silvestre, C., and Oliveira, P. (2010), ‘OptimalPosition and Velocity Navigation Filters for AutonomousVehicles’, Automatica, 46, 767–774.
Batista, P., Silvestre, C., and Oliveira, P. (2011), ‘On theObservability of Linear Motion Quantities in NavigationSystems’, Systems and Control Letters, 60, 101–110.
Crassidis, J., Markley, F., and Cheng, Y. (2007), ‘Survey ofNonlinear Attitude Estimation Methods’, Journal ofGuidance, Control and Dynamics, 30, 12–28.
Farrell, J. (1970), ‘Attitude Determination by KalmanFilter’, Automatica, 6, 419–430.
Gelb, A. (1974), Applied Optimal Filtering, Cambridge, MAand London: The MIT Press.
Jazwinski, A. (1970), Stochastic Processes and FilteringTheory, New York and London: Academic Press, Inc.
Kalman, R., and Bucy, R. (1961), ‘New Results in
Linear Filtering and Prediction Theory’, Transactions ofthe ASME – Journal of Basic Engineering, Series D, 83,95–108.
Mahony, R., Hamel, T., and Pflimlin, J.-M. (2008),‘Nonlinear Complementary Filters on the Special
Orthogonal Group’, IEEE Transactions on Automatic
Control, 53, 1203–1218.Metni, N., Pflimlin, J.-M., Hamel, T., and Soueres, P. (2006),‘Attitude and Gyro Bias Estimation for a VTOL UAV’,
Control Engineering Practice, 14, 1511–1520.Rehbinder, H., and Ghosh, B. (2003), ‘Pose Estimation
Using Line-based Dynamic Vision and Inertial Sensors’,
IEEE Transactions on Automatic Control, 48, 186–199.Sanyal, A., Lee, T., Leok, M., and McClamroch, N.
(2008), ‘Global Optimal Attitude Estimation usingUncertainty Ellipsoids’, Systems and Control Letters,
57, 236–245.Savage, P. (1998a), ‘Strapdown Inertial Navigation
Integration Algorithm Design Part 1: AttitudeAlgorithms’, Journal of Guidance, Control, and Dynamics,
21, 19–28.Savage, P. (1998b), ‘Strapdown Inertial Navigation
Integration Algorithm Design Part 2: Velocity andPosition Algorithms’, Journal of Guidance, Control, and
Dynamics, 21, 208–221.Tayebi, A., McGilvray, S., Roberts, A., and Moallem, M.
(2007), ‘Attitude Estimation and Stabilisation of a Rigid
Body using Low-cost Sensors’, in Proceedings of the 46thIEEE Conference on Decision and Control, New Orleans,
USA, pp. 6424–6429.Thienel, J., and Sanner, R. (2003), ‘A Coupled Nonlinear
Spacecraft Attitude Controller and Observer With anUnknown Constant Gyro Bias and Gyro Noise’,
IEEE Transactions on Automatic Control, 48,
2011–2015.Vasconcelos, J., Cunha, R., Silvestre, C., and Oliveira, P.
(2010), ‘A Nonlinear Position and Attitude Observer onSE(3) using Landmark Measurements’, Systems and
Control Letters, 59, 155–166.
International Journal of Control 903
Dow
nloa
ded
by [
Inst
itutio
nal S
ubsc
ript
ion
Acc
ess]
at 1
4:28
18
July
201
1