On the Singularity in the Estimation of the Quaternion-of-Rotation

11 Pages·2002·0.63 MB·English

by  Bar-Itzhack, Itzhack Y.

AIAA 2002-4831
ON THE SINGULARITY IN THE ESTIMATION OF THE QUATERNION-OF-ROTATION
Itzhack Y. Bar-ltzhack _and Ju}ic K. Thicnc) 2
ABSTRACT a-priori estimate of the full quaternion. Because the
It has been claimed in the archival literature that the difference itself is defined as a quaternion, this update is
covariance matrix of a Kalman filter, which is designed performed through a quaternion multiplication [1, 2]
to estimate the quaternfon-of-rotation, is necessarily rank .hence the name muhiplicative approach. Since this
deficient because the normality constraint of the difference is a quaternion, its length is unit), and,
quaternion produces dependence between the quaternion therefore, one of its components is a deterministic
elements. In reality, though, this phenomenon does not function of the other three.
occur. The covariance matrix is not singular, and the In the AEKF [3], the difference between the true and
filter is well behaved. Several simp]e examples are estimated quaternions is defined as a simple subtraction
presented that demonstrate the regularity of the of one vector from the other. Using this approach the
covariance matrix. First, a Kalman filter is desired to difference is estimated and then the estimate is added to
estimate variables subject to a functional relationship. the a-priori full quaternion estimate hence the name
Then the particular problem of quaternion estimation is additive approach. This vector difference does not
analyzed. It is shown that the discrepancy stems from the constitute a quatemion; therefore, its len_h is not
fact that the functional relationship exists between the necessarily equal to unity. In fact, if the quaternion and
elements of the quaternion but not between its estimated its estimate are close to one another, then surely the
elements. difference quatemion is not of unit lengh. Naturally, the
unity constraint is not imposed on the elements of the
I. INTRODUCTION difference quaternion, which become part of the
The quaternion-of-rotation is a four-element estimated state vector. Therefore, the corresponding
parameterization of attitude, and since the quatemion is covariance ma_ix is not inherently singular.
normal, one element is redundant. This fact has brought In this paper we show three realities. First we show
researchers [see e.g. Ref. I] to the conclusion that when a that even if there is a functional relationship between the
Kalman filter (KF) is used to estimate all four parameters tr_e values of states, this relationship does not
of the quaternion-of-rotation, the filter covariance matrix necessarily exist between their estimates; therefore, the
is necessarily singular. The argument behind this filter-computed eovariance matrix is not necessarily
assertion is that the dependent variables cause singular. Second we show that even if such arelationship
singularities. However, as will be shown in the ensuing is imposed on the estimates, the covariance is still not
discussion, the assertion that the covariance matrix is necessarily singular, and, third, we show that these
singular when all four elements of the quaternion are claims hold, in particular, for the case of quatemion
estimated is not necessarily true, and if it happens to be estimation when the additive approach is employed.
singular, it is not because of the quaternion normality. We establish these realities in an evolutionary
We note that there are two principal approaches to manner. We start our presentation with a conceptual
the application of an extended KF (EKF) to quaternion- example of estimating the vertices of a rotating square.
of-rotation estimation; namely, the multiplicative This example illustrates the first reality; namely, even if
approach that yields the multiplicative EKF (MEKF) and there is a functional relationship between the true values
the additive approach that yields the additive EKF of states there is not necessarily a relationship between
(AEKF). In the MEKF the difference between the their estimates. Then we present a simple linear example
estimated and the true quaternion is defined as a of estimating the position of the edges of a sliding rod.
quatemion-of-rotation between the true coordinate Here we demonstrate numerically the same reality and
system, and the estimated coordinate system. In the also the second reality; namely, even if a relationship is
estimation process the components of this difference imposed on the estimates, the covariance is still not
quaternion are estimated, and are then used to update the necessarily singular. To demonstrate that these realities
ISophie andWilliam Shamban Professor ofAerospace Engineering. "l'echnion-lsrael Institute ofTechnology, Haifa 32000, Israel.
Member Yechnion Asher Space Research/nsdm_e. Tel:+972..4.g29-3t96, Fax:4-972..4-g23-_g48, Ema/hibaritz(i3tx.technionac.if
2AerospaceEngineer,NASA GSFC, Code 5"72.Greenbelt, MD205'7]. Tel:[phone], Fax:[phone], Emai]:iutielhienel_,,esfc.nasa uov
Copyright
1
American Institute of Aeronautics and Astronautics
exiswt hentilequaternioisnestimatewde,starwt itha locationosftheverticeosf a squarek,nowingthe
simplleineaerxampolefestimatitnilgefourelemenotfs locatioonfthreeofthems,ayal,a2anda3,wecanfind
a quaterniownhenthemeasuremeanretsquaterniona4,thefourthofthem.
measuremeWnetcsh.oosaestaticcasewhertehebodiys This, however, does not mean
notrotatinTg.hischoiceenableusstoanaI)¢icaptrloyve that if we know Yl, Y2 and Y3 we also know Y4- As is
thefirstrealitfyorthiscaseIn.thefinalexampwleetreat indeed shown in Fig. 1, Yl,Y_,,Y3,Y4 do not form a
theclassicaclasewherethebodyrotateasndthe
square. In other words, the fact that we know that there is
measuremaernevtsectomr easuremeFnotrtsh.iscaswee
a deterministic relationship between the four vertices of
numericadllyemonstrtahtaetbothrealitieesxiswt hen
the square does not mean there is also a deterministic
quaternioanrseestimateudsingtheAEKFn;ameltyh,e
relationship between their estimates. Similarly, the fact
filter-compuctoevdarianmceatrixisnon-singualanrd,it
that there is a deterministic relationship between the four
remainnson-singuelavrenwhennormaliitsyi'mposeodn
elements ql,q2,q3 and q4 of a quaternion does not
thefilterestimatiensabrutforcemanneTro.explatihne
mean there is also a deterministic relationship between
resultosfthelatterexampwleeanalyztheeoperatioonf
theordinarEyKFw, hichisactuallaynAEKFa,ndshow their corresponding estimates ql, q2, q3 and q4.
thattheforcendormalizatoiofntheestimateqduaternionMoreover, if we use the normality constraint to compute
hasnobearinognthecovarianmceatrix. one element of the quatemion as a function of the other
three estimated elements, the result will not necessarily
II.ESTIMATIOONFSQUARVEERTICES be equal to the estimate &that element.
Westarot urpresentatwiointhaconcepteuxaal mple
of estimatinthgeverticeosf arotatingsquareT.his III. ESTIMATION OF THE EDGES OF A ROD
exampliellustratetshefactthatevenif thereis a After having seen conceptually that there is no reason
functionarellationshbipetweetnhecorrecvtalueosf to assume that an algebraic relationship that exists
estimatesdtatetshereisnotnecessarailryelationshipbetween the states of a system is also carried to their
betweethneirestimateCso.nsidethresystemdescribedestimates, we move fom, ard to numerically demonstrate
inFig.Iwheraesquariesplaceodnadiskthatturnsat this fact and the assertion that even if the relationship is
anangulavrelocityc,o.Weobtainnoisymeasuremenitmsposed on the estimates, the filter covariance is still not
oftheverticeosfthesquaraendtrytoestimatthee necessarily singular.
locatioonftheseverticeosnthediskS. uppotsheatour Consider the rod shown in Fig. 2. It slides along the
initiae]stimaptelacetsheverticeast×_,x:,x3andx4. x-axis at a constant velocity V. The coordinates of its
edges are s_ and s2, respectively. In order to describe
the equations of motion of the two edges in the state
space we define the following state variables
x1=s_ (1.a)
x2 = fil = V = const. (1.b)
i,_=0 (l.c)
x3 = s= (1.d)
x4 = x3 = V = const. (1.e)
i4 =0 (l,f)
L
Fig.1:Estimatioofntheverticeosfasquare I v
0 S_ S2
Aftertheseconmdeasuremuepndtateth,eestimates
movetopointsY_,Y2,Ya3ndY4respectivaenlydso
on.Theestimatekseepmovingalongthecurved
trajectorieusntiltheyreachaninfinitesimdailstance Fig. 2: Moving rod
fromtheverticeas_,a,_3,aanda4.(Fig.1showtshe
positionofthesquareattheendof theestimation
processB)ecaustheereisarelationshbipetweethne
In matrix form these equations are
2
American Institute of Aeronautics and Astronautics
rxili001ixi
I0+8_ ]
,:_ o o oJIx:[
=IV +82[
(:a) C:)
LV+S4j
L#4j 0 00JLx4 j
and in adiscrete form they are where 81 is 0.2, 82 is0.l, 83 is-0.2, and 84 is-01.
Accordingly, we set the initial covariance matrix to be
the following diagonal matrix
x; =lo 1 oo/x: /
(2.b)
xx,3] [[1:o, otilFr1xx,3/ P0=diag{(3.0.2) 2 (3-O1) 2 (3.0.2) 2 <3.0.1)2}(8)
x4 k-_ 0 0 1JLx_J_
The filter is run for 20 sec and in-spite of the following
dependence between the states x_ and x2
where At is the time difference between the discrete
x3= xI+ L (9)
time points tk and tk+1. We assume that at the time
point designated as zero, the rear end of the rod, %, no singularity is observed in zhefilzer covariance man-ix.
This is evident by asimple inspection of Fig. 3 where the
passed by the origin, 0, at the constant velocity, V.
behavior of the filter covariance matrix eigenvalues is
Therefore the true initial state vector is
presented.
ELoenvaolufIheesEs_maEtiorr_oCrovar_akn_cae_(x}'1>=L2>=:_:>3=1._)
16e ,
104I _--
eO 2 4 6 8 10 12 t4 16 18
The measured quantities are the positions of sI and s2 t0t ' ' ' /J
on the x-axis. It is assumed that these measurements are
104k
contaminated by zero mean white noise signals vt and ,o-,[<.. t
v2 respectively, thus the measurement vector is _0 2 4 6
I0" , ' _ I
(4)
Zm =[ XI +VI ] loaf i i r z i , , i
X3+ V2 ,0 2 4 6 8 I0 12 1'4 li 18
Using the following data
L=2m; V=lm/sec; At=lsec Or, =Gv2 =0.Ira 2 4 6 B 1_0 1'2 1t 1'_
(s)
Fig. 3: Eigenvatues of the covariance matrix
Eqs. (1) through (4) are used to simulate the correct state
vector and the measurements. A linear Kalman filter It is not surprising that no singularity occurs in the
filter covariance matrix because the relationship bern, con
provides estimates of the state vector. To avoid the well-
known divergence phenomenon that occurs in unexcited x3 and xI is not imposed in the filter model. One may
state vector dynamics [4], zero mean white process noise speculate that imposing the relationship on the estimates
is added to the dynamics equation in the filter only. A R3 and _I will cause the matrix to become singular. In
covariance matrix, Qk is added to the time-propagated order to examine this proposition, we imposed the
filter covariance matrix. The matrix is a diagonal matrix distance constraint between the rod end-points by forcing
with the values the new a-posteriori estimates of and Rs to be
i I
_[(+)=_[%(+)+_3(+)- L] (]O.a)
Qk =diag,0 -I 10-] 10-1 10-1} (6)
and
Our estimate of the initial state is _;(+)=}[_,(+)+_3(+)+L] (10.b)
3
American Institute of Aeronautics and Astronautics
(Notethatasaresulotfthischangaen,additionearlror The recurrence relations that describe the covariance
termmusbteaccountfeodr,sinctehestateestimataerse
propagation are
alteredT.histermisincludeidnthefilterpropagation Y
Pk (-) = Ak-TPk-1 (+)A k-I +Qk-1 (13.a)
andupdatsetageasccordintogthediscussipornesented
m SectionVsII andVIII) No singularitiny the Kk = Pk(-)HkT[HkPk(-)H T +ak] -_ (13.b)
covariance can be detected as shown in the eigenvalue Pk(+) = [I- KkHk]Pk (-) (l3.c)
history in Fig. 4.
where Ak =H k =l. In this example we choose
Qk-1 =0. Let us consider a case where Rk = rI. Eqs.
E_en_,'alLeosft_ Esiimali_ErrorCo',,_amerd_Irix()1)= )2>:)3>:),_)
(13) become
_C+k., I Pk(-) = Pk-I (+) (14.a)
jo+["_ 1
_0 2 4 6 8 10 ;2 11 16 I_, Kk = Pk(-)[Pk (-) + rI]-_ (14.b)
Pk(+) =[1- Kk ]Pk (-) (14.C)
lO'tk
2_ 104IM Using Eq. (14.a), gqs. (14.b) and (14.c) can be written as
4 6 8 _0 12 },_ 16 1_ 20
1040 2 }
Kk = Pk<C+)[Pk_t(+)+ r]]-1 (14.d)
_ 102[ !
10._F Pk(+) = [I- Kk]Pk-I (+) (14.e)
4 6 8 1_0 12 1'4 16 II_ 20
T(t.£ 2 1
Since P0 is a diagonal matrix, all the matrices in Eqs.
(14) are diagonal. Therefore, we can write the last
i r r , , ' ' , 1
equation for any of the elements of Pk (+) in terms of the
{s_l
same element in Pk-1 (+). Dropping the plus sign, we
Fig. 4: EigenvaIues of the covarianee matrix in the obtain the following recurrence relation for any eIement
constrained case. of Pk(+):
rpk-]
IV. QUATERN1ON ESTIMATION WITH Pk = _ (15.a)
QUATERNION MEASUREMENTS Pk-I +r
Ne×t in our evolutionary exposition of the with
singularity issue we demonstrate the tack of covariance
PO =_2 (15.b)
singularity when the quaternion is estimated. We start
with a simple linear example of estimating the four It is quite obvious that Pk, which is an eigenva]ue of
elements of a quaternion when the measurements are Pk (+), is not zero and thus the covariance matrix of the
quaternion measurements. We choose a static case where filter is not singular.
the body is not rotating. This choice enables Us to prove
analytically that the filter-computed covariance matrix of V. ESTIMATION OF A QUATERNION FROM
the non-normalized quaternion estimate for this case is VECTOR MEASUREMENTS
not singular. In this final example we treat the classical case where
Consider a rigid satellite body, fixed in inertial space. the body rotates and the measurements are vector
The system equations that describe this simple, static measurements, we numerically demonstrate that the
case are filter-computed covariance matrix is non-singular both
qk = [qk-I (l ].a) when we do and do not impose normality on the
estimated quaternion. First we present the simulation,
qm,k = Iqk +Vk (l l.b)
followed by the tilter development.
where I is the fourth order identity matrix, and qm,k is a
measurement of the quatemion at time, tk. This system is V.I Simulation
Without loss of generality, we consider a case where
linear; therefore, the ordinary KF can be applied in
the body rotates at the constant angular rate. Let the rate
estimating q. ConsequentIy a covariance analysis can be
vector in this simulation be
carried out which is independent of the state or of its
estimate. Let us assume that
Po= _2l (l2.a) co=[O.1 0.20.3]Trad/sec (76)
4
American Institute of Aeronautics and Astronautics
Thequaterniaotntimetk._asafunctioonfitsvalueat Using Eq. (20) we obtain
timetkisgivenby[5,p.512]
[(q_-q_-q32 +q_)h ÷2(q,q2 +qsq4)r2 +2(qlqs -q2q4)r3 ]
D(q)r =/2(qlq2 -qsq4)rl +(-ql2+q22-q_ +q42)r2+2(q2q 3+qlq4)rs/
I
-_(c; ).zxt [2(qlq3 +q2q4)rl +2(q2q3 -qlq4)r2 +(-q_ -q22' q_+q24)r._J
qk÷l = e2 qk (17)
(23)
We assume that two vector measurements are obtained at
where h, r2 and r3 are the components ofr Define
each time point, tk, They constantly point at two
Hk+! = 0[D(q)r] (24)
celestial objects located 900 apart and are given in the
3q G,,(-)
reference coordinates as
then using Eq. (23) we obtain
rI=[1 0 0]r (18.a)
r2=[0 1 0]T (18.b) (q:_ +q2r2+q:s) (-q:l +qlr2 -q:3)
In body coordinates we simulate two corresponding Hk.i =21(q2rl-qlr2 +q4h) (q:l +q2r:_+q3r3)
measured vectors as [(q3r, -q4r2 -q:s) (q,rl +q:2-q:3)
(-q:J +qd2 +qlr3) (qd_ +%r2-%h) ]
1
bl,k+ 1= D(qk+i)r t + vI (19.a) (-q4q -%r2 "+q2r3) (-q3rl +q4r2 +qlr3)
b2,k+ 1= D(qk+l)r2 + v2 (19.b) (q:_ *qff2+q:3) (q:J -q_h +q,r3) J_,.,(_)
(25)
where vIand v2 are uncorrelated, Gaussian white
and Eq. (22.d) becomes
sequences with covariance R each and [5, p. 414]
Zk+l =Hk+ldqk+l + Vk+ 1 (26)
Iq_ -q_-q32 +q] 2(qlq2 +q3q4) 2(qlq3 - q2q,) ]
D(qk.1)=_ 2(qlq2-q3q4) -q_ 2+q2-q23 2+q42 2(q2q3 +qlq4) I The filter is propagated and also updated every
k 2(q'q3 +qzq4) 2(q2q3 -q'q4) -q12-q_ +q_ +q_Jk+l
second. At every update point we have two effective
(20)
vector measurements, Zl,k+ ! and Z2,k+ 1. The algorithm
In addition to bl,k+ 1 and b2,k+ 1 the filter is also is a standard EKF algorithm. We ran two cases, one
furnished with rl, r;, co and the covariance matrix R. without normalization of the estimated quatemion and
one with normalization. Fig. 5 shows the attitude error
V.2 Filter development when normalization is not imposed on the estimated
quaternion, with an initial error of 10 degrees. Fig. 6
V.2.1 Dynamics shows the covariance matrix eigenvalues, with the scale
The filter dynamics equation is simply reduced to show the steady-state behavior. Fig. 7 shows
the deviation from unity of the norm of the estimated
quaternion. Figs. 8 and 9 present the results for the case
qk÷l = e2 qk (2l) where normalization is imposed on the estimated
quaternion. Note that the eigenvalues do not change as a
V.2.2 Measurements result of the quaternion normalization. (See Sections VII
and VIII for a discussion on the changes to the EKF
The effective measurement equation for the AEKF is
algorithm as a result of the normalization.
developed as follows
Zk+ I = bk+ l - I_k+ l = D(qk+ l)r - D(l_k+l(-))r + Vk+ 1 l
(22.a)
gk+l=D(I_Ik+l(-) +dqk+l)r - D(I_1k+l(-))r +Vk+I
(22.b)
Zk+1=D(Clk+J(-))r + _ dqk+l- D(clk+l(-))r + vk+l
(22.c)
time (secon,ts)
Zk+ 1 3[D-(q-)r] dqk+l + Vk+ 1 (22.d)
8q I_.,(_)
Fig. 5: Attitude error
5
American Institute of Aeronautics and Astronautics
1xlO_ -7" , Ix,10. ' , , ,
__o."s.,_.,....!.... _................ __o.s__.,-.....................................I................................
" / "'-., /
,,, j ..... , ;
;n ............................... _ "J
" ktll_ 500 1000 1500 2000 " _1_1<0 51)0 1000 1500 2000
=l]z--z- r , i
....i............'............:..-............1...................
_o,_' J
°'.,..i......i....
0/ ............. :.............. .,.....,.,......... ................
10"_ 500 1000 1500 2000 11_0_...500...10..00......15.0.0.......2.t0..00.................................
o,'...._............:............_......................................
°-.. ....... ,}................... ..................
! {lo._ 500 1000 1500 2000
1_10_ 500...1.000. 1500 2t000
•....".._......;..;...o...o......;..."..._........................
O_ 500' 1000 1500 2000 o_
time(seconds) time(seconds)
Fig. 9: Covariance matrix eigenvalues.
Fig. 6: Covariance matrix eigenvalues.
VI. THE FULLY RESET ADDITIVE EKF
To explain the results obtained in the preceding
examples, without the imposed constraints, we need to
analyze the operation of the AEKF, and for that we need
to review the EKF algorithm. We do it in a manner that is
somewhat different from the usual textbook algorithm
development, but the resultant algorithm is the standard
EKF. (We need to adopt this approach for the
development that will be presented in the following
section.) Here we treat the special case that is applicable
to quaternion estimation, where the measurement model
is nonlinear, but the dynamics model that describes the
state propagation is linear.
time (seconds) VI,1 Measurement Update
A measurement vector Yk, at time tk, is related to
Fig. 7: Norm error of the estimated quatemion
the state vector Xk, at time tk, according to the
following nonlinear vector function
Yk = h(Xk) + Vk (27)
where Vk is a zero mean white noise vector. Suppose
that at this time point we have an a-priori estimate,
_:k(-), of this state vector. We wish to use the
_' i¸I i ¸ ! i i measurement Yk to improve this a-priori estimate. The
improved estimate is called the a-posteriori estimate and
is denoted by ;_k(+). The KF was developed for linear
measurements whereas the measurement equation, Eq.
(27), is nonlinear. However, as will be shown in the
ensuing discussion, the EKF estimates the difference, e,
between the true state vector, X, and its estimate, X,
°OL---- 200 400 _;o0 BO(} _oo_ 1200 _4r._ lSOO 1_o0 20O0
time (seconds) and the model treated by the EKF is, to first order, a
linear model in e .
Fig. 8: Attitude error in the constrained case
6
American Institute of Aeronautics and Astronautics
Werecaltlhatthestateupdateequatioinnthelinear
ek(+) = ek(-) + KkZk (37)
KFis
_k(+)=)_k(--)+Kk[Yk-5'k] (28.a) Substitution of Eq. (36) into Eq. (37) yields
whcrc
J'k=HkXk(-) (28.b) ek(+ ) = ek(- )- KkHkek(-)- KkV k (38)
whichresultfsromthelineamr odel which can be written as
Yk = HkXk + Vk (28.0 ek(+ )=[1- KkHk ]ek (-)- Kk vk (39)
Assuming (the knowingly inaccurate assumption) that
In EKF terminology the term Yk-Yk is known as {he
the filler is unbiased; that is, E{ek(+)} = 0, the updated
effective measurement, which we denote by zk ;that is
estimation error covariance is computed as follows
Zk = Yk--)'k (29)
Pk(+) = E{ek (+)ekT(+)} (40)
Using Eq. (27) we can write the effective measurement
as follows.
Substituting Eq. (39) along with the fact that
zk = h(Xk )+ vk - )'k (30)
E{e k(-)v T} = 0, and the following notations
Note that 5'k, in Eq. (28.b), was obtained from the linear Rk =E{Vk vT} (41)
measurement equation (28.c) by dropping the noise
Pk(-) = E{ek (-) eT (-)} (42)
vector and substituting -'_k(-) for Xk . If we do the
Eq. (40) becomes
same in Eq. (27) then Eq. (30) becomes
P_(+) =[I- KkHk]Pk (-)[I- KkHk]r + KkRkK T (43)
zk = h(Xk) + vk-h(Xk(-)) (31)
X
As mentioned before, we denote by e the difference
between the true and estimated state vector; that is,
e= X - X (32)
Because X = X + e, we can expand h(X k) in a Taylor
series about 5_, drop the second and higher order terms
in e, and substitute the result into Eq. (31). This yields
zk as a linear function of e. The first order expansion of
h(Xk) about _(-) yields
h(Xk)=h(y_k(_))+ 3h(X) ek(-) (33)
_X X=Xd-)
q
and substitution of the last equation into Eq. (31) gives
tk_ t_ Time
Zk = Oh(X) ek(-) + vk (34)
8X x=fq (-) Fig. 10: The state, its estimate, the difference and its
Let estimate attime tk.
Hk = 0h(X) (35)
The sequence of events associated with the
3X x=_q (-)
measurement update in the EKF is presented in Fig. 10
then Eq. (34) can be written as and can be summarized as follows:
zk = Hke k(-) + vk (36)
1. Y(k<(+) is propagated from time tk< to time point
Following the measurement-update equation of the state tk to become Xk(-).
vector in Eq. (28.a) we write
2. The difference Xk - Xk (-) is ek(-).
7
American Institute of Aeronautics and Astronautics
3.Themeasuremuepndtatyeieldsek(+),anestimaotfe
Substituting Xk_ 1 given in Eqs. (47) into Eq. (46) yields
ek(-). Xk (-) + ek(-) = Ak-lXk-1 (+) +Ak-lek-l(+) + Wk-1 (58)
4. _k(+) is added to Xk(-) to form Xk(+) which is Since the dynamics model of(21) is linear [6, p. 75 ]
then propagated to time point tk+ I to become
J_k(-) = Alc-IXk-I (+) (59)
J_k+I (-). We call this operation full reset. Note that
once ek(+) is absorbed in Xk (+) it is not propagated Subtracting the last equation from Eq. (48) yields
separately.
ek(-) = Ak_lek_l (+) + Wk_ 1 (50)
Once ek (+) is added to X-k(-) a full reset is performed,
and there is no _k(+) to be propagated forward; Using these results we can now examine the time
propagation of the state estimate and the covariance
therefore, ek(-), the a priori estimate of e at the next
matrix in this particular EKF. Assuming E{ek(-)} = 0,
time update point, is also zero.
the propagated covariance matrix is defined as
Using the preceding explanation, the EKF algorithm
at the measurement update can be summarized as
Pk(-) =E{ek(-)ekv(-)/ (5l)
follows.
Substitution of ek(-), given in Eq. (50), into the last
At a measurement update: expression, given that ek_1(+) and W'k_I are
Signal:
uncorrelated, and using the notation
ek(-) = 0 (44.a)
Qk-1 = E{Wk-IwkT-I} we obtain the well known result [6,
ek(+) =ek (-) + Kk (Yk -Yk) (44.b)
p. 76]
Xk (+) = Xk (-) + ek(+) (44.c) Pk(-)=Ak-lPk-l( +)Ak-TI +Qk-I (52)
Note that a sequential substitution of Eqs. (44.a, b, c)
Using Eqs. (49) and (52) we can summarize the time
yields the following textbook expression for the state
propagation stage of the EKF when the dynamics model
measurement-update equation
is linear.
Xk(+) = Xk(--) + Kk(Yk -_'k) (44d) Time update:
Signal:
Covariance: Xk(-) =Ak-1Xk-_(+) (53)
Pk(+) = [l- KkHk ]Pk(-)[I- KkH k]r Covariance:
(45)
+K_Rk KT Pk(-) = Ak-lPk-l( +)Ak-Y1 +Qk-I (54)
In the next section we will use the formulation of the state We realize from the preceding development that the
measurement-update given in Eqs. (44.a-c) to show the AEKF that is used to estimate the quatemion when no
effect that normalization has on the state measurement- normalization takes place is the ordinary EKF algorithm.
update algorithm. This cannot be shown when the Moreover, it is obvious that the fact that the true
textbook expression of Eq. (44.d) is used, qaaternion is a normal vector has no bearing on the filter
covariance matrix. This is, of course, also true for all the
VI.2 Time Propagation linear examples presented before.
Consider the case where the discrete process
equation that describes the time propagation of a general VII. THE PARTIALLY RESET ADDITIVE
EXTENDED KALMAN FILTER
state vector isgiven by
To explain the results obtained in the numerical
Xk= Ak_lXk_ 1+ wk_I (46) examples, when constraints were imposed, we need to
consider a special version of the EKF. For this we will
From Eq. (32) and as illustrated in Fig. 10 make use of the developments presented in the previous
section.
X___ = Xk-l (+) +ek-I (+) (47.a)
VII.1 Time Propagation
and Figure 11.a describes the steps of the ordinary (fully
Xk = ;_k(-) + ek(--) (47.b) reset) EKF at the measurement update stage as described
8
American Institute of Aeronautics and Astronautics
intheprecedinsgectioanndillustrateindFig.10.We
We note in Fig. ll.b that the partial reset, does not
haveshowtnhataftecromputin_gk(+),theestimaotef
change the value of the actual estimation error, e_(+),
ek,itisaddetdo?(k(-) toformXk(+)whichisthen
therefore the partial reset does not influence the
propagatteodtimepointtk+b tobecomie_k+l()-. covariance matrix because the latter is _he cavariance
matrix of e, the estimation error itself.
Sincegk(+)in itsentiretyisaddedto Xk(+),this
The AEKF algorithm for the time propagation stage
operatiocnonstituteasfull resetB. ecaus_ek(+)is in this partially reset case is then as follows.
propagatefodrwardasa partof Xk(+), it is not
At a time update
propagatienddependenStulyp.posneowthat
only a part Signal:
of _k(+), denoted in Fig. l].b by Ak, is added to Xk (-) = Ak-,J(_-, (+) (56.a)
Xk(-) to form Xk(+) which is then propagated to time
ek(-) = Ak-le_-_ (+) (56.b)
point tk÷1 tO become J_k+l (-) 'This partial reset leaves Covariance:
ek (+) out of the propagated full state vector. Therefore Pk(-) = Ak-,Pk-,(+)A[__ +Qk-f (57)
b.k(+) has to be propagated forward separately. It is easy
VII.2 Measurement Update
to show that when the dynamics model is linear, _k(+)
is propagated according to From the preceding discussion of the partial reset
operation and the developments presented in Section VI,
it is obvious that the measurement update is performed as
ek. 1(-) = Akek (+) (55.a) follows.
Reducing the index by 1yields Measurement update
Signal:
_,k(-) = A__,O__(+,) (55b3
ek(+) = ek(-) + Kk (Yk -_'k) (58.a)
%(+)
ek (+)
.-v...ak X
I i;
:_(+)
tk_ 1 tk Time tk- l tk Time
(b) The partially reset case
(a) The fully reset case
Fig. 11: Fully versus partially reset case
9
American Institute of Aeronautics and Astronautics
_k(+) =_k(-). G(+) (58.b) Table I: AEKF with Full and Partial Reset
System Model:
CovaHance: Dynamics: Xk = AkXk_ 1+ Wk_ 1
Pk(+)=[I- KkHk]P_(-)[I - KkHk]T
(59) Measurement: Yk = h(Xk) + Vk
+ KkRkK T AEKF with Linear AEKF with Linear Dynamics
Dynamics and Full Reset and Partial Reset
VII.3 Reset Update Time Propagation: Time Propagation:
Although the change from X_(+) to XI(+), shown
Xk (-) = ak-lJ_k-I (+) Xk (-) = Ak-, X_-_ (+)
in Fig. l l.b, can be of any nature, because we are
^*
Pk-1 (-) = Ak-I Pk-1 (+) ek(-) = A k_lek_l (+)
dealing with the normalization operation, we chose the
particular change AkT-1 +Qk-1 Pk-1 (-) = A k-1Pk-1 (+)
(60.a)
AT_I +Qk-I
It is clear from Fig. 17.b that once XI(+) is defined, the Measurement Update: Measurement Update:
value of fi_(+) is also defined as ek (+) = Kk(Yk - Yk) _ (+) : _ (-)
xk(+) =5q(-) +_k(+)
_ (+) = X_ (+) - J_ (+) (60.b) + Kk (Yk - 9k)
Consequently:
Note that the condition of partially reset state vector _ (+) : iq (-) Xk (-I') = )(k (-) + ek (+)
Consequently:
does not come about by adding Ak to ;Kk(-) but
+ K_(y_ -5'k) _:(+) =_:_(-)+ _(-)
rather by subtracting fi_(+) from the fully reset state Pk(+)= [I- KkHk]ek (-). + K,(y,-_',)
vector J_k(+)" As explained before, the partial reset [I-K_Hk] T ek(+) = [I- KkHk]Pk(- )
operation does not influence the covariance matrix; +KkRkK T [I-KkHk] T
therefore, the reset update is performed as follows.
+ KkRkKk T
Reset update
Signal: Reset Update:
/q(+) =/q(+)//q(+) (61.a) <(+)=L (+)/IX(4
_(+) =x_(+)- .xi_(+) (61.b)
Covariance: Pk(+) =Pk(+)
P_(+) = Pk(+) (62)
Both the fully and partially reset AEKF are summarized
in Table I.
bearing on the covariance matrix. Indeed for this
VIII. THE REGULARITY OF THE AEKF FOR example we derived an analytic expression that exhibited
QUATERNION ESTIMATION no singularity.
After presenting some illustrative examples and
having prepared the theoretical background we can now In the more complicated case with a rotating body
analyze the results of the exarnples. We start with and vector measurements, the dynamics equation is linear
quaternion estimation using the AEKF without but the measurement equation is nonlinear. In that case
normalization of the estimated quaternion. we used the AEKF and still found no singularity. The fact
that the true quaternion is normal does not enter into the
Quaternion estimation without normalization computation of the covariance matrix. The only special
We presented two cases of quaternion estimation; feature of this case is the orthogonality of the transition
namely, a static case with quaternion measurements and matrix that propagates q, which preserves the norm of the
a dynamic case with vector measurements. The former propagated state vector be it normal or not.
constituted a strictly linear estimation problem that
required the use of a standard KF. In the standard KF Quaternion estimation with normalization
there is no connection between the quaternion estimate Forcing normalization on the a-posteriori estimate of
and the filier covariance matrix. Therefore, theoretically, the quaternion does not affect the covariance matrix. The
the normality quality of the true quaternion had no
10
American Institute of Aeronautics and Astronautics