THE highly successful quaternion multiplicative extended

Transcription

1 JOURNAL OF GUIDANCE, CONTROL, AND DYNAMICS Extene Kalman Filter for Spacecraft Pose Estimation Using Dual Quaternions Downloae by GEORGIA INST OF TECHNOLOGY on May 9, 01 DOI: 10.14/1.G0009 Nuno Filipe, Michail Kontitsis, an Panagiotis Tsiotras Georgia Institute of Technology, Atlanta, Georgia DOI: 10.14/1.G0009 Base on the highly successful quaternion multiplicative extene Kalman filter for spacecraft attitue estimation using unit quaternions, this paper proposes a ual quaternion multiplicative extene Kalman filter for spacecraft pose (i.e., attitue an position) an linear an angular velocity estimation using unit ual quaternions. By using the concept of error unit ual quaternion, efine analogously to the concept of error unit quaternion in the quaternion multiplicative extene Kalman filter, this paper proposes, as far as the authors know, the first multiplicative extene Kalman filter for pose estimation. The state estimate of the ual quaternion multiplicative extene Kalman filter can irectly be use by recently propose pose controllers base on ual quaternions, without any aitional conversions, thus proviing an elegant solution to the output ynamic compensation problem of the full six egree-offreeom motion of a rigi boy. Three formulations of the ual quaternion multiplicative extene Kalman filter are presente. The first takes continuous-time linear an angular velocity measurements with noise an bias an iscretetime pose measurements with noise. The secon takes only iscrete-time pose measurements with noise an hence is suitable for satellite proximity operation scenarios where the chaser satellite has only access to measurements of the relative pose, but requires the relative linear an angular velocities for control. The thir formulation takes continuous-time angular velocity an linear acceleration measurements with noise an bias an iscrete-time pose measurements with noise. The propose ual quaternion multiplicative extene Kalman filter is compare with two alternative extene Kalman filter formulations on a five egree-of-freeom air-bearing platform an through extensive Monte Carlo simulations. I. Introuction THE highly successful quaternion multiplicative extene Kalman filter (Q-MEKF) base on unit quaternions for spacecraft attitue estimation, escribe in etail in ([1] Sec. XI), has been use extensively in several NASA spacecraft []. It has been analyze in great etail throughout the years [ 6]. Part of the Q-MEKF success lies in the fact that unit quaternions provie a global nonsingular representation of attitue with the minimum number of parameters. Moreover, they appear linearly in the kinematic equations of motion, unlike Euler angles, which require the calculation of computationally expensive trigonometric functions. Although newer approaches, such as nonlinear observers, have been shown to have some avantages over the classical extene Kalman filter (EKF), a comprehensive survey of nonlinear attitue estimation methos has conclue that the classical EKF is still the most useful an practical solution to the attitue estimation problem []. Note that the lack of success of Kalman filtering before 196, when Richar Battin was eveloping Apollo s onboar navigation an guiance system, is mainly attribute to the inability to moel the system ynamics accurately enough [1]. An aitional major avantage of the Q-MEKF is that the 4 4 covariance matrix of the four elements of the unit quaternion oes not nee to be compute. As state in [1], propagating this covariance matrix is the largest computational buren in any Kalman filter implementation. By rewriting the state of the EKF in terms of the three elements of the vector part of the unit error quaternion between the true unit quaternion an its estimate, only a covariance matrix nees to be compute. The unavoiable rawback of this approach is that all three-imensional attitue representations are singular or Receive 1 August 014; revision receive 4 January 01; accepte for publication 9 January 01; publishe online 9 May 01. Copyright by. Publishe by the American Institute of Aeronautics an Astronautics, Inc., with permission. Copies of this paper may be mae for personal or internal use, on conition that the copier pay the $10.00 per-copy fee to the Copyright Clearance Center, Inc., Rosewoo Drive, Danvers, MA 019; inclue the coe 1-884/1 an $10.00 in corresponence with the CCC. *Ph.D. Caniate, School of Aerospace Engineering; nuno.filipe@gatech.eu. Stuent Member AIAA. Postoctoral Research Associate, School of Aerospace Engineering. Dean s Professor, School of Aerospace Engineering. Fellow AIAA. Article in Avance / 1 iscontinuous for certain attitues [4]. Inee, by construction, the Q-MEKF escribe in ([1] Sec. XI) will fail if the attitue error between the true attitue an its estimate is larger than 180 eg. However, unlike the true attitue of the boy, which can vary arbitrarily, the attitue error between the true attitue of the boy an its estimate is expecte to be close to zero, especially after the Q-MEKF has converge. Hence, in the Q-MEKF, whereas the attitue covariance matrix is only, the boy can still have any arbitrary attitue. This is one of the most appealing properties of the Q-MEKF. Note that the 180 eg restriction in the Q-MEKF is benign because the Q-MEKF will most likely fail even before the attitue error reaches 180 eg, ue to the linearization assumptions intrinsic to the EKF. The vector part of a unit quaternion is only one of several possible three-imensional representations of the attitue error in the Q- MEKF [4]. Other possible representations are, for example, the rotation vector, the Rorigues parameters, or the moifie Rorigues parameters. These representations have been shown to be equivalent up to thir orer an hence are equivalent for the EKF an seconorer filters [4]. In this paper, the attitue error is represente using the vector part of a unit quaternion as in [1]. For a thorough iscussion of the pros an cons of each representation, the reaer is referre to [4]. Note, however, that, as mentione earlier, all three-imensional attitue representations are singular or iscontinuous for certain attitues [4]. Recently, Filipe an Tsiotras [,8] have propose a new metho to evelop pose controllers starting from existing attitue-only controllers by using the transfer principle between quaternion an ual quaternion escriptions [9]. In particular, unit ual quaternions offer a compact representation of the position an attitue of a frame with respect to another frame. Their properties, incluing examples of previous applications, are iscusse in length in [8]. However, the property that makes ual quaternions most appealing for the applications we are intereste in, is that the combine translational an rotational kinematic an ynamic equations of motion, when written in terms of ual quaternions, have the same form as the rotationalonly kinematic an ynamic equations of motion written in terms of quaternions (albeit the operations have now to be interprete in ual quaternion algebra). The traitional approach to estimate the pose of a boy consists of eveloping separate estimators for attitue an position. For example, Romano et al. [10] suggests two iscrete-time linear

