A Trident Quaternion Framework for Inertial-based Navigation Part II: Error Models and Application to Initial Alignment
Abstract —This work deals with error models for trident quaternion framework proposed in the companion paper “A Trident Quaternion Framework for Inertial-based Navigation Part I: Motion Representation and Computation” and further uses them to investigate the static and in-motion alignment for land vehicles. Specifically, the zero-velocity and odometer velocity measurements are applied in the static and in-motion alignment process, respectively. By linearizing the trident quaternion kinematic equation, the right and left trident quaternion error models are obtained. The resultant models are found to be equivalent to those derived from profound group affine. Then the two models are used to design the corresponding extended Kalman filters (EKF), namely, the left-quaternion EKF (LQEKF) and the right-quaternion EKF (RQEKF). Simulations and field tests are conducted to evaluate their actual performances. For the static alignment, owing to their high consistency, the L/RQEKF converge much faster than the EKF even without any heading information. For the in-motion alignment, however, the two filters still need the assistance of the analytical/optimization-based in-motion alignment methods at the very start to avoid extremely large attitude errors, although they possess much larger convergence region than the traditional EKF does.
Index Terms —Initial alignment, Odometer, Extended Kalman filtering, Trident quaternion, Linearized kinematic models I. I NTRODUCTION H E coarse alignment is indispensable for a strapdown inertial navigation system (INS), in order to drive attitude errors to within several degrees [1], [2]. The main reason of this requirement resides in assuredly making valid the first-order linearization assumption of the extended Kalman filter (EKF) [3]. The initial alignment can be classified into the static alignment and the in-motion alignment, and the former particularly requires high-accuracy gyroscopes to sense the earth rotation for successful self-gyrocompassing [4]. In general, the performance of the subsequent fine alignment stage is largely determined by the rapidness and accuracy of the coarse alignment process. Methods of attacking the initial alignment can be roughly classified into the vector observation-based and the Kalman filtering-based. As for the methods using finite vector observations, the initial attitude is directly determined with at least two non-collinear vectors, also known as the three-axis The paper was supported in part by National Key R&D Program of China (2018YFB1305103) and National Natural Science Foundation of China (61673263). attitude determination method (TRIAD) [1], [5], [6]. Moreover, the initial attitude can also be obtained with the optimization-based approach proposed in [8], [9], which are relied on a number of vector observations. Both the analytical alignment methods and optimization-based methods are popular in static and in-motion alignment applications [10]-[15]. However, the optimal transition time between the coarse and fine alignment is non-trivial to be predetermined and the attitude covariance information is generally unavailable. In contrast, the Kalman filtering-based approaches can estimate the attitude along with the attitude error covariance simultaneously. Nevertheless, the standard EKF necessitates critical attitude initialization to obviate the divergence since the linearization of the kinematic model is invalid under large attitude errors. To avoid this problem, researchers turned to use nonlinear angle-error models [16]-[18] or nonlinear filters, such as the unscented Kalman filter [19] to account for large attitude errors. Unfortunately, these methods are mostly less effective than reported in practical implementation, regardless of the complicated computation. Recently, an interesting route to deal with this problem is to circumscribe or even eliminate the inconsistency caused by inappropriate linearization of the original nonlinear kinematic model in the EKF. To achieve this goal, several effective methods have been proposed, of which the most influential approach is the invariant EKF (IEKF) in [20], [21]. It describes the system states on Lie groups and new error definitions were accordingly obtained, making the resultant linearized error model invariant as long as the “group affine” condition is satisfied by the original system. Here, the intrinsic “invariant” characteristics of a system are essentially epitomized in the Jacobian matrices after the linearization, which are time-invariant over the linearizing interval. The IEKF has been successfully used in improving the EKF consistency of EKF-based strapdown INS [22], visual-inertial navigation [23]-[25], legged robot state estimation [26], to name but only a few. As shown in [2], the IEKF can converge under large heading misalignments (e.g. 120 deg) in the static alignment, however, whether the IEKF could be directly used in the initial alignment from the very start is uncertain. In addition, the theoretical basis underneath the invariant theory is less friendly to novices and practitioners [27], [28]. Another way to improve the EKF
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 II: Error Models and Application to Initial Alignment
Wei Ouyang, Yuanxin Wu,
Senior Member , IEEE T L INEARIZED T RIDENT Q UATERNION E RROR M ODELS
In this section, the linearized trident quaternion kinematic models are derived. As shown in the companion paper, the vehicle’s attitude, velocity and position can be represented in one trident quaternion compactly as eb eb eb ebe i e ieb i eb i eb q q q qq q q
C r C r (1) where eb q is the attitude quaternion of the body frame relative to the earth frame. ei C is the attitude matrix from the inertial frame to the earth frame. i r and i r are the inertial velocity and position, respectively. , are two imaginary units, satisfying and , 0 . The trident quaternion differential equation is given as e i bi eb ebbeb eb e e ieb i ebe i ebibeieb eib ie ii eb i ee ii beb q q fq g q qq qqq C r C rC r rC r C (2) which can be manipulated into the compact form b eieeb eb ib eb q q q (3) And, the solutions of two trident twists are assumed to take the form bb bib ibie ie ee e f xg x (4) where , x x are usual quaternions. Substitute Eq. (4) into Eq. (3) and compare it with Eq. (2), we have eb eb e ii eb q x x qq C r (5) Then the first-type special solutions for trident twists, which are used in the companion paper, are obviously obtained as
11 2 b bib ibie i be e e eie i fg C r (6) And, the second-type special solutions for trident twists are + b b ib bib ibie ie ie e e fg C r (7) Note that only vectors are denoted in the bold format. However, when performing vectorial addition and multiplication with quaternions, vectors will be automatically transformed to vector quaternions by default. For instance, in Eq. (5) the vector e ii
C r has been directly used as a vector quaternion without further clarifications. In the following derivations, the subscripts of quaternions are neglected for brevity. Additionally, the unit quaternion is defined as and the unit trident quaternion is defined as in the sequel. A. Left trident quaternion error model
The traditional definition of the left quaternion error is l l q q q α (8) where l α is the small-angle attitude error. By analogy, the left trident quaternion error to the first order is defined as
11 2 ll q q q (9) Note here ‘1’ actually denotes the unit trident quaternion, and the error term is also expressed in the trident vector quaternion as l l l l (10) 3where the vector quaternions are specifically =[0, ] l l σ , =[0, ] l l σ and [0, ] l l σ . Accordingly, the derivative of the left trident quaternion error is computed as ˆ ˆ2 2ˆ ˆˆ ˆ ˆ ˆˆˆ ˆˆ ˆˆ b eib ieb eib ieb eib ieb eib el i d q dt q q qq q q q qq q qq q q q q qq q q q (11) where the formula dq dt q dq dt q is used. Using (4), it can be obtained that
12 1 2 1 b e be lb bl ib l ib b bib lb be l ib l g ffx x x CC (12) of which the detailed derivations are provided in Appendix A. It can be seen than only the second imaginary part of Eq. (12) contains the unknown input quaternions [0, ] x x and x . In order to linearize the second imaginary part w.r.t. the trident quaternion errors and make the resultant coefficients time-invariant, we hope that the following condition in terms of the second imaginary part in Eq. (12) can be fulfilled be l l l l x C x x σ k σ k σ k σ (13) in which, , , k k k are time-invariant real matrices. In addition, for the left trident quaternion error, its imaginary parts are further denoted as the error-state vectors (see Appendix B for details) e iie ii bl ebl e C rC r σ Cσ C (14) Substitute Eq. (14) into (13), we have e i e ii i b b be l l e e C r C r x C x x σ k σ k C k C (15) Instead of investigating all possible solutions, two groups of apparent special solutions are directly examined. Substitute Eqs. (6) and (7) into the above condition, and it can be readily found that the first-type solutions are valid with , , k 0 k I k 0 . And the second imaginary term in Eq. (12) can be finally denoted as e ii b bl e ib lbl ib l C r
C ωσ σσ ω σ (16) Note that the pivotal motivation to find the invariant linearization is to finally improve the consistency of the resultant EKF. In order to linearized the left two parts, the body frame angular velocity measurements are explicitly defined as b bib ib g g bω ω n , and the measured specific forces are b b a a f f b n . Accordingly, their error terms are taken as the corrected estimate minus the truth ˆ gb b bib ib i gb g b bω ω ω n (17) ˆ b b ba a a f f b f b n (18) According to Appendix B, e eb l r C , so the gravitational force error in Eq. (12) can be computed as ˆ = e ee e e e lbe e e g gg g g r C σr r (19) in which, the term e e g r is approximated as (see [4], ch. 14) ˆ ˆ2ˆ ˆ e e eT ee e eeS g g r rr r r (20) where the formula for the geocentric radius ˆ eeS r and the gravitational force e g can also be found in [4] (ch. 2). Substitute Eqs. (16)-(19), the linearized left-error model is finally united in l l l l k x F x G w (21) where the error states, Jacobian matrices and noises are given as l Tl all g x σ σ σ b b (22) k g a grw arw T w w w w w (23) bib eb b bib e e ebl bib ω 0 0 I 0gf ω C 0 IrF 0 I ω 0 00 0 0 0 00 0 0 0 0C (24) l I 0 0 00 I 0 0G 0 0 0 00 0 I 00 0 0 I (25)
Remark 1:
In the Jacobian matrix F l , the term related to the gravitational gradient is in the order of 10 -8 in general, therefore, the left trident quaternion error model can be regarded as estimate-invariant over the linearization interval. B. Right trident quaternion error model
The right trident quaternion error is defined as r r q q q (26) And, the right trident quaternion error is further denoted as r r r r (27) The differential equation of the left trident quaternion error is accordingly derived as 4 ˆ ˆ2 2 2 ˆ ˆˆ ˆ ˆ ˆ ˆˆ ˆ r r b e b eib ie ib ieb e b eib ie r ib r r ie q q q q dq dtq q q q q q q qq q q q q q q (28) It can be further obtained that = e e br r ie b ibeie r re i e b e bi b ib be e bb ib re eb eie er fxg xx g CC r C Cr CC (29) for which detailed derivations are provided in Appendix C. Let us focus on the linearization of the second imaginary part. Similarly, it is expected to satisfy the following condition so as to get time-invariant coefficients w.r.t. the trident quaternion errors e r rr rb x x x σ k σ σ kC k σ (30) in which, , , k k k are time-invariant matrices. Substitute the following relationships (see the derivation of Eq. (90)) into Eq. (30), e i e ii ie i e ir rrrr i ie e σ C r σ C rσ C r σ C rr σ r (31) and test two groups of special solutions in Eqs. (6) and (7), respectively. It indicates that the first-type solutions are still valid and , , k 0 k I k 0 . As a result, the second imaginary part in Eq. (29) is computed as e e b ebr ib r ie r σ r C ω σ ω σ (32) Next, we further consider the term related to the gravitational error ee eee ree ere ee re r gg rrgrg g r r r σ σ r σ σ (33) in which, the second relationship in Eq. (31) is used. Finally, we also reformulate the differential equation as r r r r k x F x G w (34) where the error states and the Jacobian matrices are given as Tr r r r g a x σ σ σ b b (35) e eie be ee e e eie be iieie be e eb er e ω 0 0 C 0g gg ω C Cr r0 I r C 00 0 0 0 00 0 0 0 0C rω rF (36) eb e eb be ii eb er C 0 0 0C C 0 0r C 0 0 00 0 I 00 0 0 IC r G (37) Remark 2:
Note that the terms related to e r and i r cannot be omitted due to large magnitudes of the position vector (10 ) and the velocity vector (10 ). Therefore, the right trident quaternion error model is just quasi estimate-invariant since these estimate-related terms are still adversely affected by the velocity and position estimates, especially in the in-motion alignment scenarios. In fact, the “translation” of the trident quaternion could be alternatively expressed in the body frame as b i b ieb eb eb i eb i q q q q C r C r (38) which is equivalent to Eq. (1) since e i e i b ii eb eb eb i eb eb i q q q q q C r C r C r (39) As for the robocentric representation [26], the trident quaternion can be alternatively defined as the e -frame relative to the b -frame, i.e., b i b ibe i bebe i be qq qq C r C r (40) The corresponding linearized left- and right-error models of robocentric formulations are directly provided in the Appendix D following similar derivation process. III. R ELATIONS TO E XTANT S TATE E RRORS
This section clarifies the relationship of newly defined trident quaternion errors w.r.t. traditional error definitions and invariant error definitions in [21], [26]. In addition, the correspondences of their covariance are also presented, which are vital in applying these methods in EKF. A. Relations with Traditional Error Definitions 1)
Left errors
Usually, the left errors of the attitude, velocity and position are defined as ˆ ˆˆ e e ee e e θ θ θv v vr r r (41) in which the small error-angle vector θ is routinely defined as the attitude error in the standard EKF. 5 In contrast, the left trident quaternion errors are listed as follows (see Eqs. (9), (10) and (14)) ˆ ˆˆ 1 2ˆ 1 l ll b e i e i b e ie i i e ib e el e be e q q q C C r C r C C rC rσ rσ C r (42) where the left attitude error is actually the error quaternion and [0, ] l l σ . It can be noticed that the error-angle vector in Eq. (41) just entails the first real part of the trident quaternion error if the error angles are small enough. As for the covariance, we should pay attention to the relationship between l σ and e v . Since e i e e ei ie C r ω r v (43) where Teie ie ω is the earth rotation angular rate expressed in the earth frame. Then, we have E EEE E b e i T e i bTe i i e Tb e e e e e e bTe ie ie eb e eT bTe e Tb e e eT e bTe ie ie eTl l
C C r C r CC ω r v ω r v CC v v CC ω r r ωσ Cσ (44) Specifically, the last term can be omitted in static alignment, given the small magnitude of the earth rotation rate. Similarly, the covariance for l σ is computed as E = E b e eT bTeTl l e σ σ C r r C (45) To sum up, the transformation between two covariance matrices are reformulated as
Tl l l P J PJ (46) where P denotes the traditional covariance including the uncertainties of the attitude/velocity/position, and the left transformation matrix is b e bl e ie e be I 0 0J C ω C 00 0 C (47) where I and are 3 × Right errors
In contrast, the right trident quaternion error is defined as (see Eqs. (26) and (31))
1ˆ 1 2 r rr rr e i e ii ie er q q q C r σ C rrσ σσ r (48) in which [0, ] r r σ . According to Eq. (43), the covariance of r σ can be given as EE EE
Te i T e ii r i Te e eT eTr r r e eTie ie
C r σ σ C rω r r ω v vσ σ (49) Similarly, the covariance for r σ is computed as E = E E+
Tr r Te T e e eTrr σ σ r σ σ r r r (50) The relationship between the traditional covariance with the newly defined covariance can be written cohesively as
Tr r r P J PJ (51) where the transformation matrix is e i er i iee I 0 0J C r I ωr 0 I (52) B. Relations with Error Definitions in [21], [35]
Although the invariant EKF and the proposed nonlinear definitions of state errors [21] did not consider the rotational earth for high-accuracy navigation applications, we noticed that in the recent work [35] the earth rotation was taken into account in developing the exact preintegration formulae. According to [35], the extended pose of a rigid body on SE (3) is defined as e eb R v r0 R C0 (53) Note that the auxiliary variable v was introduced in [35] to construct the “group affine” structure, defined as follows e e eie v v ω r (54) which is exactly the “translation” (cf. Eq. (43)) corresponding to the first imaginary part in the trident quaternion. Therefore, the modified SE (3) representation in Eq. (53) is theoretically equivalent to the currently proposed trident quaternion representation. Based on Eq. (53), the following left and right errors can be derived. Left errors
As in [21], [26] and [35], the left Lie group error l SE is defined as ˆ ˆ ˆ ˆˆˆ 0 1 00 0 1 T T T e el
R R R v v R r r (55) The corresponding Lie algebra ^ l is related with l as ^ 1 33 2^ (exp , 1 00 1 3) R v rl l ll m l l ξ ξ ξ00 se (56) where vectors , , R v rl l l ξ ξ ξ are the new error states developed in the left IEKF, and exp m is the matrix exponential. 6 Proposition 1:
The left group error of Eq. (53) entails the nonlinear errors of the velocity and position in Eq. (55), which are exactly included in the left trident quaternion error as the first and the second imaginary parts defined in Eq. (26).
Proof.
Comparing Eqs. (55) and (42), the nonlinear velocity error ˆ ˆ T R v v and the nonlinear position error ˆ ˆ
T e e R r r are exactly equivalent to l σ and l σ , respectively. In addition, the left attitude error Rl ξ is equal to l σ . Remark 3:
In [21] and [26], the error-state vectors , ,
R v rl l l ξ ξ ξ defined in the left IEKF are the error vectors corresponding to the lie algebra on (3) se . They are related with the group error to the first order as ^ l d l I , where d I is the group identity element. Therefore, the state vectors actually used are ˆ R Tl ξ R R I , ˆ ˆ v Tl ξ R v v , and ˆ ˆ r T e el ξ R r r . Indeed, the error states l σ , l σ and l σ are also first-order approximation of the left trident quaternion error. Right errors
The right Lie group error r SE is denoted as ˆ ˆ ˆ ˆˆˆ 1 00 1 T T e T er
RR v RR v r RR r00 (57) and the corresponding Lie algebra ^ r is given as ^ 1 33 2^ 1 (exp , 1 00 1 3) R v rr r rr m r r ξ ξ ξ00 se (58) where vectors , , R v rr r r ξ ξ ξ are the error states in the right IEKF. Proposition 2:
The right group error entails the nonlinear errors of the velocity and position in Eq. (57), which are equivalent to the first and the second imaginary parts of the right trident quaternion error defined in Eq. (48).
Proof . The transformed right velocity error and the right position error in Eq. (57) can be approximated to the first order as ˆ ˆ ˆ+ˆ
T RrRr v RR v v I ξ vv ξ v (59) and ˆ ˆ ˆ+ˆ e T e e R ere R er r RR r r I ξ rr ξ r (60) Comparing with the results in Eq. (48), the proof is finished.
Remark 4:
In [21] and [26], the state vectors , ,
R v rr r r ξ ξ ξ defined in the right IEKF are the error vectors corresponding to the Lie algebra on (3) se . They are related with the errors on Lie group to the first order as ^ r d r I and accordingly ˆ R Tr ξ RR I , ˆ ˆ v Tr ξ v RR v and ˆ ˆ r e T er ξ r RR r . With Proposition 2 and Eq. (48), the equivalence between the error states in the right IEKF and the right trident quaternion errors is obvious. IV. M EASUREMENT M ODEL AND L INEARIZATION
This section first provides preliminaries about the INS/odometer integrated navigation scheme. A navigation-grade IMU is mounted on the land vehicle, and the odometer (magnetic encoder) is mounted on the non-steering wheel. As shown in Fig. 1, the center of the vehicle frame O m is situated at the middle point of the rear non-steering axle of the vehicle. The x m axis points forward, y m axis points upward, and z m axis is along the right direction. The odometer measures the covered distance in terms of accumulated pulses, i.e., the number of pulses generated from the very start, and we assume that the measurement frame is overlapped with the vehicle frame for simplicity. The IMU frame, however, is usually misaligned with the vehicle frame by mounting angles 𝜑 , ψ , θ . The displacement between the IMU center O b and the vehicle center O m is the lever arm l b , expressed in the body frame. The navigation frame is defined as north, up and east. Besides, Fig.1 also shows the relationship of the vehicle body frame w.r.t. the earth-centered earth-fixed (ECEF) frame. m x m z m y m O b O b z b x b y bb m l e O e x e z e y Figure 1: IMU installed on a vehicle, with definitions of the IMU body frame, the vehicle frame, the navigation frame and the ECEF frame.
As in [31] and [36], the odometer scale factor K is defined as the number of pulses generated when travelling the 1 m distance. In EKF, the mounting angles, lever arms, and the scale factor K are regarded as random constants. For a navigation-grade IMU, the biases b g , b a for gyroscopes and accelerometers can also be modeled as constants, respectively. The attitude matrix transforming the IMU body frame b to the vehicle frame m , by the 2-3-1 rotation sequence, is given as mb C M M M (61) where i M denotes the elementary rotation matrix along the i -th axis. Since the mounting angle along the forward direction 7is unobservable [31], after omitting the angle the parameters estimated in EKF is modeled as
0, 0, 0, , , b g grw a arw K l 0 b w b w (62) where grw w and arw w are gyroscope and accelerometer random walks, respectively. Besides, the measurement model of the odometer pulse velocity is given as in [36]. T m b n b bb n eb y K e C C v ω l (63) where beb ω is the angular velocity of the vehicle body frame w.r.t. the earth frame. e is the three-dimension unit vector with the first element being 1. n v is the velocity in the navigation frame. The symbol “ × ” denotes the antisymmetric operation or the cross product between two three-dimension vectors. For this INS/odometer integrated navigation system, the linearized measurement models for the static alignment and the odometer-aided in-motion alignment are subsequently derived. Specifically, the static alignment relies on the zero-velocity measurement and the odometer-aided in-motion alignment depends on the land vehicle odometer pulse velocity information along with the non-holonomic constraints [36]. Based on Eqs. (22) and (35), the left and right error states in the EKF are respectively defined as , , , , TTl l bT K x lx (64) and , , , , TTr r bT K x lx (65) A. Linearization of Static Alignment Measurement Model
The zero-velocity measurement model for static or quasi-static initial alignment is given as e k y v ν (66) where the true measurement values of e v are always zero and the noise vector k ν accounts for the vehicle’s vibrations. And, the measurement error is defined as ˆ ˆ e e k y y v v ν (67) Left error model
According to the definition of the left trident quaternion error in Eq. (42) and the relationship in Eq. (43), the measurement error can be denoted as ˆ ˆ ˆˆ e i e e i ei i ke i ei e eie ieeieel ke eb b kie l y y C r r C r r υC r r υC Cω ωωσ ω σ υ (68) Therefore, the measurement matrix is computed w.r.t. the left trident quaternion error as ee el b bie
H 0 C C 0ω (69) Right error model
According to the right trident quaternion error in Eq. (48), the measurement error can be expressed as ˆ e i ei ke i ei ke eie er r ie r re eie r r iei ei kr y y C r r υC r σ r σ υC r rω σ ω σω σ ωσ σ υ (70) Thus, the measurement matrix is computed as e i ee eie ei ir H C r r I 0ω ω (71) B. Linearization of In-motion Alignment Measurement Model
The pulse velocity measurement model of INS/odometer integrated navigation system is given as [36] ([ 1 1]) m b e b bD b e eb diag K y C C v ω l (72) Then the measurement error can be further approximated to the first order as ˆˆˆ eT eTv b b be gKe bl K y y J C C J b JJ l Jv J v (73) where coefficients matrices are specifically computed as ˆˆ([ 1 1]) ˆˆ ˆ([ 1 1]) ˆ ˆˆ ˆˆ ˆˆ([ 1 1]) ˆ ˆˆ ˆˆ ˆˆ([ 1 1])ˆˆ ˆ([ 1 1]) ˆ ˆ ˆˆˆ([1 0 0]) mv bm bb b eT e b bM b ebeT e b bM b ebm bl b ebm eT e b bK b b eb diag Kdiag Kdiag Kdiag Kdiag Kdiag J CJ C lJ C v ω lJ C v ω lJ C ωJ C C ωD vDM lM (74) See detailed derivations presented in Appendix E. Left error model
In Eq. (73), the first term should be linearized w.r.t. the left trident quaternion error. According to Eq. (43), we have ˆˆ ˆ ˆ e ee e eiT eT eT eTb b b e ll b
C C Cv v ωσ Cv σ σ (75) where the detailed computations are provided in Appendix F. Together with Eq. (73), the derivatives of the measurement error w.r.t. the left trident quaternion errors are ˆˆ eeieTl v bl v eTl v b e J J CJ C vωJJ J (76) Finally, the measurement matrix is formulated as , , , , , , , ,
Tl l l l b l K
H J J J J 0 J J J J (77) Right error model
The first error term in Eq. (73) is linearized w.r.t. the right trident quaternion error in Eq. (48) as (see Appendix G for details) ˆ ˆˆ ˆˆ eT eT eT e eb b b ieeT eT eb b iee e rr r
C C C r ωCv v C ω σσ σ (78) Combining with Eq. (73), the derivatives of measurement error w.r.t. the right trident quaternion errors are 8 ˆˆˆ eT e er v b ieeTr v beT er v b ie
J J C r ωJ J CJ J C ω (79) Finally, the measurement matrix is formulated as , , , , , , , ,
Tr r r r b l K
H J J J J 0 J J J J (80) It can be seen that the linearized right measurement error model is less affected by inaccurate estimates than the left format in Eq. (76) due to the small magnitude of the earth rotation rate eie ω in r J . Especially for the in-motion alignment process, the initial velocity errors in l J are more prominent than the position error. In addition, the measurement model in Eq. (78) is intrinsically right-invariant [21], which is more amenable to the right-error definition. V. S IMULATION R ESULTS
Numerical simulations are conducted to show the benefits of using trident quaternion error models in the static alignment and the odometer-aided in-motion alignment. The ultimate object is to investigate whether these models along with the EKF framework could be directly used in the initial alignment under extremely large initial heading errors. Specifically, the land vehicle is equipped with a navigation-grade IMU, which consists of a triad of gyroscopes (bias , random walks ) and accelerometers (bias
30 g , random walks ). The odometer scale factor is 59.8 p/m, i.e., roughly 59.8 pulses would be generated in 1 m distance. The IMU mounting angles are 3 deg in the yaw direction and 2 deg in the pitch direction. The lever arm is set to Tb l m. The numerical simulations and data processing are implemented in the MATLAB platform. A. Static Alignment
Traditionally, the analytical or optimization-based methods are used to deal with coarse attitude, hopefully better than 5 deg. Subsequently, the EKF-based fine alignment algorithm continues to improve the attitude accuracy. To imitate the practical scenarios, we assume that the uncertainties of the initial attitude error are 180 deg in three directions. This is because the unknown heading of a vehicle in the navigation frame means that the attitude in the earth frame is nearly undetermined along three directions due to the relationship e e nb n b C C C . In this simulation, the navigation frame attitude error along the roll and pitch directions are presumed as 5 deg since they can be readily determined to even higher accuracies from the accelerometers [14], whereas the heading errors vary from -180 deg to 180 deg uniformly with gaps of 5 deg. The Monte-Carlo simulation is conducted to assess the comprehensive performances of the traditional EKF, the left trident quaternion error EKF (LQEKF) and the right trident quaternion error EKF (RQEKF). The initial standard deviations of attitude errors are given as 180 deg for the e -frame attitude along three directions to account for extremely large heading errors. Note that the body attitude is estimated in the earth-frame algorithm and then transformed to the navigation frame to compute the estimation errors. As shown in Fig. 2, the LQEKF is superior to EKF and RQEKF in terms of the converging speed and stability. Figure 3 illustrates the RMSEs of attitude errors in the navigation frame. It can be noticed that the LQEKF obviously outperforms the RQEKF and the traditional EKF. The time elapsed to reach the 5-deg heading error is 10s and 51s for the LQEKF and RQEKF, respectively. In addition, the standard deviations of the heading estimation errors in Fig. 4 indicate the consistency of three methods under 180 deg initial heading error. The L/RQEKF show much better filtering consistency than the EKF, and therefore might be a competitive candidate to be used in the static alignment. Figure 2: Monte-Carlo simulation results of three methods. Standard EKF (red), RQEKF (blue) and LQEKF (Green).
Figure 3: RMSEs of attitude estimation errors for static alignment.
Time (s) -4-20 Figure 4: The consistency of three methods under 180 deg initial heading error. EKF (top), RQEKF (middle), LQEKF (bottom). B. In-motion Alignment
In this work, the challenging self-contained in-motion alignment is performed based on the odometer pulse velocity information and the non-holonomic constraints. During the in-motion alignment process, the vehicle moves with varying velocities and turning maneuvers. The Monte-Carlo simulations are similarly conducted when the initial heading errors range from -180 deg to 180 deg with 5-deg gaps. The initial standard deviations of initial attitude errors are also set to 180 deg in three directions. Unlike the static cases, the RMSEs shown in Fig. 5 indicate that all of the three filtering-based methods cannot converge to small angles under arbitrary initial heading errors in a specific time interval, even the best RQEKF can only reach about 4 deg steady-state RMSE in the yaw direction. Therefore, the L/RQEKF cannot be directly exploited in such in-motion alignment scenario, especially under extremely large initial heading errors. In practice, the L/RQEKF could be assisted with analytical/optimization-based methods at the very start, and the time slot assigned for the analytical/optimization-based methods is expected to be shortened a lot in the L/RQEKF methods.
Figure 5: RMSEs of attitude estimation errors for in-motion alignment under -180~180 deg initial heading errors.
The mean attitude errors are compared in Fig. 6, which also show that the RQEKF has the best performance. More specifically, the error statistics of the last 20 seconds in Fig. 7 further indicate that L/RQEKF have much wider convergence regions that the EKF. Besides, the stability of the RQEKF is better than the LQEKF. In this regard, the RQEKF is hopefully to be used to expedite the INS/odometer in-motion alignment process.
Figure 6: Mean attitude errors in the last 100 seconds of the in-motion alignment.
Figure 7: Yaw error distributions of three methods in the last 20 seconds. The standard EKF (top), RQEKF (medium), LQEKF (bottom).
VI. F IELD T EST R ESULTS
Land vehicle experiments are implemented to determine the feasibility and effectiveness of applying LQEKF and RQEKF to initial alignment scenarios. The vehicle was equipped with a navigation-grade IMU set and an odometer with the scale factor about 53 p/m. The bias and random walk for gyroscopes are nominally and , respectively. For accelerometers, the bias is
50 g , and the random walk is
10 g / sqrt(Hz) . The sampling frequency of IMU is 100Hz and the 2-sample algorithm was exploited in the navigation solution [4]. A. Static Alignment
As in the simulations, we also assume no prior information about the vehicle’s initial attitude, and thus, the initial standard deviations for three directions are set to 180 deg as well. The reference attitude is obtained by combing the OBA with subsequent EKF refinement. Besides, the heading errors range Yaw error3- Time (s) Figure 8: RMSEs of attitude errors in system
Figure 9: RMSEs of attitude errors in system
TABLE
I S
TEADY - STATE ATTITUDE
RMSE
S OF THE STATIC ALIGNMENT
Axis Methods
Yaw EKF 30.32
RQEKF
LQEKF 0.049
Pitch EKF 0.214 0.48 B. In-motion Alignment
As observed from the simulation results, the L/RQEKF cannot be directly applied to the in-motion alignment, especially when no turning maneuvers are conducted. In practice, the analytical/optimization-based approaches can be firstly conducted to ensure the convergence under any initial attitude errors, and subsequently the L/RQEKF are applied to further expedite the converging process. Here, we also assume that the initial heading errors range from -180 deg to 180 deg to see the actual performances of three methods. The initial standard deviations in the Monte-Carlo tests are accordingly set as 180 deg in three directions. For each of the four systems, the experiment data are partitioned into twenty isolated segments, each with 600s length, which are further used to test these in-motion alignment methods. Figures 10-11 show four representative Monte-Carlo results from four systems, and the corresponding steady-state attitude RMSEs of four data are listed in Table II. These results indicate that the L/RQEKF have much better performance than the EKF in real systems. Besides, the RQEKF converges faster and achieves higher accuracy than the LQEKF.
Figure 10: RMSEs of attitude errors in system
Figure 11: RMSEs of attitude errors in system
In these extensive tests, however, we indeed found that the performance of L/RQEKF depends on the moving patterns of R o ll ( deg ) H ead i ng ( deg ) Time (s) P i t c h ( deg ) R o ll ( deg ) H ead i ng ( deg ) EKFRQEKFLQEKF
Time (s) P i t c h ( deg ) Figure 12: Statistics of steady-state heading errors over 20 segments with 90 deg initial heading error: RQEKF (left), LQEKF (right).
TABLE
II S
TEADY - STATE ATTITUDE
RMSE
S OF THE IN - MOTION ALIGNMENT
Axis Methods
RQEKF
LQEKF
Yaw EKF
RQEKF
LQEKF
Pitch EKF
RQEKF
LQEKF
Note that the initial values of the gyroscopes, accelerometers and the odometer scale factor are set as given in the specifications, whereas the lever arm and mounting angles are initially set to zeroes and estimated during the in-motion alignment process. Figures 13-14 illustrate the online estimations of lever arm, odometer scale factor and mounting angles by the L/RQEKF and the EKF even under 120 deg heading error. It indicates that the system parameters can be calibrated together with the attitude based on the L/RQEKF method without prior calibration. In contrast, the results from the EKF are less accurate. Note that the odometer scale factor is also estimated online in Fig. 14 since it is usually susceptible to the tire pressure, ambient temperature and the vehicle’s load.
Figure 13: The online calibration of lever arms during the in-motion alignment.
Figure 14: The online calibration of the odometer scale factor and mounting angles during the in-motion alignment.
VII. C ONCLUSIONS AND D ISCUSSIONS
Based on the trident quaternion description of the strapdown INS kinematic model, the linearized left/right-error models are accordingly derived and found to be equivalent to those derived from profound group affine. Moreover, the static and in-motion alignment algorithms for the land vehicle are studied using these error models. In applying their corresponding filtering methods, i.e., the L/RQEKF to the static alignment problem, both of them perform much better than the standard EKF. To some extent, the LQEKF might be directly used without prior attitude information in the static alignment. As for more challenging in-motion alignment scenarios, the L/RQEKF cannot ensure the convergence under extremely large initial attitude errors and thus the analytical/optimization-based methods are recommended to be used to avoid extremely large initial attitude errors. It appears that the L/RQEKF have more large convergence regions than the EKF does, and the LQEKF performs relatively worse than the RQEKF in in-motion alignments. The linearized trident quaternion error models derived in this article is expected to improve the performance of other inertial-based navigation systems. 12A
PPENDIX
A Firstly, the errors of twists are defined as ˆˆ beb b b bib ib ib ibie ie iee e e xfg x (81) where the fact that eie is used. Then, substitute Eq. (81) into (11) b b e el ib ib l l ie ieb el ib ieb b e eib ib l l ie ieb el ib ieb b bib l ib ib q q q qq q q q qq q + el ieb bib l l lb beb b b b bib ib ibb eibb ibl l ll lbib il b f x f xq g x qf x fx q xqg q qq q
12 1 2 1 b e b b be ibb be ib l ll l g f fx x x CC (82) in which the following relationship has been used b b bl ib l ib ib l (83) A PPENDIX
B According to the primitive definition of the left trident quaternion error ˆ ˆ ˆ ˆˆ ˆ ˆ ˆ ˆ l q q q q q q q q qq q q q q q q q q q (84) We have ˆ2 ˆ2 2ˆ ˆ ˆ ˆˆ ˆˆ ˆ 11 2 l e i e ii ie i e ii i e ii e il ib ie i l e q q qq q q qq qq q q qqq q q C r C rC r C rC r C rC C r (85) Similarly, we can prove that b e i b el e i e σ C C r C r (86) A
PPENDIX
C Similar to the derivations in Appendix A, Eq. (28) is further computed as b e e er ib ie r r ie ieb bib ib rb e e eib ie r r ie ieb bib ib re er ie ie r q q q qq q qq qq q q b eib iee b er ie ib iee br ie ibeie r rbibb bibb bib ibee ree i r qq qq qg q qq q q f qq q q qx xgq x q
12 2 21 eie r re e br ie b ib e i e b e bi b ib be e bb ib re eb ie r e e fgx xx g
C C r C Cr CC (87) In specific, we have used the following relationships in the derivations. b bib ibb bib ibe b e bb ib b ibe i e b e b e ii b ib b ib ie i e b e b e ii b ib b ib ie i e bi b ib q q q qq q q q q q q qq q q qq q C CC r C C C rC r C C C rC r C (88) b b e e bib ib b ib q q q q r C (89) ˆ ˆ2 2 2ˆ ˆ ˆ ˆ1 11 12 212 e i e ii ie i e i e ii i ie i e ir r r rr rr e ii i ie i e ii i q q q q qq q q q
C r C rC r C r C rC r C r C rC r C r (90) 13A
PPENDIX
D The robocentric and previously derived world-centric representations are summarized in Table III. A
PPENDIX
E The velocity measurement error is approximated to the first order as ˆ ˆˆ ˆˆˆ ([ 1 1]) ˆ ˆ ˆˆ([ 1 1]) ˆ ˆˆˆ ˆ ˆˆ([ 1 1]) ˆˆ ˆ([ 1 1]) ˆ ˆˆ ˆ([ 1 ˆ1]) ˆ m eT b bb b ebmb Mee e MeT b b bb eb gm eT eTb b bm b eb g eM b diag Kdiag K Kdiag Kdiag Kdiag K y y C C ω lC M DD MC ω b l lC C CC l bvv vD CvM ˆˆˆˆ ˆˆ ˆˆ ˆˆ([ 1 1]) ˆˆ ˆ([ 1 1])ˆ ˆ ˆˆˆ+ ([1 0 0])ˆˆ e T e b bebeT e b bM b ebm b bb ebm eT e b bb b ebeT eT bv b b b g l Ke diag Kdiag Kdiag K K v ω lC v ω lC ω lC C v ω lJ C C J b J J J lDv JMv (91) where the approximation b b b eeb ib e ie g ω ω C ω b is used. And the IMU-vehicle misalignment matrix and the derivatives of the elementary rotation matrix are given as mb C M M (92) sin 0 cos sin cos 0= 0 0 0 , = cos sin 0cos 0 sin 0 0 0
M M
D D (93) A
PPENDIX
F Derivations of Eq. (75) is given as ˆ ˆ ˆˆ ˆ= ˆˆ eT eT eT e eTb b b be b TeTe eb ree e
C C C C CC Rv v v vv v (94) which is naturally denoted in the right attitude error. However, in this case it should be expressed with the left attitude error based on the relationship from Eq. (41) ˆ ˆ e eTr b l b
R C R C (95) And Eq. (75) could be further re-expressed as
TABLE
III S
UMMARY OF W ORLD - CENTRIC AND R OBOCENTRIC REPRESENTATIONS
World-centric F matrix G matrix
Left-error bib eb ebb bib e ewcl bib ω 0 0 I 0gf ω C 0 IrF 0 I ω 0 00 0 0 0 00 0 0 0 0C wcl
I 0 0 00 I 0 0G 0 0 0 00 0 I 00 0 0 I
Right-error e iieie eie be ee e e eie b be bee e ewcr e ω 0 0 C 0g gg ω C Cr r0 I r C 00 0 0 0 00 0 0 0 0C rω rF eb e eb bi be i e wc er C 0 0 0C C 0 0r C 0 0 00 0 I 00 0 0 IC r G Robocentric F matrix G matrix
Left-error rc e eie be e eie be e e i ei be ee e eie b l e ω 0 0 C 0g ω CF 0 I 00g g C r Cr r0 0 0 0ω 0r0 0 C0 0 r rc eb e b e i e i be eb l C 0 0 0C 0 00 0 00 0 IC r Cr C 00 0 0 I G Right-error rc bib eb b bib e er b ei bb ω 0 0 I 0gf ω C 0 IrF 0 I ω 0 00 0 0 0 00 0 0 0 0C rc r I 0 0 00 I 0 0G 0 0 0 00 0 I 00 0 0 I
Superscripts wc denotes the world-centric versions and rc denotes the robocentric versions. ˆ ˆ ˆ ˆˆ ˆˆ ˆˆ ˆˆ ˆˆ ˆ ˆˆ ˆˆ ˆˆˆ TeT eT eT e eTb b b b l bTeT eTb l b TeT eTb l beT eTb b leT e i e eTb i b leT eT e eTb b b leT ee e e ee ee ee e e Tb l b eiee el iee v v v vv vv vv vC C C C R CC R CC I σ CC C σC C r r C σC C r C σC σ ω vσ vσ Cωv eie l ω σ (96) A PPENDIX
G Derivations of Eq. (78) are based on Eqs. (43) and (48), ˆ ˆ ˆˆˆˆ ˆ ˆˆ ˆ ˆˆ= e ee e rreT eTb beT e eTb b b TeT e i e e e i e eb i ie i ieeT e i e i e e e eb i i ie ieeT e e e e eT eT eb ie ie rr r rb b ieeT e eb ie br
C CC C CC C r ω r I C r ω rC C r C r ω r ω rC ω r ω r C C ωCv vv v σσω σr C σσ σσ ˆ eT eT eb ir re C ωσ σ (97) where the following relationship is used a b a b b a (98) R
EFERENCES [1]
K. R. Britting,
Inertial Navigation Systems Analysis , John Wiley & Sons, Inc., 1971. [2]
A. Barrau. Non-linear state error based extended Kalman filters with applications to navigation. Automatic. Phd thesis, Mines Paristech, 2015. [3]
A.J. Krener, The Convergence of the Extended Kalman Filter. In: Rantzer A., Byrnes C.I. (eds) Directions in Mathematical Systems Theory and Optimization. Lecture Notes in Control and Information Sciences, vol. 286. Springer, Berlin, Heidelberg, 2003. [4]
P. D. Groves,
Principles of GNSS, Inertial, and Multisensor Integrated Navigation, Systems , 2nd ed. Boston, MA, USA: Artech House, 2013. [5]
M. D. Shuster, S. D. Oh, Three-axis attitude determination from vector observations, Journal of Guidance, Control and Dynamics, vol. 4, no. 1, pp. 70-77, 1981. [6]
F. L. Markley, Attitude Determination Using Vector Observations: A Fast Optimal Matrix Algorithm,
Journal of the Astronautical Sciences , vol. 41, no. 2, pp. 261-280, 1993. [7]
Q. Fu, Y. Liu, Z. Liu, S. Li and B. Guan, High-Accuracy SINS/LDV Integration for Long-Distance Land Navigation, in
IEEE/ASME Transactions on Mechatronics , vol. 23, no. 6, pp. 2952-2962, 2018. [8]
M. Wu, Y. Wu, X. Hu and D. Hu, Optimization-based alignment for inertial navigation systems: Theory and algorithm.
Aerospace Science and Technology , vol. 15, no. 1, pp. 1-17, 2011. [9]
P. M. G. Silson, Coarse Alignment of a Ship's Strapdown Inertial Attitude Reference System Using Velocity Loci,
IEEE Transactions on Instrumentation and Measurement , vol. 60, no. 6, pp. 1930-1941, 2011. [10]
L. Chang, J. Li and K. Li, Optimization-based alignment for strapdown inertial navigation system: comparison and extension,
IEEE Transactions on Aerospace and Electronic Systems , vol. 52, no. 4, pp. 1697-1713, 2016. [11]
Y. F. Jiang, Error analysis of analytic coarse alignment methods.
IEEE Transactions on Aerospace and Electronic Systems , vol. 34, no. 1, pp. 334-337, 1998. [12]
G. M. Yan, W. S. Yan, D. M. Xu and H. Jiang, SINS initial alignment analysis under geographic latitude uncertainty.
Aerospace Control , vol. 26, no. 2, pp. 31-34, 2008. [13]
H. Xue, X. Guo, Z. Zhou, and K. Wang, In-motion Alignment Algorithm for Vehicle Carried SINS Based on Odometer Aiding,
Journal of Navigation , vol. 70, no. 6, pp. 1349–1366, 2017. [14]
Y. Wu, X. Pan, Velocity/position integration formula part I: Application to in-flight coarse alignment.
IEEE Transactions on Aerospace and Electronics Systems , vol. 49, no. 2, pp. 1006-1023, 2013. [15]
Y. Wu, J. Wang and D. Hu, A New Technique for INS/GNSS Attitude and Parameter Estimation Using Online Optimization,
IEEE Transactions on Signal Processing , vol. 62, no. 10, pp. 2642-2655, 2014. [16]
R. M. Rogers, IMU In‐Motion Alignment without benefit of attitude initialization.
Navigation , vol. 44, pp. 301-311, 1997. [17]
B. M. Scherzinger, Inertial navigator error models for large heading uncertainty.
Proceedings of the IEEE 1996 Position Location and Navigation Symposium (IEEE PLANS), Atlanta, GA, Apr. 22—26, pp. 477-484, 1996. [18]
X. Kong, E. M. Nebot and H. Durrant-Whyte, Development of a nonlinear psi-angle model for large misalignment errors and its application in INS alignment and calibration,
Proceedings 1999 IEEE International Conference on Robotics and Automation , vol.2, pp. 1430-1435, Detroit, MI, USA, 1999. [19]
E. A. Wan and R. Van Der Merwe, The unscented Kalman filter for nonlinear estimation,
Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium , pp. 153-158, Lake Louise, Alberta, Canada, 2000. [20]
A. Barrau and S. Bonnabel, Invariant Kalman Filtering, Annual Review of Control, Robotics, and Autonomous Systems. vol. 1, no. 1, pp. 237-257, 2018. [21]
A. Barrau and S. Bonnabel, The Invariant Extended Kalman Filter as a Stable Observer,
IEEE Transactions on Automatic Control , vol. 62, no. 4, pp. 1797-1812, 2017. [22]
E. Robert and T. Perrot, Invariant filtering versus other robust filtering methods applied to integrated navigation, , pp. 1-7, St. Petersburg, 2017. [23]
K. Wu, T. Zhang, D. Su, S. Huang and G. Dissanayake, An invariant-EKF VINS algorithm for improving consistency, , pp. 1578-1585, Vancouver, BC, 2017. [24]
T. Zhang, K. Wu, J. Song, S. Huang and G. Dissanayake, Convergence and Consistency Analysis for a 3-D Invariant-EKF SLAM,
IEEE Robotics and Automation Letters , vol. 2, no. 2, pp. 733-740, 2017. [25]
M. Brossard, S. Bonnabel and A. Barrau, Invariant Kalman Filtering for Visual Inertial SLAM, , pp. 2021-2028, Cambridge, 2018. [26]
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. [27]
S. Bonnabel, P. Martin and P. Rouchon, Symmetry-Preserving Observers,
IEEE Transactions on Automatic Control , vol. 53, no. 11, pp. 2514-2526, 2008 [28]
S. Bonnabel, P. Martin and P. Rouchon, Non-Linear Symmetry-Preserving Observers on Lie Groups,
IEEE Transactions on Automatic Control , vol. 54, no. 7, pp. 1709-1713, 2009. [29]
G. P. Huang, A. I. Mourikis and S. I. Roumeliotis, Observability-based Rules for Designing Consistent EKF SLAM Estimators,
International Journal of Robotics Research , vol. 29, no. 5, pp. 502-528, 2010. [30]
G. P. Huang, A. I. Mourikis and S. I. Roumeliotis, A Quadratic-Complexity Observability-Constrained Unscented Kalman Filter for SLAM,
IEEE Transactions on Robotics , vol. 29, no. 5, pp. 1226-1243, 2013. [31]
Y. Wu, Versatile land navigation using inertial sensors and odometry: Self-calibration, in-motion alignment and positioning, in
Proc. Inertial Sensors Syst.-Symp . Gyro Technol (ISS-SGT), pp. 1-19, Karlsruhe, Germany, September, 2014. [32]
Y. Wu, M. Wu, X. Hu, D. Hu, “Self-Calibration for Land Navigation Using Inertial Sensors and Odometer: Observability Analysis,” in
AIAA Guidance, Navigation, and Control Conference , Chicago, Illinois, 10-13, August, 2009 [33]
Y. Wu and X. Pan, Velocity/position integration formula (II): Application to strapdown inertial navigation computation.
IEEE Trans. Aerosp. Electron. Syst. , vol. 49, no. 2, pp. 1024–1034, 2013. [34] A. Barrau and S. Bonnabel, Extended Kalman Filtering With Nonlinear Equality Constraints: A Geometric Approach,
IEEE Transactions on Automatic Control , vol. 65, no. 6, pp. 2325-2338, 2020. [35]
A. Barrau and S. Bonnabel, A Mathematical Framework for IMU Error Propagation with Applications to Preintegration, , pp. 5732-5738, Paris, France, 2020. [36]
Y. Ou, Y. Wu and H. Chen, INS/Odometer Land Navigation by Accurate Measurement Modeling, , Saint Petersburg, pp. 1-9, Russia, 2020., Saint Petersburg, pp. 1-9, Russia, 2020.