A Trident Quaternion Framework for Inertial-based Navigation Part I: Rigid Motion Representation and Computation
Abstract —Strapdown inertial navigation research involves the parameterization and computation of the attitude, velocity and position of a rigid body in a chosen reference frame. The community has long devoted to finding the most concise and efficient representation for the strapdown inertial navigation system (INS). The current work is motivated by simplifying the existing dual quaternion representation of the kinematic model. This paper proposes a compact and elegant representation of the body’s attitude/velocity/position, with the aid of a devised trident quaternion tool in which the position is accounted for by adding a second imaginary part to the dual quaternion. Eventually, the kinematics of strapdown INS are cohesively unified in one concise differential equation, which bears the same form as the classical attitude quaternion equation. In addition, the computation of this trident quaternion-based kinematic equation is implemented with the recently proposed functional iterative integration approach. Numerical results verify the analysis and show that incorporating the new representation into the functional iterative integration scheme achieves high inertial navigation computation accuracy as well.
Index Terms —Inertial navigation, Kinematic representation, Navigation computation, Trident quaternion, Functional iterative integration I. I NTRODUCTION HE transition from gimbaled inertial navigation to strapdown inertial navigation has spurred a wealth of frameworks and algorithms over the last half century to achieve an accurate solution of attitude, velocity and position of a rigid body [1]-[7]. In a strapdown inertial navigation system (INS), a triad of gyro outputs are used to propagate the attitude with respect to a reference frame, which is further exploited to calculate the velocity/position with a triad of accelerometer outputs [8]. Appropriate parameterization of the body attitude is vital to the INS mechanism and the subsequent navigation computation process. A number of attitude representations have been proposed for strapdown attitude computation [9]. Among them the Euler angle, the rotation vector, the direction cosine matrix (DCM), and the quaternion are routinely used in the navigation and control of land/underwater vehicles, airplanes, spacecraft and robots, etc [10], [11]. The fundamental object of attitude The paper was supported in part by National Key R&D Program of China (2018YFB1305103) and National Natural Science Foundation of China (61673263). A short version was submitted to International Conference on Integrated Navigation Systems, Saint Petersburg, Russia, 2021. computation is to solve the ordinary differential equations in those parameters [12], [13]. Specifically, the three-dimension rotation vector-based attitude parameterization is believed to be minimal and free from the intrinsic constraints of the quaternion and DCM. Despite the unit-norm constraint, the quaternion parameterization has been widely accepted in the navigation community owing to its benefits of non-singularity, succinctness and the linearity in its differential equation for efficient algebraic computation [14]-[17]. This work primarily considers the quaternion-related description of the strapdown INS. Since initially developed by Hamilton in 1843 [18], the quaternion has emerged as a powerful and efficient tool in representing the attitude kinematics and dynamics in many fields. Further efforts have been devoted to enriching the quaternion algebra family by developing new algebraic structures. The description of spatial rotation and translation of a rigid body was initially studied in the screw theory developed by Ball in 1900 [19]. This combined motion was further described by the dual quaternion, which is believed as the most elegant and compact mathematical description of rigid body motions [20]. Indeed, the dual quaternion is by definition the combination of the dual number and the quaternion [21]. In 1992, Branets and Shmyglevsky [22] firstly proposed to use dual quaternions to describe the attitude and translation simultaneously in the strapdown INS, where three continuous kinematic equations in dual quaternions were formulated to express the kinematics of strapdown INS. To the author’s knowledge, researches and applications of the dual quaternion are somewhat tepid in the navigation community until our group further investigated the dual quaternion formulation for the strapdown INS algorithm design in [23] and derived linearized error models in terms of dual quaternions in [24]. However, the inherent redundancies of three dual quaternion kinematic equations impeded their applications in strapdown INS computation and integrated navigation estimation. In recent twenty years, especially after our group’s work [23], booming studies have revived in applying dual quaternions to kinematics and dynamics of mechanics and robotics, manipulator control and state estimation [25]-[30]. For instance, Wang exploited the geometric structure of the dual quaternion in kinematic modeling and control law design for position
W. Ouyang and Y. Wu are with Shanghai Key Laboratory of Navigation and Location-based Services, School of Electronic Information and Electrical Engineering, Shanghai Jiao Tong University, Shanghai 200240, China (email: [email protected], [email protected]).
A Trident Quaternion Framework for Inertial-based Navigation Part I: Rigid Motion Representation and Computation
Wei Ouyang and Yuanxin Wu,
Senior Member , IEEE T M ATHEMATICAL P RELIMINARIES
This section provides a brief summary of the quaternion and dual quaternion. Their operations and properties in detail can be found in [23] and [29]. A. Quaternion
Quaternions are generalized complex numbers invented by Hamilton to represent rotations in the three-dimensional space. A quaternion is defined as [ , ] q s v or q s ix jy kz , where [ , , ] T x y z v is the vector part and , , , s x y z are real scalars. , , i j k are the generalized imaginary units, which satisfy i j k , ij ji k , ki ik j , and jk kj i . Note that in this work only the three-dimensional vectors are denoted in bold font. By definition, the addition and multiplication rules of two quaternions are given as [ , ][ , ] q q s sq q s s s s v vv v v v v v (1) where ‘ ’ is the vector cross product, and ‘ ’ is the quaternion product. The norm of a quaternion is computed as q q q . Therefore, the conjugate of a unit quaternion is given as [ , ] q q s v . Regarding the rotation of the frame O to the frame N about a unit vector n with an angle , the unit quaternion [cos 2 ,sin 2 ] ON q n transforms the vector coordinate in the original frame O to that in another frame N as N O OON ON NO NO r q r q q r q (2) where , N O r r are quaternions with zero scalar part, e.g., the vector quaternion [0, ]
N N r r and ON NO q q . The quaternion kinematic equation is defined as N OON ON ON ON q q q (3) 3where [0, ]
N NON ON ω and the vector part NON ω is the angular rate vector of the frame N w.r.t. the frame O , expressed in the frame N . B. Dual Quaternion
The dual quaternion is by definition the combination of dual number theory with quaternions. Dual quaternions can be regarded as quaternions with dual parts, i.e., [ , ] q s v , where s s s is the dual number and v v v is a dual vector [23]. The dual unit is defined by and 0 . Moreover, a dual quaternion can be explicitly reformulated as a dual number with quaternions as the real and dual part, respectively. q q q (4) where both q and q are usual quaternions. In this work, the dual quaternions with vanishing scalar part in each quaternion are named as dual vector quaternions, which are indiscriminate with usual dual quaternions. The addition and multiplication between dual quaternions are defined as q q q q q qq q q q q q q q (5) Similarly, the conjugate of a dual quaternion is denoted as q q q or [ , ] q s v . The norm of a dual quaternion is = q q q and its inverse is equivalent to its conjugate for a unit dual quaternion. Dual quaternion is usually used to represent the rotation and translation between two frames simultaneously. The relative motion of the frame N w.r.t. the frame O can be described by a rotation ON q followed by a translation N t or a translation O t followed by a rotation ON q . The unit dual quaternion defined to describe this relationship is O NON ON ON ON ON ON ON q q q q t q q q t (6) in which 2
O NON ON ON q t q q t and [0, ]
O O t t , [0, ] N N t t . Similar to the kinematic equation of quaternions, the kinematic equation of dual quaternion is formulated as N OON ON ON ON ON q q q (7) where the dual vector quaternions
NON and OON , also called twists, are further denoted as [23] N N N N NON ON ONO O O O OON ON ON t tt t (8) where vectors are automatically transformed to the corresponding vector quaternions in computations with quaternions. III. D UAL Q UATERNION AND V ECTOR H YBRID R EPRESENTATION
The dual quaternion formulation of inertial navigation was presented in [22]-[24], where three differential equations of dual quaternion were formulated to represent the principle of inertial navigation. However, there are considerable redundancies in these equations as it uses twenty-four states to encode the rigid motion with nine degrees of freedom (three for each of attitude, velocity and position). Next we will show that the redundancies could be significantly reduced by a novel formulation of dual quaternion and vector hybrid model. Here, we start with similar thought of [23] and devote to simplifying the previous dual quaternion representation of strapdown INS kinematic equations. Consider the inertial frame ( i -frame), the WGS-84 Earth frame ( e -frame) and the IMU body frame ( b -frame). e r denotes the position vector of the IMU relative to the earth. The position coordinates in i -frame and e -frame are related by i i ee r C r (9) Note that for normal navigation applications near the earth, the earth revolution is typically not considered [9], except for long-distance/long-time navigation scenarios. The superscripts denote the frames where to express the vector and ie C is the attitude matrix of i -frame with respect to e -frame. The attitude kinematic equation is given by i i ee e ie C C ω (10) where = 0, 0, Te eie ie ω is the constant earth rotation angular velocity in e -frame and eie ω is the antisymmetric matrix formed by this vector. Taking the time derivative of Eq. (9) and using Eq. (10), i i e e i ee ie e r C ω r C r (11) Take a further time derivative of Eq. (11) and define e e v r i i i i e e i e e i ee ie e ie e f g r C ω r C ω v C v (12) where i f denotes the specific force by the applied non-gravitational forces and i g is the gravitational acceleration. From Eq. (12), the vector ti i i i d r r f g (13) where i r is the initial velocity of the IMU in i -frame at time zero. As the specific force is measured in b -frame onboard the vehicle and the gravitational acceleration is usually provided in e -frame or some local-level frames, Equation (12) can be rewritten as the traditional velocity equation in e -frame by multiplying ei C on both sides, 2 e e b e e eb ie l v C f ω v g (14) where the term e e e el ie g g ω r is collectively known as the local gravity vector, and eb C is the attitude matrix of e -frame with respect to b -frame and satisfies the kinematics as e e b e b e eb b eb b ib ie b C C ω C ω ω C (15) 4where bib ω is the body angular velocity measured by gyroscopes. Equations (14)-(15), together with the trivial definition e e r v , constitute the routine navigation equations in e -frame [9]. In specific, they respectively characterize the translation and rotation of the vehicle. The attitude matrix in (15) could be alternatively expressed in attitude quaternion. ( ) b i r ( ) a frame v frame i e ii C r frame v frame e Figure 1: The relationship of the newly defined “total velocity” v -frame with i -frame and e -frame. (a) the v -frame is displaced from the i -frame with the vector i r ; (b) the v -frame is displaced from the e -frame with the vector e ii C r Notably, the dual quaternion provides a unified and efficient way to encode both rotational and translational motion between two frames. Its property is very much like that of quaternion for attitude. In order to simplify the dual quaternion representations in [22], [23], first define a new total velocity frame ( v -frame) which aligns with b- frame in orientation but displaces relative to i -frame by the velocity vector i r , as illustrated in Fig. 1(a). Since e -frame is related with i -frame with the rotation matrix ei C , the translation between e -frame and v -frame becomes e ii C r , as shown in Fig. 1(b). Intuitively, the abstract concept of “translation” can be perceived as the relative displacement between two frames, which is happened to be the velocity vectors here. Note that in Eqs. (9)-(15), i r is the displaced vector of the IMU relative to i -frame and e ii C r expresses this vector in e -frame. It relates to the usual e -frame velocity by e ii e e eie C r ω r v , according to Eq. (11). In this regard, the “translation” here is a local and abstract concept between two considered frames [23]. According to Eq. (6), the dual quaternion used to describe the relationship between v -frame and e -frame is given as e iev ev ev eb i eb q q q q q C r (16) Note that the vector e ii
C r in Eq. (16) is automatically used as a vector quaternion when multiplying with another quaternion. Taking the time derivative of Eq. (16), the kinematic equation of the dual quaternion is given by (see detailed derivations in Appendix A) b eev ev ib ie ev q q q (17) The above two dual vector quaternions (also called as twists) are given as be e eb bib ibie ie fg (18) where bib is exactly the twist for the total velocity frame, and eie is exactly the twist for the gravitational velocity frame in [23]. As a matter of fact, Eq. (17) can be alternatively obtained from combining the two previously derived dual quaternions equations in [23] yet by a minor adjustment of frame definitions (see Appendix B for details). It is a nice property that bib or eie can be readily built from direct measurements (the gyro outputs bib ω and the accelerometer outputs b f ) or the earth model (the earth rotation angular velocity eie ω and the gravitational acceleration e g ). From Eq. (11), the IMU position relative to e -frame can be represented by the dual quaternion ev q as * e e i e e e ei ie ev ev ie q q r C r ω r ω r (19) To summarize, the inertial navigation equation has been so far represented by a dual quaternion and vector (DQv) hybrid model, as given in Eqs. (17) and (19). Specifically, the former encodes the attitude and the velocity by way of a dual quaternion, and the latter directly uses the common position vector. Compared against the original dual quaternion formulation in [22], [23], the state dimension has been reduced from twenty four to eleven now. The DQv’s information flow is plotted in Fig. 2 and compared with that of the conventional e -frame formulation [9]. The attitude and velocity (six degrees of freedom) are now entangled together by the eight-dimensional dual quaternion. This section paves the way for more efficient and elegant representation of kinematics in Section IV. ( ) b bib ω ( ) a b f eb C e v e g bib ω b f ev q e g Figure 2: Information flow in (a) typical Earth-frame mechanization and in (b) the derived dual quaternion & vector model. Arrowed lines indicate information flow directions and the associated symbols mean that their computation needs to feed on the source information.
IV. T RIDENT Q UATERNION R EPRESENTATION
The former section has simplified the strapdown INS kinematic representation from three dual quaternion kinematic equations to one dual quaternion kinematic equation and one vector equation. However, it can be observed that the position vector equation cannot be absorbed into the dual quaternion equation. This is mainly caused by the fact that only one “translation” is defined in the dual quaternion theory, while the spatial motion of a rigid body involves relative velocity “translation” and relative position “translation” w.r.t. a reference frame. To address this problem, a new algebraic structure is developed here to achieve more succinct 5representation of the strapdown INS kinematics. A. Trident Quaternion and Its Properties Definition 1 . The trident dual numbers are defined as a a a a , where , , a a a , , and . Theoretically, the trident dual numbers are direct extension of the dual number by juxtaposing a second imaginary part. The two imaginary parts are equivalent in algebraic operations. Similarly, we can define the arithmetic operations of trident numbers like the dual numbers as follows a b a b a b a ba a a aab a b a b a b a b a b (20) where is a real scalar. Alternatively, trident vectors can be accordingly defined as trident numbers, except that their real part and imaginary parts are usual vectors. A trident quaternion is devised as follows by combining quaternions with trident numbers. Definition 2 . The trident quaternion is defined as q q q q , where , , q q q are usual quaternions and the imaginary units satisfy , and . For any two trident quaternions q q q q and q q q q , the arithmetic operations naturally follow as q q q q q q q qq q q qq q q q q q q q q q q q (21) As for the investigated inertial navigation system, in which the total velocity frame is defined relative to the e -frame, the trident quaternion is defined as eb eb eb eb q q q q (22) in which, we directly replace v with b in contrast with Eq. (16). Similar to the derivations in Section III, the second imaginary part extended here is used to encompass the position translation of the total velocity frame w.r.t. the e -frame. Hence, the trident quaternion is finally given as e i e ieb eb i eb i eb q q q q C r C r (23) Taking the derivative of Eq. (23) and using (50), the kinematic equation is obtained as be i bi eb ebbeb eb e e ieb i ebe i e ii eb iibeb eie eb eib ie bi bi ee q q fq g q qq qqq C r C rC rC r C r (24) which can be further formulated in a beautiful and compact form b eieeb eb ib eb q q q (25) Referring to the representations in Section III, two trident twists could be accordingly defined as
11 2 beb bib ibie ie e e iie fg C r (26) See Appendix C for further discussions about other feasible expressions of the two trident twists. Based on Eqs. (25)-(26), it can be seen that the whole strapdown INS kinematics are encompassed in one elegant trident quaternion differential equation, which takes the same form as the traditional attitude kinematic equation in Eq. (49)and the dual quaternion kinematic equation in Eq. (17). Table I summarizes the aforementioned representations for easy reference.
TABLE
I S
UMMARY OF K INEMATIC R EPRESENTATIONS OF S TRAPDOWN
INS
Methods Representations
Traditional representation e e b e eb b ib ie b e e b e e eieb le e C C ω ω C v C f ω v gr v Dual quaternion representation (DQ) ˆˆ ˆ ˆˆ22 ˆˆ2 ˆˆ
TIT IT ITGIG IG IGUIU IU IU q qq qq q
Dual quaternion and vector hybrid representation (DQv) * e e eev ev ieb eev ev ib ie ev q q qq q r ω r Trident quaternion representation (TriQ) b eieeb eb ib eb q q q In Eq. (23), we actually applied the trident quaternion to represent the spatial motion of a rigid body, including the attitude, velocity and position relative to a reference frame, which is also termed as the extended pose on SE in [41]. The intrinsic reason for this correspondence is that the set of trident quaternions is innately a Lie group as proved in the following theorem. Theorem 1:
A set of unit trident quaternions, denoted as
TriQ , is a Lie group under the trident quaternion multiplication.
Proof:
According to the properties of a group, we have: (1) The closure is obvious in that for any , q q TriQ , we have the third equation in Eq. (21) and q q , q q q q q q q q are usual quaternions. Therefore, the closure is ensured by the multiplication T iQq rq . (2) The associate of any , , q q q TriQ can be easily verified as = q q q q q q . (3) The identity element is by definition. (4) The inverse is of a unit trident quaternion is q q . Properties (1)-(4) indicate that the set of trident quaternions is a group. It is also known that any unit quaternion q is a differential manifold of three dimensions [26]. Thus, any q TriQ is a manifold of nine dimensions since its real and imaginary parts are usual quaternions. Besides, for any , r Qq iq T ,
11 2 1 2 , q q q q is a smooth (differentiable) mapping in terms of multiplications. As a result, a set of trident quaternions form a Lie group under the trident quaternion multiplication. Proof ends here. Note that an alternative representation of the trident quaternion can be given as b i b ieb eb eb i eb i q q q q C r C r (27)which can be obtained from Eq. (23) by the following relationship e i e i b ii eb eb eb i eb eb i q q q q q C r C r C r (28) Moreover, the trident quaternion can be reversely defined as the e -frame relative to the b -frame, which is actually the robocentric formulations as in [41], [42]. We have b i b ibe i bebe i be qq qq C r C r (29) Or e i e ibe be i beb ie q qq q
C r C r (30) The differential equations of Eqs. (29) and (30) can be easily computed as in Eq. (24). Of course, the trident quaternions (29) and (30) are just conjugate counterparts of (23) and (27), with opposite signs in the imaginary parts. V. F UNCTIONAL I TERATIVE I NTEGRATION OF T RIDENT Q UATERNION K INEMATICS
This section devotes to solve the trident quaternion differential equation by the functional iterative integration framework proposed in [44], named as the tqFIter algorithm hereafter. The trident quaternion kinematic equation based on the first specific solution in Eq. (63) is repeatedly given here, omitting subscripts of the trident quaternion for symbolic brevity in the subsequent development. b eieib q q q (31) where q q q q . Notice that the velocity and position can be readily recovered from Eqs. (23) and (11) ee e eie q qq q rv ω r (32) According to the functional iterative iteration approach, integrating the differential equation in Eq. (31) over the time interval [0, ] t gives t b eib ie q q q q dt (33) According to Eqs. (23) and (32), the initial value of the trident quaternion is given as e e e eiet t q q q q q v ω r r (34) Without loss of generality, we will consider the navigation updates over the time interval [0, ] t , in which N samples of triads of gyroscopes and accelerometers are available. At time instants k t (
1, 2, k N ), assume the discrete angular velocity k t ω or the angular increment (integrated angular velocity) k t θ measurements by a triad of gyroscopes, and the discrete specific force k t f or velocity increments (integrated specific force) k t v measurements by a triad of accelerometers are available. In order to apply the Chebyshev polynomials, the actual time interval is mapped onto [ 1, 1] by letting N t t . Assume the trident quaternion estimate at the l -th iteration is expressed by a weighted sum of Chebyshev polynomials, say ,0 q ml l i ii q b F (35) where q m denotes the maximum degree of Chebyshev polynomials and , l i b is the coefficient of i th -degree Chebyshev polynomial at the l -th iteration. i F x is the i th -degree Chebyshev polynomial of the first kind. For any , 0 j k , it satisfies the equality [49] j k j k j k F F F F (36) According to the integral property of the Chebyshev polynomial, we have , 1 1 1 1 12 22 2 1 , 11 1 1 1, 12 kk k k ii i k k i k i k k i kk k
G F diF F iF F ii i i i i (37) In specific, the integrated i th -degree Chebyshev polynomial can be expressed as a linear combination of ( i +1) th -degree Chebyshev polynomials, given by , 1 11 1 02 21 11 1 02 211 022 1 0 ii i i i ii ii i iiii i G F diF F iF F Fi i i iF FiF iF F Fi i i iFF F ii i iF F i (38) The fitted trident twists (angular velocity and specific force) using the Chebyshev polynomials can be respectively written as [44] , 1 b nbib i i bi c F n N (39) and, , ,0 , 1 e neie l l i i ei d F n N (40) where b n and e n denote the maximum degrees of Chebyshev polynomials. The coefficient i c are determined by solving the least-square equations of the discrete gyroscope/accelerometer measurements and the coefficient. As the terms e g nonlinearly depends on the trident quaternion, , 1 , 2 l i l l i l d d d d should be approximated by a process given by [49], namely,
10, 0* eie P eil i lkl lll d i kd qP Pd q q g (41) Using Eq. (35), the third equality in the above equation can be expressed as *, ,0 0*, , = 2 q qq q m ml l j j l k kj km m l j l k j k j kj k d b F b Fb b F F (42) The integral in Eq. (33) is transformed to that over the mapped interval of Chebyshev polynomials, that is, b eNl l ib ie l l tq q q q d (43) Substitute Eq. (35)-(40) to (43), the numerical solution of the trident quaternion is obtained as b eNl l ib ie l lm nl i i j ji jN mn l j j l i ij im n l i j i ji jN l j l i j q e qeq e tq q q q db F c Ftq dd F b Fb c F F dtq d b F
10 0 , , 1 , 10 0 , , , 1 , 10 0 max , 1 polynomial truncation1, 1,0 0 mn ij im n l i j i j i ji jN mn l j l i i j i jj i n n m ml i i l i ii i qeq e qeb e q
F db c G Gtq d b G Gb F b F q (44) In contrast with [44], where the attitude computation precedes the velocity/position computation, the current computation involves only one iterative computation process, thanks to the compact trident quaternion representation. VI. N UMERICAL R ESULTS
Numerical tests are conducted to verify the representation development and the tqFIter algorithm. Note that the simulation data were generated analytically in the local-level frame and then transformed to the earth frame [48]. The vehicle flies around the surface at the ground velocity
0, 0, sin( ) Tn a wt v with an initial east velocity v , in which the magnitude of acceleration a , and the angular frequency of the varying acceleration w . The vehicle is initially located at zero longitude, zero latitude and zero height. In addition, the body frame performs a classical coning motion with the attitude quaternion cos( / 2) sin( / 2)[0 cos( ) sin( )] Tnb q t t (45) which denotes the attitude of the body frame w.r.t. the local-level navigation frame. The coning angle
10 deg and the coning frequency . The true velocity and position are obtained as
00 200 (0) 0 0 ( cos( ) )( sin( ) ) / 0 0 t Tn n nt Tn nc E dt v a wt a wdt v t a wt awt w R v v vp R v (46) where c R is a function of the current position, the transverse radius of curvature E R and the meridian radius of curvature N R . 8 Ec N
R h LR h R (47) Finally, the gyroscope and accelerometer outputs at 100Hz sampling rate are generated according to b nib nb nb nbb b n n n n nn i nine e q q q f C v ω ω v g (48) In this numerical test, N = 8 samples of gyroscopes and accelerometers are used to fit the angular velocity and specific force. The orders of Chebyshev polynomials involved in the fitting are set as b e n n N . Besides, the orders of truncation in approximating the attitude quaternion, the first imaginary part and the second imaginary part are all set to q m N . The maximum iteration number in solving Eq. (44) is set as N . Then, the root-mean-square discrepancy of polynomial coefficients between two iterations is adopted as the termination criterion, which is set to . Figures 3-5 show the computation errors of the attitude angle, velocity, and position in 200 seconds. Comparing with the routine two-sample algorithm [5], [6], the tqFIter algorithm (dotted lines) significantly outperforms the traditional method (dashed lines) under the considered scenario. In specific, the attitude/velocity/position computation accuracy is about 8 orders higher than the two-sample algorithm. Note that this accuracy is comparable to the iNavFIter algorithm based on the traditional representation of strapdown INS, albeit the iNavFIter is somewhat more accurate in position computation as shown in Fig. 5. These results show the feasibility and effectiveness of applying the functional iterative integration framework to numerically solve the newly developed trident quaternion kinematics. It should be useful for not only pure inertial navigation but kinematics forward propagation in inertial-based navigation with aiding sensors. Figure 3: Principal angle errors. Dashed line denotes the results of two-sample algorithm, and the dotted line denotes those of the tqFIter algorithm. The solid line denotes the results of the iNavFIter.
Figure 4: Velocity errors in local-level frame (north, up, east). Dashed line denotes the results of two-sample algorithm, and the dotted lines denotes those of the tqFIter algorithm. Solid lines denote the results of the iNavFIter.
Figure 5: Position errors in local-level frame (north, up, east). Dashed line denotes the results of two-sample algorithm, and the dotted lines denotes those of the tqFIter algorithm. Solid lines denote the results of the iNavFIter.
VII. C ONCLUSIONS
In this article, the kinematic representation of the strapdown INS is investigated. In view of the redundancy existing in the dual quaternion representation, a new total velocity frame is defined to simplify it. The strapdown INS kinematics are subsequently described by a dual quaternion with a position vector. In order to achieve the most concise representation, trident numbers and trident quaternions are devised, which successfully incorporate the position vector into the algebraic representation. Interestingly, the kinematics of the whole strapdown inertial navigation are elegantly described by one unique trident quaternion differential equation. Moreover, we numerically solve the trident quaternion kinematic equation by using the functional iterative integration approach. The proposed trident quaternion model not only provides the most compact representation of strapdown INS, but contributes to the state estimation of inertial-based navigation with aiding sensors, which is further covered in the companion paper. 9A
PPENDIX A The derivation of Eq. (17) is provided here. The derivative of right-side scalar part in Eq. (16) is eb eb eb b eeb ib i beb e q q q q (49) Then, the derivative of the imaginary part of Eq. (16) is e i e i e ii i eb i ebe i e i ii eb i ebe ii eb ebe i e i e bi i eb b ebe e ieb ieie b eib iee eie ie b eib iebib eb ebe i b ei e ib eb eb q qq qq q q qg q qq q qf qg C r C C rC r C frω gC r C r C r C fC rC r e ii ebee q C r (50) In the second step of Eq. (50), the following relationship between two vector quaternions [0, ] v v and [0, ] v v is used. v v v v v v v v (51) Based on Eq. (49), (50), the result is obtained in Eq. (17). A PPENDIX
B This appendix provides another perspective to derive Eq. (16) from [23] yet by a minor adjustment to the original frame definitions in contrast with the current work. Note that the symbols may be slightly different from the original ones. In [23], the thrust velocity is defined as the sum of the initial velocity of the vehicle and integration of the specific force i s , which is denoted in the inertial frame. ti i it d v r s (52) Further define the gravitational velocity as minus integration of the gravitational acceleration, ti ig d v g (53) Then, the velocity of the IMU in i -frame is expressed as ti i i i i it g dt r r s g v v (54) Firstly, the thrust velocity frame ( t -frame) is defined to align with b- frame in orientation but displaced relative to i -frame by the thrust velocity it v , and therefore it ib q q . Suppose a unit dual quaternion 12 iit it it it t it q q q q v q characterizes the general displacement of t -frame relative to i -frame, where [0, ] i it t v v . According to [23], the twist expressed in t -frame is given by t b bit ib f (55) where the specific force actually satisfies b iib ib q q f s and [0, ] b b f f . The kinematic equation of it q is given as tit it it q q (56) Secondly, the gravitational velocity frame ( g -frame) is defined to align with e- frame in orientation but displaces relative to i -frame by the gravitational velocity. Let the general displacement of g -frame relative to e -frame be represented by the other unit dual quaternion iig ig ig ig g ig q q q q v q , where [0, ] i ig g v v . The twist expressed in g- frame is g e eig ie g (57) The kinematic equation of ig q is gig ig ig q q (58) Let gt ig it q q q characterize the relative motion of t -frame with respect to g -frame. With Eq. (54), then it can be shown that * * ** ** gt ig it ig it ig iti igt ig t it ig g iti igt ig t g ite igt i gt q q q q q q qq q v q q v qq q v v qq q C r (59) which is exactly equal to Eq. (16) because g -frame and t -frame share the same orientations with e -frame and b -frame, respectively. That is to say, gt eb q q . Finally, the kinematics of gt q satisfy * * ** * * gt ig it ig ig ig itt gig it it ig ig ig ig itt ggt it ig gt q q q q q q qq q q q q qq q (60) which is just the Eq. (17) in the current work. A PPENDIX
C Actually, the expressions of two trident twists can be assumed as the following general format bb bib ibie ie ee e f xg x (61) where , x x are vector quaternions. Substitute Eq. (61) into Eq. (25) and compare with Eq. (24), the following condition should be satisfied eb eb e ii eb q x x qq C r (62) Obviously, two special solutions of this equation are [0, ], [0, ] e ii x x (63) and, [0, ], [0, ] b ii x x C r 0 (64) where is the three-dimensional vector. This work adopts the first solution to represent the kinematic model and more discussions about two specific solutions are to be made in the companion work. 10R EFERENCES [1]
D. A. Tazartes, Inertial navigation: From gimbaled platforms to strapdown sensors.
IEEE Trans. Aerosp. Electron. Syst. , vol. 47, no. 3, pp. 2292–2299, 2010. [2]
J. W. Jordan, An accurate strapdown direction cosine algorithm NASA TN-D-5384, 1969. [3]
J. E. Bortz, A new mathematical formulation for strapdown inertial navigation.
IEEE Trans. Aerosp. Electron. Syst. , vol. 7, no. 1, pp. 61–66, Jan. 1971. [4]
M. B. Ignagni, Optimal strapdown attitude integration algorithms.
J. Guid., Control, Dyn. , vol. 13, pp. 363–369, 1990. [5]
P. G. Savage, Strapdown inertial navigation integration algorithm design, Part 1: Attitude algorithms.
J. Guid., Control, Dyn. , vol. 21, pp. 19–28, 1998. [6]
P. G. Savage, Strapdown inertial navigation integration algorithm design, Part 2: Velocity and position algorithms.
J. Guid., Control, Dyn. , vol. 21, pp. 208–221, 1998. [7]
P. G. Savage, A unified mathematical framework for strapdown algorithm design.
J. Guid. Control Dyn. , vol. 29, pp. 237–249, 2006. [8]
D. H. Titterton, J. L. Weston,
Strapdown Inertial Navigation Technology , 2nd ed.: the Institute of Electrical Engineers, London, United Kingdom, 2007. [9]
P. D. Groves,
Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems , 2nd ed.: Artech House, Boston and London, 2013. [10]
M. D. Shuster, A survey of attitude representations.
Journal of the Astronautical Sciences , vol. 41, no. 4, pp. 439-517, 1993. [11]
F. L. Markley, J. L. Crassidis.
Fundamentals of Spacecraft Attitude Determination and Control . New York, USA, Springer, 2014. [12]
Y. Wu, RodFIter: Attitude Reconstruction from Inertial Measurement by Functional Iteration,
IEEE Trans. on Aerospace and Electronic Systems , vol. 54, pp. 2131-2142, 2018. [13]
Y. Wu, Q. Cai, T.-K. Truong, Fast RodFIter for Attitude Reconstruction from Inertial Measurement,
IEEE Trans. on Aerospace and Electronic Systems, vol. 55, pp. 419-428, 2019. [14]
E J. Leffens, F. L. Markley, M. D. Shuster, Kalman filtering for spacecraft attitude estimation,
J. Guid. Control Dyn. , vol. 5, no. 5, pp. 417-429, 1982. [15]
G. Yan, J. Weng, X. Yang, and Y. Qin, An accurate numerical solution for strapdown attitude algorithm based on Picard iteration J. Astronaut., vol. 38, pp. 65–71, 2017. [16]
J. Funda, R. H. Taylor, R. P. Paul, On homogeneous transforms, quaternions, and computational efficiency.
IEEE Transactions on Robotics and Automation , vol. 6, no. 3, pp. 382-388, 1990. [17]
Y. Wu and G. Yan, Attitude Reconstruction From Inertial Measurements: QuatFIter and Its Comparison with RodFIter,
IEEE Transactions on Aerospace and Electronic Systems , vol. 55, no. 6, pp. 3629-3639, 2019. [18]
W. R. Hamilton,
Elements of Quaternions . Cambridge: Cambridge University Press, 2010. [19]
R. S. Ball, A Treatise on the Theory of Screws. New York: Cambridge University Press, 1900. [20]
N. A., Aspragathos, and J. K. Dimitros, A comparative study of three methods for robot kinematics.
IEEE Transactions on Systems, Man and Cybernetics Part B: Cybernetics , vol. 28, no. 2, pp. 135-145, Apr. 1998. [21]
F. Thomas, Approaching dual quaternions from matrix algebra.
IEEE Transactions on Robotics , vol. 30, no. 5, 1037-1048, 2014. [22]
V. N. Branets, and I. P. Shmyglevsky, Introduction to the Theory of Strapdown Inertial Navigation System (In Russian). Moscow: Nauka, ch. 1-6, 1992. [23]
Y. Wu, X. Hu, D. Hu, T. Li, J. Lian, Strapdown inertial navigation system algorithms based on dual quaternions, IEEE Transactions on Aerospace and Electronic Systems, vol. 41, pp. 110-132, 2005. [24]
Y. Wu, M. Wu, D. Hu, X. Hu, Strapdown inertial navigation system using dual quaternions: error analyis, IEEE Transactions on Aerospace and Electronic Systems, vol. 42, pp. 259-266, 2006. [25]
P. Tsiotras, A. Valverde, Dual Quaternions as a Tool for Modeling, Control, and Estimation for Spacecraft Robotic Servicing Missions.
The Journal of the Astronautical Sciences , vol. 67, no. 2, pp. 595-629, 2020. [26]
X. Wang, D. Han, C. Yu, Z. Zheng, The geometric structure of unit dual quaternion with application in kinematic control,
Journal of Mathematical Analysis and Applications , vol. 389, no. 2, pp. 1352-1364, 2012. [27]
X. Wang, C. Yu, Z. Lin, A dual quaternion solution to attitude and position control for rigid-body coordination.
IEEE Transactions on Robotics , vol. 28, no. 5, pp. 1162-1170, 2012. [28]
D. Han, Q. Wei, Z. Li, W. Sun, Control of oriented mechanical systems: A method based on dual quaternion.
IFAC Proceedings , vol. 41, no. 2, pp. 3836-3841, 2008. [29] N. Filipe, A. Valverde, P. Tsiotras. Pose tracking without linearand angular-velocity feedback using dual quaternions.
IEEE Transactions on Aerospace and Electronic Systems , vol. 52, no. 1, pp. 411-422, 2016. [30]
N. Filipe, M. Kontitsis, P. Tsiotras. Extended Kalman filter for spacecraft pose estimation using dual quaternions.
Journal of Guidance, Control, and Dynamics , vol. 38, no. 9, pp. 1625-1641, 2015. [31]
K. Li, F. Pfaff, U. D. Hanebeck, Unscented Dual Quaternion Particle Filter for SE (3) Estimation.
IEEE Control Systems Letters , vol. 5, no. 2, pp. 647-652, 2020. [32]
R. A. Srivatsan, G. T. Rosen, D. F. N. Mohamed, H. Choset. Estimating SE (3) elements using a dual quaternion based linear Kalman filter.
Robotics: Science and systems , 2016. [33]
S. Bultmann, K. Li, U. D. Hanebeck, Stereo visual slam based on unscented dual quaternion filtering. pp. 1-8, July, 2019. [34]
A. Sveier and O. Egeland, Dual Quaternion Particle Filtering for Pose Estimation,
IEEE Transactions on Control Systems Technology , early access, doi: 10.1109/TCST.2020.3026926. [35]
J. A. Fike, J.J. Alonso, The development of hyper-dual numbers for exact second-derivative calculations. . AIAA, July, 2011-886. [36]
J. A. Fike, J.J. Alonso, Automatic differentiation through the use of hyper-dual numbers for second derivatives.
Recent Advances in Algorithmic Differentiation . Springer, Berlin, Heidelberg, pp. 163-173, 2012. [37]
A. Cohen, M. Shoham, Application of hyper-dual numbers to rigid bodies equations of motion.
Mechanism and Machine Theory , vol. 111, pp. 76-84, 2017. [38]
A. Cohen, M. Shoham. Hyper Dual Quaternions representation of rigid bodies kinematics.
Mechanism and Machine Theory , vol. 150, no. 103861, 2020. [39]
F. Messelmi, Multidual numbers and their multidual functions.
Electronic Journal of Mathematical Analysis and Applications , vol. 3, no. 2, pp. 154-172, 2015. [40]
D. Condurache, Multidual Algebra and Higher-Order Kinematics.
European Conference on Mechanism Science , Springer, Cham. pp. 48-55, Sept, 2020. [41]
R. Hartley, M. Ghaffari, R. M. Eustice, J.W. Grizzle., Contact-aided invariant extended Kalman filtering for robot state estimation.
The International Journal of Robotics Research . vol. 39, no. 4, pp. 402-430, 2020. [42]
M. Bloesch, M. Burri, S. Omari, M. Hutter, R. Siegwart. Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback.
The International Journal of Robotics Research . Vol. 36, no. 10, pp. 1053-1072, 2017. [43]
G. Huang, Visual-Inertial Navigation: A Concise Review, , Montreal, QC, Canada, 2019, pp. 9572-9582 [44]
Y. Wu, iNavFIter: Next-Generation Inertial Navigation Computation Based on Functional Iteration,
IEEE Trans. on Aerospace and Electronic Systems , vol. 56, pp. 2061-2082, 2020. [45]
M. B. Ignagni, Enhanced Strapdown Attitude Computation,
Journal of Guidance, Control, and Dynamics , vol. 43, no. 6, pp. 1220-1224, 2020. [46]
M. B. Ignagni, Accuracy Limits on Strapdown Velocity and Position Computations,
Journal of Guidance, Control, and Dynamics , 1-5, 2020, doi.org/10.2514/1.G005538. [47]
P. G. Savage and , M. B. Ignagni, Honeywell Laser Inertial Navigation System (LINS) Test Results,