2 Article in Avance / FILIPE, KONTITSIS, AND TSIOTRAS Downloae by GEORGIA INST OF TECHNOLOGY on May 9, 01 DOI: 10.14/1.G0009 Kalman filters to estimate the relative attitue an position separately. Because the translation Kalman filter requires the attitue estimate by the rotation Kalman filter, the former is only switche on after the latter has converge. Owing to this inherent coupling between rotation an translation, several authors have propose estimating the attitue an position simultaneously. For example, in [11], a laner s terrain-relative position an attitue are estimate simultaneously using an EKF. The state of the EKF contains the vector part of the unit error quaternion (like in the Q-MEKF) an the position vector of the laner with respect to the inertial frame expresse in the inertial frame. In [1], the relative position an attitue of two satellites are also estimate simultaneously using an EKF. In this case, the state of the EKF contains the vector part of the unit error quaternion (like the Q-MEKF) an the position vector of the chaser satellite with respect to the target satellite expresse in a reference frame attache to the target satellite. The approach escribe in [1] is cooperative, in the sense that the two satellites share their angular velocity measurements. Finally, Goar [1] also estimates the position an attitue between two frames simultaneously using a iscrete-time EKF. In [1], the state contains the position vector of a boy with respect to some reference frame expresse in that reference frame along with the four elements of the true quaternion escribing the orientation of the boy. Hence, Goar [1] oes not take avantage of the concept of unit error quaternion. Moreover, in [1], the optimal Kalman state upate is ae to, an not multiplie with, the current best unit quaternion estimate, making the EKF presente in [1] aitive instea of multiplicative. Next, we elaborate why a multiplicative error escription is more appropriate for this problem. Goar [1] takes avantage of the compactness of ual quaternions to represent three-imensional lines (an their relative position an orientation) to evelop the measurement upate of the EKF. As far as the authors know, the only previous EKF formulations where the state inclues a unit ual quaternion are given in [14,1]. However, these EKF formulations inclue the true unit ual quaternion escribing the pose of the boy an not the error unit ual quaternion between the true unit ual quaternion an its best estimate. Therefore, the state of the EKF formulations presente in [14,1] contains all eight elements of the unit ual quaternion. Moreover, the EKF formulations propose in [14,1] are aitive EKF formulations (i.e., the optimal Kalman state upate is ae to, an not multiplie with, the current best unit ual quaternion estimate). As a consequence, the preicte value of the unit ual quaternion immeiately after a measurement upate oes not fulfill the two algebraic constraints that a unit ual quaternion must satisfy. Hence, in [14], the preicte value after a measurement upate is further moifie to satisfy these constraints through a process that inclues parameters that must be tune by the user. On the other han, in [1], these two algebraic constraints are simply not enforce after a measurement upate, which can lea to numerical problems. Finally, it shoul be mentione that the iscrete-time EKF formulations in [14,1] are esigne to take only measurements from a camera. Compare with the existing literature, the main contributions of this paper are as follows: 1) By using the concept of error unit ual quaternion, efine analogously to the concept of error unit quaternion of the Q-MEKF, this paper proposes, as far as the authors know, the first multiplicative EKF for pose estimation. As a consequence, the preicte value of the unit ual quaternion immeiately after a measurement upate automatically satisfies the two algebraic constraints of a unit ual quaternion. Unlike in [14], no aitional parameters nee to be tune by the user. ) By using the error unit ual quaternion instea of the true unit ual quaternion, the state of the ual quaternion multiplicative extene Kalman filter (DQ-MEKF) is reuce from eight elements (as in [14,1]) to just six. As a consequence, the associate computational complexity for implementation is reuce. Moreover, the state estimate of the DQ-MEKF can be irectly use by the pose controllers given in [,8] without aitional conversions. ) Similar to the Q-MEKF, the DQ-MEKF is a continuous-iscrete Kalman filter [16] (i.e., the state an its covariance matrix are propagate continuously between iscrete-time measurements). One of the avantages of this approach is that the iscrete-time measurements o not nee to be equally space in time, making irregular or intermittent measurements easy to hanle. Moreover, this structure eases the incorporation of ifferent sensors with ifferent upate rates. In particular, the DQ-MEKF escribe in this paper is esigne to take continuous-time linear an angular velocity measurements with noise an bias an iscrete-time pose measurements with noise. This paper also proposes two extensions of this stanar DQ-MEKF. The first extension is esigne to take only iscrete-time pose measurements with noise an estimate the linear an angular velocities. This version is suitable for satellite proximity operation scenarios where the chaser satellite has only access to measurements of the relative pose (e.g., from a camera), but requires the relative linear an angular velocities for control. In the secon extension, the linear velocity measurements of the stanar DQ-MEKF are replace with linear acceleration measurements with bias an noise. This version is suitable for a satellite equippe with an accelerometer an having no means of irectly measuring linear velocity. 4) Finally, the two extensions of the stanar DQ-MEKF are valiate experimentally on a five egree-of-freeom (-DOF) airbearing platform. The first extension is also compare with two alternative EKF formulations, similar to the ones use in [10 1], on the same -DOF platform an through Monte Carlo simulations. It is shown that the DQ-MEKF compares favorably with these alternative formulations. This paper is organize as follows. In Sec. II, the main equations of a stanar EKF are reviewe. Then, the DQ-MEKF is erive in Sec. III, starting with a brief introuction about quaternions an ual quaternions an ening with the erivation of two variations of the DQ-MEKF that may be most useful for spacecraft proximity operations in space. In Sec. IV, the DQ-MEKF is valiate experimentally an compare with two alternative EKF formulations. Finally, in Sec. V, the first extension is compare again with the same two alternative EKF formulations through Monte Carlo simulations. II. Extene Kalman Filter The main equations of the EKF are reviewe to introuce the necessary notation for the remaining sections. The review is base on a similar review provie in [1] an serves as the starting point of the DQ-MEKF formulation. The state equation of the EKF can be written as _x n t f n x n t;tg n p x n t;tw p t (1) where x n t R n is the state an w p t R p is the process noise. The process noise is assume to be a Gaussian white noise process, whose mean an covariance are given by Efw p tg 0 p 1 an Efw p tw T pτg Q p p tδt τ, respectively, where Q p p t R p p is a symmetric positive semiefinite matrix. The initial mean an covariance of the state are given by Efx n t 0 g ^x n t 0 x n;0 R n an Efx n t 0 x n;0 x n t 0 x n;0 T g P n n t 0 P n n;0 R n n an are assume to be known. [Note that in [14,1], p n an g n p x n t;ti n n ]. A. Time Upate Given the initial mean of the state, the minimum covariance estimate of the state at a future time t in the absence of measurements is given by the conitional expectation ^x n t Efx n tj^x n t 0 x n;0 g. This estimate satisfies the ifferential equation _^x n t Eff n x n t;tg, which is typically approximate as _^xn t f n ^x n t;t () Hence, in the absence of measurements, the state estimate is propagate using Eq. (). In aition to the state estimate, the covariance matrix of the state also nees to be propagate. The covariance matrix of the state is given by

3 Article in Avance / FILIPE, KONTITSIS, AND TSIOTRAS Downloae by GEORGIA INST OF TECHNOLOGY on May 9, 01 DOI: 10.14/1.G0009 P n n t EfΔx n tδx T ntg R n n where Δx n t x n t ^x n t R n is the state error. As a first-orer approximation, the erivative of the state error is given by t Δx nt F n n tδx n tg n p tw p t () an the covariance matrix of the state satisfies the Riccati equation where _P n n t F n n tp n n tp n n tf T n nt G n p tq p p tg T n pt (4) F n n t f nx n ;t x n ^xn t R n n an G n p t g n p ^x n t;t R n p () Hence, in the absence of measurements, the covariance matrix of the state is propagate using Eqs. (4) an (). B. Measurement Upate Assume that a measurement is taken at time t k that is relate with the state of the EKF through the nonlinear output equation z m t k h m x n t k v m t k R m (6) where v m t k R m is the measurement noise assume to be a iscrete Gaussian white noise process, whose mean an covariance are given by Efv m t k g 0 m 1 an Efv m t k v T mt l g R m m t k δ tk t l, where R m m t k R m m is a symmetric positive efinite matrix. Immeiately following the measurement at time t k, the minimum variance estimate of x n t k is given by where ^x n t k ^x n t k Δ ^x n t k () Δ ^x n t k K n m t k z m t k ^z m t k (8) is the optimal Kalman state upate, where ^z m t k h m ^x n t k (9) an ^x n t k an ^x n t k are the preicte values of the state immeiately before an after the measurement, an K n m t k is the Kalman gain. The Kalman gain is given by K n m t k P n nt k H T m nt k H m n t k P n nt k H T m nt k R m m t k 1 10 where P n nt k is the preicte state covariance matrix immeiately before the measurement an H m n t k h mx n R x n ^x m n (11) n t k is the measurement sensitivity matrix. Immeiately after the measurement, the state covariance matrix is given by P n nt k I n n K n m t k H m n t k P n nt k I n n K n m t k H m n t k P n nt k I n n K n m t k H m n t k T (1) K n m t k R m m t k K n m t k T (1) where Eq. (1) is numerically more stable than Eq. (1). III. Extene Kalman Filter for Spacecraft Pose Estimation Using Dual Quaternions This section provies a quick introuction to quaternions an ual quaternions. Then, a combine angular an linear velocity measurement moel analogous to the angular velocity measurement moel escribe in [1] is propose. After that, the DQ-MEKF for pose estimation base on ual quaternions is erive using ual quaternion algebra. Finally, two versions of the DQ-MEKF that may be useful in practice are propose. The first one is useful when angular an linear velocity measurements are not available (i.e., only pose measurements are available) an the secon is useful when linear velocity measurements are replace by linear acceleration measurements. A. Mathematical Preliminaries For the benefit of the reaer, the main properties of quaternions an ual quaternions, which are essential for eriving the results presente in this paper, are summarize in this section. For aitional information on quaternions an ual quaternions, the reaer is referre to [,1]. 1. Quaternions A quaternion is efine as q q 0 q 1 i q j q k, where q 0, q 1, q, q R an i, j, an k satisfy i j k 1, i jk kj, j ki ik, an k ij ji [18]. A quaternion can also be represente as the orere pair q q 0 ; q, where q q 1 q q T R is the vector part of the quaternion an q 0 R is the scalar part of the quaternion. Vector quaternions an scalar quaternions are quaternions with zero scalar part an vector part, respectively. The set of quaternions, vector quaternions, an scalar quaternions will be enote by H fq: q q 0 q 1 i q j q k; q 0 ;q 1 ;q ;q Rg, H v fq H: q 0 0g, an H s fq H: q 1 q q 0g, respectively. The basic operations between quaternions are efine as follows: Aition: Multiplication by a scalar: Multiplication: Conjugation: Dot prouct: a b a 0 b 0 ; a b H λa λa 0 ; λ a H ab a 0 b 0 a b; a 0 b b 0 a a b H (14) a a 0 ; a H a b 1 a b b a 1 ab ba a 0 b 0 a b;0 1 H s Cross Prouct: a b 1 ab b a 0;b 0 a a 0 b a b H v where a; b H, λ R, an 0 m n is an m n matrix of zeros. Note that the quaternion multiplication is not commutative. In fact, some authors [1] efine Eq. (14) as ba, an not as ab as originally efine by Hamilton [19]. This paper follows the original efinition by Hamilton. Finally, the quaternions 1; 0 an 0; 0 will be enote by 1 an 0, respectively.

4 4 Article in Avance / FILIPE, KONTITSIS, AND TSIOTRAS Downloae by GEORGIA INST OF TECHNOLOGY on May 9, 01 DOI: 10.14/1.G0009 The bijective mapping between the set of quaternions an R 4 will be enote by : H R 4, where q q 0 q 1 q q T. Using this mapping, the cross prouct of a H v with b H v can be compute as a b a b, where : H v R 4 4 is efine as 0 a a ; where a 4 0 a a a 0 a 1 (1) a a 1 0 Likewise, the left quaternion multiplication of a H with b H can be compute as ab a b, where : H R 4 4 is efine as a0 ā a T ; where ã a ā ã 0 I ā (16) The relative orientation of a frame fixe to a boy with respect to another frame, enote here as the I frame, can be represente by the unit quaternion q cos ϕ ; sin ϕ where the boy frame is sai to be rotate with respect to the I frame about the unit vector n by an angle ϕ. A unit quaternion is efine as a quaternion that belongs to the set H u fq H: q q qq q q 1g. From this constraint, assuming that 180 < ϕ < 180 eg, the scalar part of a unit quaternion can be compute from q q 0 1 k qk (1) where k k enotes the usual Eucliean norm in R. The coorinates of a vector in the B frame v B can be calculate from the coorinates of the same vector in the I frame v I, an vice versa, via v B q vi q an v I q v B q, where vb 0; v B an v I 0; v I. This is equivalent to v B Rq v I an v I Rq vb, where Rq an Rq are the rotation matrices corresponing to q an q, respectively.. Dual Quaternions A ual quaternion is efine as q q r ϵq, where ϵ is the ual unit efine by ϵ 0 an ϵ 0. The quaternions q r, q H are the real part an the ual part of the ual quaternion, respectively. Dual vector quaternions an ual scalar quaternions are ual quaternions forme from vector quaternions (i.e., q r, q H v ) an scalar quaternions (i.e., q r, q H s ), respectively. The set of ual quaternions, ual scalar quaternions, an ual vector quaternions will be enote by H fq: q q r ϵq ;q r ;q Hg, H s fq: q q r ϵq ; q r ;q H s g, an H v fq: q q r ϵq ;q r ;q H v g, respectively. The basic operations between ual quaternions are efine as follows [0,1]: Aition: Multiplication by a scalar: Multiplication: n a b a r b r ϵa b H λa λa r ϵλa H Dot prouct: Cross prouct: a b 1 a b b a 1 ab ba a r b r ϵa b r a r b H s a b 1 ab b a a r b r ϵa b r a r b H v where a; b H an λ R. Note that the ual quaternion multiplication is not commutative. In this paper, the ual quaternions 1 ϵ0 an 0 ϵ0 will be enote by 1 an 0, respectively. The bijective mapping between the set of ual quaternions an R 8 will be enote by : H R 8, where q q r T q T T. Using this mapping, the left ual quaternion multiplication of a H with b H can be compute as ab a b, where : H R 8 8 is efine as ar 0 a 4 4 (18) a a r Finally, it is convenient to efine : H R 6 6 as ear 0 ~a ea ea r (19) where : H R 6 is efine as a a T r a T T R 6, an : H R 6 6 is efine as a a r 0 (0) a Similarly, r : H H is efine as a r a r an : H H is efine as a a. The attitue an position (i.e., pose) of a boy frame with respect to another frame, say, the I frame, can be represente by a unit quaternion q H u an by a translation vector r I R or r B R,wherer X Y Z is the translation vector from the origin of the Z frame to the origin of the Y frame expresse in the X frame. Alternatively, the pose of the boy frame with respect to another frame can be represente more compactly by the unit ual quaternion [] q q ;r ϵq ; q ϵ 1 ri q q ϵ 1 q r B a r (1) where r X Y Z 0; rx Y Z. Note that the ual part of q (i.e., q ; )isa representation of the position of the boy frame with respect to the I frame. Given q, the position of the boy frame with respect to the I frame can be obtaine in I frame coorinates from r I q ; q an in B frame coorinates from rb q q ;. Figure 1 illustrates this relation between r B, q ;, anr I.Note that, whereas the relation between r B an ri is quaratic in q, q ; is relate linearly in q with r B an ri. A unit ual quaternion is efine as a ual quaternion that belongs to the set [] Conjugation: ab a r b r ϵa r b a b r H a a r ϵa H Fig. 1 Relation between r B, q ;, an r I.

5 Article in Avance / FILIPE, KONTITSIS, AND TSIOTRAS Downloae by GEORGIA INST OF TECHNOLOGY on May 9, 01 DOI: 10.14/1.G0009 H u fq H : q q qq q q 1g fq H : q r q r 1 an q r q 0g () From this constraint, assuming that 180 < ϕ < 180 eg, the scalar parts of the real an ual parts of a unit ual quaternion can be compute from their respective vector parts from q q r;0 1 k q r k an q ;0 q r T q q r;0 () The rotational an translational kinematic equations of the boy frame with respect to another frame can be written compactly in terms of ual quaternions as [] _q 1 ωi q 1 q ω B (4) where ω X Y Z is the ual velocity of the Y frame with respect to the Z frame expresse in the X frame, ω B ωb ϵvb, ωi ωi ϵv I ωi ri, ωx Y Z 0; ωx Y Z, ωx Y Z is the angular velocity of the Y frame with respect to the Z frame expresse in the X frame, v X Y Z 0; vx Y Z,an vx Y Z is the linear velocity of the origin of the Y frame with respect to the Z frame expresse in the X frame. Note that the rotational an translational kinematic equations written in terms of ual quaternions []. B. Angular an Linear Velocity Measurement Moel The ual velocity measurement moel is efine analogously to the angular velocity measurement moel typically use in literature [1,], that is, ω B ;m ωb b ω η ω () where ω B ;m ωb ;m ϵvb ;m Hv ; ωb ;m 0; ωb ;m ; ω B ;m is a measurement of the angular velocity of the boy frame with respect to the I frame expresse in the boy frame, v B ;m 0; v B ;m ; vb ;m is a measurement of the linear velocity of the origin of the boy frame with respect to the I frame expresse in the boy frame; b ω b ω ϵb v is the ual bias, b ω 0; b ω ; b ω R is the bias of the angular velocity measurement, b v 0; b v ; b v R is the bias of the linear velocity measurement, η ω η ω ϵη v, η ω 0; η ω ; η ω R is the noise of the angular velocity measurement assume to be a zero-mean Gaussian white noise process, η v 0; η v ; an η v R is the noise of the linear velocity measurement assume to be a Gaussian white noise process with Efη ω g 0 6 1, an covariance Efη ω tη T Q ωτg Q ω tδt τ ω t Q ωv t Q ωv t δt τ Q v t (6) where Q ω t R 6 6 is a symmetric positive semiefinite matrix. The ual bias is not constant, but assume to be riven by another zeromean Gaussian white noise process through _b ω η bω () where η bω 0; η bω ϵ0; η bv, Efη bω g 0 6 1, an covariance Efη bω tη T b ω τg Q bω tδt τ " # Q bω t Q bω b v t δt τ (8) Q bω b v t Q bv t where Q bω t R 6 6 is a symmetric positive semiefinite matrix. If the I frame is inertial, ω B shoul be interprete as the inertial angular an linear velocities of the satellite expresse in the B frame. In that case, ω B can be measure using a combination of, say, rate gyros, Doppler raar, an GPS. On the other han, if the I frame is not inertial, ω B shoul be interprete as the relative angular an linear velocities of the satellite with respect to a frame attache, for example, to another satellite. In that case, ω B can be measure using a combination of, say, rate gyros on both satellites [1], Doppler raar, ifferential GPS, an light etection an ranging. C. Dual Quaternion Multiplicative Extene Kalman Filter In this section, the DQ-MEKF for pose estimation is erive. The state an process noise of the DQ-MEKF are initially selecte as x 16 δq R b ω 16 ηω an w 16 R η bω 16 (9) where the ual error quaternion δq H u is efine analogously to the error quaternion [1] δq ^q q H u as δq ^q q H u (0) that is, δq H u is the ual quaternion between the actual ual quaternion q H u an its current best guess ^q H u. Analogously to the propagation of ^q H u in [4], ^q is propagate using where, from Eq. (), t ^q 1 ^q ^ω B (1) ^ω B EfωB gefωb ;m b ω η ω gω B ;m ^b ω () with ^b ω Efb ω g an t ^b ω Efη bω g0 () The approximation in Eq. (1) is a result of using the typical EKF approximation given by Eq. () in the erivation of Eq. (1) [4]. Analogously to [4], for 180 < ϕ < 180 eg, δq is parameterize by δq an the expecte value of δq is require to be zero (i.e., Efδq g ). Hence, Efδq δq g 1. Note that the current best guess of q, given by ^q, is not efine as the stanar expecte value of the ranom variable q because this woul require the expectation to be efine with respect to a nontrivial probability ensity function in H u. As shown in [], even the efinition of probability ensity function on H u is not trivial. A complete iscussion of probability ensity functions in H u is outsie the scope of this paper. The reaer is referre to [] for a iscussion of possible probability ensity functions in H u. A geometric interpretation of the ual error quaternion δq is given in Fig.. It is the ual unit quaternion that escribes the relative pose between the B frame an the ^B frame. The B frame represents the true pose of the boy frame, whereas the ^B frame represents the expecte pose of the boy frame; in other wors, it represents the best available guess of the pose of the boy frame. To etermine the state equation of the DQ-MEKF, the time erivative of δq nees to be calculate. Taking the time erivative of Eq. (0) yiels t δq t ^q q ^q t q (4) Substituting Eqs. (4) an (1) in Eq. (4) yiels

6 6 Article in Avance / FILIPE, KONTITSIS, AND TSIOTRAS Numerical errors in the propagation of ^q through Eq. (1) can result in the violation of the algebraic constraints specifie by Eq. (). Hence, after each integration step, these algebraic constraints are enforce by calculating q ;r q ;r an kq ;r k q ; I 4 4 q ;rq ;r T kq ;r k q ; (4) Fig. Interpretation of the ual error quaternion. where the latter equation correspons to the projection of q ; onto the subspace orthogonal to q ;r. As for the covariance matrix of x 1, that is, Downloae by GEORGIA INST OF TECHNOLOGY on May 9, 01 DOI: 10.14/1.G0009 t δq 1 ^ωb ^q q 1 ^q q ω B 1 ^ωb δq 1 δq ω B () Combining Eqs. () an () yiels ω B ^ωb ^b ω b ω η ω (6) Finally, inserting Eq. (6) in Eq. () results in t δq 1 ^ωb δq 1 δq ^ω B 1 δq ^b ω 1 δq b ω 1 δq η ω () At this point, as in the erivation of the Q-MEKF, reuce state an process noise vectors are selecte, namely, x 1 δq b ω R 1 ηω an w 1 R η 1 (8) bω where δq an b ω are the vector parts of δq an b ω, respectively. The state equations of the DQ-MEKF are then given by the vector parts of Eqs. () an (), yieling f 1 x 1 t;t 1 ^ωb δq 1 δq ^ω B 1 δq ^b ω 1 δq b ω (9) g 1 1 x 1 t;t 1 e δq I 6 6 (40) By replacing the scalar parts δq ;r;0 an δq ;;0 through Eq. () in Eqs. (9) an (40) an using Eq. (), F 1 1 t an G 1 1 t can be etermine to be F 1 1 t 4 1 I an " 1 G 1 1 t # I 6 6 (41) P 1 1 t EfΔx 1 tδx 1 t T g δq t 06 1 δq t 06 1 T E b ω t ^b ω t b ω t ^b ω t (4) it is propagate accoring to Eq. (4) given P 1 1 t 0 an where Q Q 1 1 t ω t (44) Q bω t. Measurement Upate In this section, it is assume that a measurement of q is available. If the I frame is a moving frame, this measurement can come, for example, from a vision-base system. If the I frame is an inertial frame, this measurement can come, for example, from a combination of a star sensor an a GPS. If the pose measurement is available in terms of a quaternion an a translation vector, then the corresponing ual quaternion can be compute from Eq. (1). Then, the output equation is efine analogously to the output equation use in [4,1] when a quaternion measurement is available, that is, ^q t k q ;m t k δq t k v 6 t k (4) where, in accorance with Eq. (6), z 6 t k ^q t k q ;m t k an h 6 x 1 t k δq t k Hence, using Eq. (11) to calculate the measurement sensitivity matrix yiels H 6 1 t k I (46) In summary, for the measurement upate of the DQ-MEKF, the Kalman gain is calculate from Eq. (10), whereas the optimal Kalman state upate is calculate from Eq. (8) as Δ ^x 1 t k 4 Δ δ ^q t k K 1 6 t k z 6 t k ^z 6 t k Δ ^bω t k K 1 6 ^q t k q ;m t k (4) The estimate of the state at time t k after the measurement is then calculate from ^q t k ^q t kδ δ ^q t k (48) 1. Time Upate For the time upate of the DQ-MEKF, ^q, ^ω B, an ^b ω are propagate using Eqs. (1 ), respectively, given ^q t 0 an ^b ω t 0. ^b ω t k ^b ωt k Δ ^bω t k (49) where Δ δ ^q is efine as the unit ual quaternion

7 Article in Avance / FILIPE, KONTITSIS, AND TSIOTRAS Downloae by GEORGIA INST OF TECHNOLOGY on May 9, 01 DOI: 10.14/1.G0009 Δ δ ^q q! 1 kδ δ ^q ;r k ; Δ δ ^q ;r! ϵ Δ δ ^q T ;r Δ δ ^q ; q ; Δ δ ^q ; 1 kδ δ ^q ;r k (0) Note that Eq. (0) assumes that the attitue error between the true attitue an its estimate is smaller than 180 eg. Moreover, note that Eq. (0) oes not assume that the scalar part of the real part of Δ δ ^q is ientically one as in [1], but instea uses the nonlinear quaternion reset suggeste in [4]. If the initial guess of the state is not close enough to the real state, the norm of Δ δ ^q ;r may become larger than one, which will make the scalar part of the quaternions in Eq. (0) complex. Hence, if the norm of Δ δ ^q ;r is larger than one, Eq. (0) is replace by 0 1 Δ δ 1 Δ δ ^q ;r q ; qa 1 kδ δ ^q ;r k 1 kδ δ ^q ;r k 0 1 ϵ@ Δ δ ^q T ;r Δ δ ^q ; q ; Δ δ ^q ; A 1 1 kδ δ ^q ;r k Note that, whereas Eq. (49) is a irect application of Eq. (), Eq. (48) is not. Since Δ δ ^q t k is a unit ual quaternion, ^q t k is calculate using the ual quaternion multiplication, making the propose EKF multiplicative. Finally, the covariance matrix of the state immeiately after the measurement at t k is compute from Eq. (1). Any measurement that is a nonlinear function of the state of the DQ-MEKF [i.e., any measurement that satisfies Eq. (6)] can be use in the measurement upate. If another measurement is use, only the measurement sensitivity matrix given by Eq. (46) nees to be recalculate. For example, if the measurements are q ;m an r I ;m, then the output equation is efine as 4 ^q t k δq q ;m t k ;r t k r I ;m t 4 k ^q δq δq ^q v 6 t k (1) since r I q ;q ^q δq δq ^q. By replacing δq ;r;0 an δq ;;0 through Eq. () in Eq. (1), the new sensitivity matrix can be etermine to be I H 6 1 t k 0 R ^q 0 0 () D. Special Case: No Angular or Linear Velocity Measurements A special case of particular interest is when pose measurements are available, but angular or linear velocity measurements are not available. Although angular an linear velocity measurements are not available, angular an linear velocity estimates might be require for pose stabilization or tracking [8]. In this section, it is shown how this case can be hanle by moifying the inputs an the parameters of the DQ-MEKF algorithm, without any moification to the structure an basic equations of the DQ-MEKF algorithm. This version of the DQ- MEKF is specially suite for satellite proximity operations where the relative pose is measure using vision-base systems, which typically o not provie irect relative velocity measurements [4]. In this scenario, the I frame is the moving frame of the target satellite. If angular an linear velocity measurements are not available, but estimates are require, ω B ;m an η ω are set to zero in Eq. (). This results in b ω ω B () an Q ω The ual velocity estimate is still given by Eq. (), which now has the form ^ω B ^b ω. The time erivative of b ω is still calculate as in Eq. (). However, because b ω is now expecte to be time varying an not constant, the effect of the noise η bω might have to be increase by increasing Q bω. In summary, this special case can be hanle by just setting ω B ;m an Q ω to zero an, if necessary, by increasing Q bω. E. Special Case: Linear Acceleration Measurements Unlike the previous case, the structure of the DQ-MEKF algorithm escribe in Sec. III.C nees to be moifie for the case of a satellite having no means of irectly measuring linear velocity, but with the ability to measure linear acceleration using an accelerometer or an inertial measurement unit (IMU). Because an accelerometer measures accelerations with respect to the inertial frame, in this section, the I frame shoul be interprete as an inertial frame. The main moifications compare with the algorithm escribe in Sec. III.C are the aition of the bias of the accelerometer to the state of the DQ- MEKF an a new expression for the time erivative of b v, which in this case is not calculate from Eq. (). Because angular (but not linear) velocity measurements an linear (but not angular) acceleration measurements are assume to be available, the uality between the linear an angular motion is broken in this case. Hence, the equations of the DQ-MEKF for this particular case cannot be written compactly in terms of ual quaternions as in Secs. III.C an III.D. First, similar to the angular an linear velocity measurement moel, the linear acceleration measurement moel is efine as [11] n B A I;m nb A I b n η n (4) where n B A I 0; nb A I, nb A I is the nonimensional specific force at the location of the accelerometer with respect to the inertial frame expresse in the boy frame, n B A I;m 0; nb A I;m, nb A I;m is a measurement of n B A I prouce by the accelerometer/imu, b n 0; b n, b n is the bias of the specific force measurement, η n 0; η n,anη n is the noise of the specific force measurement assume to be a Gaussian white noise process with Efη n g0 1 an Efη n tη T nτg Q n tδt τ, where Q n t R is a symmetric positive semiefinite matrix. The bias is not constant, but assume to be riven by another zero-mean Gaussian white noise process through _b n η bn () where η bn 0; η bn, Efη bn g 0 1, an Efη bn tη T b n τg Q bn tδt τ, an Q bn t R is a symmetric positive semiefinite matrix. From Eq. (4), the expecte value of n B A I is given by where ^b n Efb n g.likewise, ^n B A I nb A I;m ^b n (6) t ^b n Efη bn g0 () Moreover, combining Eqs. (4) an (6) yiels n B A I ^nb A I ^b n b n η n (8) The state an process noise of the DQ-MEKF are now selecte as x 0 δq T b ω T b n T T R 0 an w 0 η ω T η bω T η n T η bn T T R 0 (9) where the state equation for δq is given by Eq. () an the state equation for b ω (i.e., the real part of b ω ) is given as in Eq. () by _b ω η bω, which implies that

8 8 Article in Avance / FILIPE, KONTITSIS, AND TSIOTRAS t ^b ω Efη bω g0 (60) Whereas, in Sec. III.C, the time erivative of b v was also calculate from Eq. (), here, the time erivative of b v is calculate as follows. Because there are no linear velocity measurements, v B ;m an η v are set to zero (as in Sec. III.D) in Eq. (), resulting in b v v B an Q v Q ωv 0. This, in turn, implies that ^v B ^b v (61) Taking the time erivative of both sies of b v v B leas to _b v _v B.Notethat_vB is relate to nb A I through _b v _v B ^ωb ^b ω b ω η ω b v c ^n B A I ^b n b n η n δq ^q gi ^q δq ^ω B ^b ω b ω η ω ^ω B ^b ω b ω η ω r B A B ^ω B ^b ω b ω b v c ^n B A I ^b n b n δq ^q gi ^q δq ^ω B ^b ω b ω ^ω B ^b ω b ω r B A B b v η ω cη n η ω ^ω B ^b ω b ω r B A B ^ω B ^b ω b ω η ω r B A B η ω η ω r B A B (6) Downloae by GEORGIA INST OF TECHNOLOGY on May 9, 01 DOI: 10.14/1.G0009 _v B ωb vb cnb A I q gi q α B rb A B ωb ωb rb A B (6) where c R is a scaling constant specific to each accelerometer, g I 0; g I, g I is the local gravity accelerationvector expresse in the inertial frame (assume to be known), α B 0; αb, αb is the angular acceleration of the boy frame with respect to the inertial frame expresse in the boy frame, r B A B 0; rb A B, an rb A B is the translation vector from the origin of the boy frame to the accelerometer expresse in the boy frame (assume to be known). Hence, _b v _v B ωb b v cn B A I q gi q α B rb A B ωb ωb rb A B Finally, neglecting α B, which is assume to be unknown, an using Eq. (8) an the real parts of Eqs. (0) an (6) results in The last term of Eq. (6) is quaratic with respect to η ω an hence oes not have the same form as Eq. (1). Because the typical EKF formulation oes not account for terms quaratic with respect to the process noise, this term is neglecte. Note that, by using the typical approximation given by Eq. (), the time erivative of ^b v can be calculate from Eq. (6) to be _^b v ^ω B ^b v c ^n B A I ^q gi ^q ^ω B ^ωb rb A B (64) At this point, as before, reuce state an process noise vectors are selecte, namely, x 1 δq T b T ω b T n T R 1 an w 1 η T ω η T b ω η T n η T b n T R 1 (6) The state equations of the DQ-MEKF when linear acceleration measurements are available are then given by f 1 x 1 t;t an g 1 1 x 1 t;t, efine, respectively, as 1 ^ωb δq 1 δq ^ω B 1 δq ^b ω 1 δq b ω ^ω B ^b ω b ω b v c ^n B A I ^b n b n δq ^q gi ^q δq ^ω B ^b ω b ω ^ω B ^b ω b ω r B A B ; 1 e δq ;r e δq ; 1 e δq ;r I b v ^ω B ^b ω b ω r B A B ^ω B ^b ω b ω r B A B 0 0 ci I 0 1 By replacing δq ;r;0 an δq ;;0 through Eq. () in f 1 x 1 t;t an g 1 1 x 1 t;t an using Eq. (), F 1 1 t an G 1 1 t can be etermine to be ^ω B 0 1 I 0 0 ^v B ^ω B 0 1 F 1 1 t I ^q gi ^q 0 ^b v ^ω B r B A B ^ω B r B A B ^ω B ci (66) an G 1 1 t I I I 0 0 r B A B 0 0 ci I ^b v ^ω B r B A B ^ω B (6)

9 Article in Avance / 9 FILIPE, KONTITSIS, AND TSIOTRAS H 6 1 tk I (0) The optimal Kalman state upate is now calculate base on Eq. (8) from Δ δq^ tk 6 6 Δ x^1 tk 6 Δ b ^ω tk 4 Δ b^n tk K1 6 tk z6 tk z^6 tk K1 6 q^ tk q;m tk (1) Downloae by GEORGIA INST OF TECHNOLOGY on May 9, 01 DOI: 10.14/1.G0009 Finally, the estimate of the state at time tk after the measurement is calculate from Eqs. (48) an (49) an b^n tk b^n tk Δ b^n tk. IV. Fig. Five egree-of-freeom experimental platform of the ASTROS facility. 1. Time Upate When acceleration measurements are available, for the time upate ^ B are propagate of the DQ-MEKF, q^, b^ω, b^v, b^n, v^b, an ω using Eqs. (1), (60), (64), (), an (61), an the real part of Eq. () ^ B ωb;m b^ω ), respectively, given q^ t0, b^ω t0, (i.e., ω ^ an bn t0. Numerical errors in the propagation of q^ through Eq. (1) can result in the violation of the algebraic constraints specifie by Eq. (). Hence, after each integration step, these algebraic constraints are enforce by using Eq. (4). As for the covariance matrix of x1, P1 1 t T δq t δq t # < = b ω t 4 b^ω t A@4 b ω t 4 b^ω t A : ; b n t b n t b^n t b^n t (68) it is propagate accoring to Eq. (4) given P1 1 t0, an where Q ω t Q1 1 t 6 0 Q bω t 0 (69) Q n t Qbn t. Measurement Upate When acceleration measurements are available, the measurement upate is performe as in Sec. III.C with the measurement sensitivity matrix now given by Table 1 Experimental Results In this section, the two special cases of the DQ-MEKF are valiate experimentally on the Autonomous Spacecraft Testing of Robotic Operations in Space (ASTROS) facility at the School of Aerospace Engineering of the Georgia Institute of Technology. This experimental facility inclues a -DOF platform supporte on hemispherical an linear air bearings moving over a flat epoxy floor to simulate as best as possible the frictionless environment of space. The experimental facility also inclues a VICON motion capture system mounte on an aluminum gri above the experimental area. The VICON system measures the attitue an position of the platform with respect to a reference frame fixe to the room. These measurements are then transmitte wirelessly to the platform. A picture of the platform is shown in Fig.. More information about the ASTROS facility an its -DOF platform can be foun in [,6]. The most relevant characteristics of the sensors use in the experiments are summarize in Table 1, where SD stans for stanar eviation. The scaling constant of the IMU is c 9.8 m s an it is locate at rba B 0.11; 0.016; T m. The groun truth for attitue an position was obtaine from VICON measurements at 100 Hz. The groun truth for linear velocity was obtaine by passing these position measurements through a linear time-invariant (LTI) system with transfer matrix H s s s I. The position of the pole was chosen by trial an error to minimize noise an lag. Finally, the groun truth for the angular velocity was obtaine by passing the quaternion measurements through an LTI system with transfer matrix H s s s I4 4 an by using the relation ωb q q_. Note that, whereas the LTI filters can reuce the noise at the cost of lag, the groun truth for linear an angular velocity will still have some noise. A. DQ-MEKF with No Angular an Linear Velocity Measurements For this experiment, the DQ-MEKF was fe attitue an position measurements from the VICON system at 10 Hz, moele through the output equation given by Eq. (1). The initial estimate of the state is given in Table. The same table also shows an a posteriori guess of the initial state base on the measurements. The DQ-MEKF was initialize with the covariance matrices given in Table. The pose estimate by the DQ-MEKF is compare with the groun truth in Fig. 4. The two appear almost ientical. This is to be expecte ue to the relatively high upate rate of pose measurements in this case. Note that the motion only starts aroun 0 s after the beginning of the experiment. The pose estimation error obtaine with the DQ-MEKF is plotte in Fig.. Note that the pose error increases at aroun 0 s, when the Characteristics of the sensors Measurement Sensor Noise SD Humphrey RG0--1 rate gyro 0.0 eg s ω B;m Crossbow AHRS400CC-100 IMU 1. mg n BA I;m 8 VICON Bonita B10 cameras < 10 q;m I 8 VICON Bonita B10 cameras <1 mm r ;m Bias < eg s <8. mg Refresh rate 100 Hz 100 Hz Variable ( 0 Hz) Variable ( 0 Hz)

10 10 Article in Avance / FILIPE, KONTITSIS, AND TSIOTRAS Downloae by GEORGIA INST OF TECHNOLOGY on May 9, 01 DOI: 10.14/1.G0009 motion starts. The same figure also shows the pose estimation error obtaine with two alternative EKF formulations. This first alternative EKF formulation, the quaternion vector aitive EKF (QV-AEKF), is an aitive EKF, where the state contains the vector part of the unit error quaternion (like in the Q-MEKF) an the position vector of the boy with respect to the inertial frame expresse in the boy frame. The QV-AEKF is erive in etail in Appenix A. The biggest ifferences between the DQ-MEKF an the QV-AEKF are that, in the former, the position is represente by the ual part of the ual quaternion an the position measurement upate is performe using the ual quaternion multiplication, whereas, in the latter, the position is represente by the boy coorinates of the position vector, an the position measurement upate is performe by aing the optimal correction to the current best estimate. The secon alternative EKF formulation, the split quaternion vector aitive EKF (SQV-AEKF), is essentially the QV-AEKF split into two aitive EKFs, one for the attitue an another one for the position. The SQV-AEKF is erive in etail in Appenix B. For the comparison between the DQ-MEKF, the QV-AEKF, an the SQV-AEKF to be fair, the three filters were fe the same measurements, were initialize with the same initial estimate of the state (given in Table ), an were tune with the same covariance matrices (given in Table ). The linear an angular velocity estimation errors obtaine with the three filters are shown in Fig. 6. The rms attitue, position, angular velocity, an linear velocity estimation errors after 0 s, obtaine with the three filters, are given in Table 4. Note that the rms attitue an angular velocity estimation errors obtaine with the three filters are the same. This is not surprising because the DQ-MEKF, QV-AEKF, an SQV-AEKF represent an upate the attitue in the same way an the attitue is inepenent from the position. However, whereas the rms position an linear velocity estimation errors obtaine with the DQ-MEKF an the QV- AEKF are the same, the rms position an linear velocity estimation errors obtaine with the SQV-AEKF are higher. This is unerstanable because the DQ-MEKF an the QV-AEKF take into consieration the fact that the position vector of the boy with respect to the inertial frame expresse in the boy frame epens on the attitue of the boy, whereas the SQV-AEKF oes not. Another way to see this is to realize that some of the elements of Eqs. (A4) an (A8) o not appear in Eqs. (B), (B6), (B9), an (B1). To compare the filters in a more emaning scenario, the same experimental ata were fe into the DQ-MEKF, QV-AEKF, an SQV-AEKF, but now with an upate rate of 0. Hz. All other parameters were kept the same. The pose estimate by the DQ-MEKF is compare with the groun truth in Fig.. As expecte, the pose estimation error in this case is visibly higher than in Fig. 4. The attitue, position, angular velocity, an linear velocity estimation errors obtaine with the DQ-MEKF, QV-AEKF, an SQV- AEKF are compare in Figs. 8 an 9, an in Table. Like in Table 4, the rms attitue an angular velocity estimation errors obtaine with the three filters are the same, an the SQV-AEKF exhibits the highest rms position an linear velocity estimation errors. However, unlike in Table 4, the rms position an linear velocity estimation errors obtaine with the DQ-MEKF are smaller than the ones obtaine with the Table Case 1: initial estimate an a posteriori guess of the state Variable Initial estimate A posteriori guess q ; 0; 0; 0.01 T 0.98; 0.01; 0.019; T r I 0 0.; ; 1T,m 0.6;.04; T,m b ω 0 0; 0; 0 T, eg s 0; 0; 0 T, eg s b v 0 0; 0; 0 T, m s 0; 0; 0 T, m s Fig. 4 Fig. Case 1: estimate an true pose (pose measurements at 10 Hz). Case 1: pose estimation error (pose measurements at 10 Hz). QV-AEKF. In other wors, as the upate rate of the pose measurements ecreases, the DQ-MEKF starts proucing better position an linear velocity estimates than the QV-AEKF. This can be justifie in part by Fig. 1. Because the relation between r B an ri is quaratic in q, whereas the relation between q ; an r I is linear in q, the linearization error committe when linearizing the output equations of the QV-AEKF an of the DQ-MEKF [i.e., Eqs. (A) an (1), respectively] with respect to δq is smaller in the DQ-MEKF case. B. DQ-MEKF with Linear Acceleration Measurements For this experiment, the DQ-MEKF was fe attitue an position measurements from the VICON system at 1 Hz, linear acceleration measurements from the IMU at 100 Hz, an angular velocity measurements from the rate gyro at 100 Hz. The initial Table Case 1: covariance matrices Matrix P Q 1 1 R 6 6 Value iag ( ; ; ; ; ; ; ; ; ; ; ; ) iag (0; 0; 0; 0; 0; 0; 1 10 ; 1 10 ; 1 10 ; ; ; ) iag ( ; ; ; ; ; )