Strapdown Inertial Navigation System Initial Alignment based on Group of Double Direct Spatial Isometries
1 Abstract —The task of strapdown inertial navigation system (SINS) initial alignment is to calculate the attitude transformation matrix from body frame to navigation frame. In this paper, such attitude transformation matrix is divided into two parts through introducing the initial inertially fixed navigation frame as inertial frame. The attitude changes of the navigation frame corresponding to the defined inertial frame can be exactly calculated with known velocity and position provided by GNSS. The attitude from body frame to the defined inertial frame is estimated based on the SINS mechanization in inertial frame. The attitude, velocity and position in inertial frame are formulated together as element of the group of double direct spatial isometries ( (3) ).It is proven that the group state model in inertial frame satisfies a particular “group affine” property and the corresponding error model satisfies a “log-linear” autonomous differential equation on the Lie algebra. Based on such striking property, the attitude from body frame to the defined inertial frame can be estimated based on the linear error model with even extreme large misalignments. Two different error state vectors are extracted based on (3) right and left matrix multiplications and the detailed linear state-space models are derived based on the right and left errors for SINS mechanization in inertial frame. With the derived linear state-space models, the explicit initial alignment procedures have been presented. Extensive simulation and field tests indicate that the initial alignment based on the left error model can perform quite well within a wide range of initial attitude errors, although the used filter is still a type of linear Kalman filter. This method is promising in practical products abandoning the traditional coarse alignment stage. Index Terms —Strapdown inertial navigation system, initial alignment, group of double direct spatial isometries, attitude matrix decomposition, invariant Kalman filtering. I. I NTRODUCTION
HE strapdown inertial navigation system (SINS) equipped with accelerometers and gyroscopes can provide attitude, velocity and position successively in a self-contained manner [1]. Initial alignment is the essential stage for the dead-reckoning based SINS. The initial velocity and position can be easily obtained from other navigation sources, say GNSS. In contrast, finding the initial attitude is much tricky as
The paper was supported in part by National Natural Science Foundation of China (61873275). The authors are all with the Department of Navigation Engineering, Naval University of Engineering. (e-mail: [email protected], [email protected], [email protected]). no sensor can measure it accurately and directly. In this respect, the initial alignment always refers in particular to attitude alignment. The rapidity and accuracy are the ultimate pursuit for SINS initial alignment, although they are contradicted with each other [2]. Traditionally, there are two successive alignment stages, i.e. coarse alignment and fine alignment. In the coarse alignment stage, some analytical methods are applied making use of the relationship between the inertial sensors measurements and the geographical information [1, 2]. The fine alignment, always based on state estimation methods, such as Kalman filtering (KF), is used to refine the coarse alignment results [3]. Meanwhile, the inertial sensors drift biases are expected to be estimated in the fine alignment. Since the traditional coarse alignment is only suitable under static condition, much effort has been made to seek robust coarse alignments which are suitable for more practical situations, such as swaying mooring condition or even in-flight condition. In recent few years, the most representative coarse alignment method is the optimization-based alignment (OBA). The OBA transforms the initial alignment problem into a constant attitude determination problem using vector observations [4-8]. It is more robust to angular disturbance and noise than traditional coarse alignment. Aiding by GNSS information, it can also perform in-flight alignment. Given the roughly known attitude by the OBA, the KF-based fine alignment can be readily carried out. A key issue affect the fine alignment performance is the state observability. Different state model, maneuverability or measurements can result in different state observability. In recent few years, Silva et al. have investigated the KF-based fine alignment from the perspective of observability/estimability, measurement selection and estimation algorithms [9-11]. There will be inevitable transitions between the coarse and fine alignment stages. Meanwhile, there is no definite time duration for coarse alignment to provide adequate accurate result for the following fine alignment. Different application conditions necessitate different time allocation for the two stages. With this consideration, researchers have never given up to seek unified alignment methods which can abandon the traditional the two-stage based procedures. The first effort for such end is making use of nonlinear filtering based on the nonlinear error state models [12-16]. However, due to the defects of the nonlinear error state models, such methods
Strapdown Inertial Navigation System Initial Alignment based on Group of Double Direct Spatial Isometries
Lubin Chang, Fangjun Qin, and Jiangning Xu T false observability ” problem. These applications include simultaneous localization and mapping (SLAM), visual inertial odometry (VIO), attitude estimation and so on [17-26]. Actually, matrix Lie group theory has been applied for long in inertial navigation. This is essentially a matter of representation regarding orientation, and how to treat rotation errors in a filter. In the traditional fine alignment, the Euler angles can be viewed as the rotation errors corresponding to the matrix Lie group (3) . It is pointed out that for some inertial based dynamic models, if the state is defined on a Lie group, the dynamic models satisfy a particular “ group affine ” property. Based on such property, the corresponding linear error state model can reflect the nonlinear group error propagation process exactly. It has been found that inertial navigation equations on flat earth are group-affine, that is, they possess some kind of linearity when using a judicious Lie group embedding. The Lie group in question that makes the equations group affine was found to be the group of double direct isometries ( (3) ) [25-28]. Based on this knowledge, the invariant extended Kalman filtering (IEKF) is proposed to perform the unified initial alignment without coarse alignment [25, 26]. Commercial navigation systems applications indicate that the IEKF performs quite well in initial alignment even with very large initial misalignments. Moreover, it is still a type of linear KF and the computational cost is similar with the traditional KF based methods. In [26], the initial alignment is used as an industrial application example of the IEKF. The SINS differential equations on flat earth are virtually expressed in inertial reference frame, which is not suitable for navigation on earth surface. Even in the inertial frame system model, there is either no technical detail about the state-space model and filtering procedure. For SINS applications on earth surface, the local-level geographical frame is commonly used as the navigation frame, since the user-intuitive attitude and position can be obtained directly from the output of the mechanization equations. For this mechanization, there is a tightly coupling between the navigation parameters due to the earth rotation and movement of the local-level frame over Earth’s curvature. Unfortunately, SINS mechanization in the local-level frame does not satisfy the group affine property due to the strong coupling between the attitude, velocity and position. In previous works, the earth rotation and movement of the local-level frame over Earth’s curvature have been ignored in most applications of the IEKF as pointed out in [27]. These facts mainly motivate this paper, which is devoted to judicious making use of the matrix Lie group theory for SINS initial alignment with consideration of the earth rotation and movement of the local-level frame over Earth’s curvature. Based on the aforementioned discussion, this paper introduces an inertial frame which is artificial defined through freezing the local-level navigation frame at the very start of the initial alignment task. Based on the defined inertial frame, the attitude from body frame to navigation frame can be divided into two parts. One encodes the attitude changes of the navigation frame corresponding to its initial position. This part can be calculated directly if the geographical position and velocity are known, which is always the case for initial alignment aided by GNSS. Another part of the decomposed attitude represents the attitude from body frame to inertial frame (artificially defined). The kinetics of this attitude is just the attitude differential equation in inertial frame. Based on the calculated attitude from navigation frame to the defined inertial frame and the known velocity and position, we can provide the initial values and inputs for the velocity and position differential equations in the defined inertial frame. If the attitude, velocity and position in the defined inertial frame are defined on (3) , the corresponding dynamic model satisfies the group affine property. Therefore, the attitude from body frame to the defined inertial frame can be estimated using the corresponding linear error model with even very large attitude misalignments. After obtaining the two attitude parts, the desired attitude from body frame to navigation frame can be readily obtained, that is, the initial alignment is accomplished. It should be noted that the (3) tool is mainly used to define the group state errors and derive the corresponding vector state error models in this paper. Making use of Lie group to design the SINS calculation algorithm is not the research topic of this paper, as that in [29]. That is to say, in our investigated methods, SINS calculation is still accomplished using the traditional methods. In [30, 31], the special orthogonal group (3) is used to represent the initial attitude matrix in the framework of OBA method, which can improve the convergent speed. Just as we have pointed out that, the OBA is essentially a coarse alignment method. In contrast, the presented method is a linear KF based alignment method using ingeniously defined state errors, which is expected to unify the coarse and fine alignment even with extreme large misalignments. That is to say the alignment structure of the presented method is totally different with that in [30, 31]. The remainder of the paper is organized as follows. Section II presents the mathematical tools that we will use to derive the error state model for (3) -KF. In Section III, the desired attitude by initial alignment is decomposed into two parts through introducing the initial inertially fixed navigation frame. In section IV, the error state models are presented for SINS mechanization in inertia frame through formulation the attitude, velocity and position as (3) . Section V derives the detailed error state models based on (3) theory. Both right and left error definitions are considered in this section. Section VI shows the explicit application procedures of the (3) -KF for initial alignment. In section VII, the sound theoretical basis for the reason why the linear error state model can be used to accomplish the initial alignment with large misalignments is presented. Section VIII carries out extensive simulations to 3 evaluate the performance of the (3) -KF based initial alignment. Section IX is further devoted to evaluating the performance of these linear initial alignment methods making use of field test data. Conclusions are drawn in the final section. II. M ATHEMATICAL P RELIMINARIES
In this section, we will present the bare minimum of matrix Lie group and the associated Lie algebra which are required to derive the error state model for the (3) -KF. For more details about the Lie group theory, one can refer to [26, 32]. The group of double direct spatial isometries introduced in [26, 28] is given by (3)(3) , C v p CT 0 I v p (1) where (3) is the special orthogonal group. The inverse of an element of (3) is also element of (3) . Given the matrix Lie group in (1), the corresponding inverse is given by (3) T T T
C C v C pT 0 I (2) The Lie algebra associated with (3) is given by (3) Ξ ζ ζ se (3) where , , φ φ υ ρζ υ υ ρ ρ (4) The matrix Lie group (3) is related to its associated Lie algebra (3) se through the exponential map km k k T ζ ζ ζ = (5) The closed-form solution of (5) is given by expexp l l φ J υ J ρ T ζ (6) where exp cos 1 cos sin T φ I aa a (7) sin sin 1 cos1 Tl J I aa a (8) φ a where is the angle of the rotation and a is the unit-length axis of rotation. The inverse operation of the exponential map is denoted by ln ζ T (9) Give the matrix Lie group in (1), the solution of (9) is given by ll a ζ J v
J p (10) where tr 1cos ,2 C Ca a (11) cot cot12 2 2 2 2 Tl J I aa a (12) Eq. (11) implies that a is a (unit-length) eigenvector of C corresponding to an eigenvalue of 1. III. SINS M ODEL D ECOMPOSITION
In this section, the traditional SINS formulation on navigation frame is decomposed to derive the formulation on inertial frame. The application of SINS formulation on inertial frame aims to reduce the complexity of the corresponding error state equations. Moreover, it will be demonstrated in Section VI that the SINS formulation on inertial frame satisfies a particular group affine properties, which is the theoretical basis for the linear initial alignment with arbitrary initial misalignments. Denote the body frame by b , the local level navigation frame as n , the earth frame as e and the inertial frame as i . The directions of the used navigation frame are “East-North-Up”. Denote the attitude as the transformation matrix from body frame to navigation frame, i.e. nb C , the velocity vector as Tn n n nE N U v v v v and the position vector as Tn L h p with L being the latitude, longitude and h height. The navigation frame differential equations of attitude, velocity and position are given by n bn b nbbn n b n n n nb ie enn nc C ω Cv C f ω ω v gp R v (13) where b b nT n b nT n nnb ib b in ib b ie en ω ω C ω ω C ω ω (14) bib ω is angular rate of the body frame with respect to the navigation frame, expressed in body frame. bib ω can be measured by gyroscopes. nie ω is the earth angular rate expressed in navigation frame and is given by Tnie ie ie
L L ω (15) ie is the earth rotation rate. nen ω is the angular rate of the navigation with respect to earth frame, expressed in navigation frame. nen ω is caused by the linear motion on the curve of spheroid and is given by tan n n nn N E Een M N N LR h R h R h v v v ω (16) where M R is the meridian radius of curvature of the WGS-84 reference ellipsoid and N R is the transverse radius. b f is the specific force and is measured by accelerometers. n g is the gravity vector. c R is the local curvature matrix and is given by 4
10 0sec 0 00 0 1
Mc N
R hLR h R (17) According to the attitude matrix multiplication chain rule, the attitude matrix at time instant t can be decomposed as n t n t n n tn ib ib t n b t b t t C C C C C C (18) where n is the navigation frame at the very state of the initial alignment. n can be viewed as inertial frame because that it is a non-rotational frame. The differential equation of in t C is given by i i ninn t n t C C ω (19) According to the definition of the frame i , it can be easily deduced that the initial value of in t C is error-free, as nin n C C I . The initial alignment is always carried out aided by the GNSS which can provide precise velocity and position information. Therefore, nin ω can also be viewed as known value. In this respect, in t C can be readily calculated based on the known velocity and position information with exact initial value, and therefore can be viewed as known value. The differential equation of ib t C is given by i i bibb t b t C C ω (20) It can be deduced that the initial value of ib t C has a constant misalignment matrix according to the following attitude matrix decomposition n n bib t b t b b t C C C C (21) where b is the body frame at the very start of the initial alignment and can also be viewed as inertial frame. The attitude matrix transformation nb C between two inertial frames is therefore constant. According to the attitude matrix decomposition, if ib t C can be estimated, nb C can be readily obtained with the calculated in t C . Therefore, the problem of estimating nb C has been transformed into estimating ib t C . Accordingly, the inertial frame differential equations of attitude, velocity and position are given by i i bb b ibi i b i nb ni i C C ω v C f C gr v (22) It should be noted that the inertial frame in (20) is essentially n . For initial alignment, i nn C g can be viewed as known input. The relationship between i r and geographical n p is given by i i e i ne e r C r C p (23) where cos coscos sin1 sin Ne n NN
R h LR h LR e h L r p (24) The attitude transformation matrix ie C is given by n eie ee C C C (25) where e is the initial inertially fixed frame corresponding to earth frame e . Accordingly, ee C encodes the attitude changes of the earth frame caused by the earth rotation and is given by cos sin 0sin cos 00 0 1 ie iee ie iee t tt t C (26) ne C is the function of the initial geographical position and is given by sin cos 0sin cos sin sin coscos cos cos sin sin ne L L LL L L C (27) The ground velocity n v can be derived as n n e n ee e v C v C r (28) The velocity i v is given by i i i e i e i e e i ee e e ie e v r C r C r C ω r C r (29) Substituting (28) into (29) gives i i e e i ne ie n v C ω r C v (30) This is the relationship between i v and ground velocity n v . It can be used to initialize i v for velocity calculation in (22) and can also be used as the measurement for the initial alignment. IV. SINS E RROR S TATE M ODELS BASED ON (3) In traditional SINS error state equations, only the attitude is treated specially with consideration of its manifold structure. The reset of the state, including the velocity, position and inertial sensors’ constant drift biases, are all treated as general vectors, that is (3) n δ x where n is the dimension of the state other than attitude. In this paper, we term such error equations as error state equations based on (3) , which is corresponding to the following error state equations based on (3) . According to the expressed frames, there are two different attitude error definitions, one is reference frame attitude error and the other is body frame attitude error. For the decomposed model (22), the reference frame is the inertial frame. In this section, we will present the explicit error state 5 equations for the two different attitude error definitions, which is the basis for the derivation of the (3) based error state equations in the next section. In traditional SINS error model, the velocity and position are treated as vectors in Euclidean space and their errors are defined directly as , i i i i i i δ v v v δ r r r (31) where i v and i r are the SINS calculated velocity and position making use of the formulation (22). There are two different attitude error definitions according to the expressed frames. The first one is the inertial frame attitude error, which is defined as i i Tb b δ C C C (32) In the above definition, the attitude error is difference between the inertial frame and calculated inertial frame, i.e. i i T ib b i δ C C C C (33) With the small attitude error assumption, δ C can be approximated as i ii C I φ (34) where i φ is the attitude error in Euler angles form corresponding to δ C .With small attitude error assumption, the rotation sequence does not affect the result of the corresponding attitude matrix δ C . Actually, i φ can also be viewed as a rotation vector, because that both the Euler angles and rotation vector are two times the vector part of the corresponding quaternion form [33]. Omitting the trivial derivation process, the error model corresponding to the inertial differential equations (22) in terms of i φ , i δ v and i δ r is given by i i bb ib φ C δω (35a) i i b i i bb b δ v C f φ C δ f (35b) i i δ r δ v (35c) where bib δω is the errors corresponding to bib ω ,and is defined as b b bib ib ib δω ω ω . b δ f is the errors corresponding to b f ,and is defined as b b b δ f f f . For the inertial sensors errors, if we only consider the constant drift bias and noise, bib δω and b δ f can be expanded as b b bib g δω ε η (36a) b b ba δ f η (36b) where b ε is gyroscope drift bias and bg η is the measurements noise of gyroscope. b is accelerometer drift bias and ba η is the measurements noise of accelerometer. If we augment the drift biases into the state vector, that is TiT iT iT bT bT
Rso δ x φ δ v δ r ε (37) the corresponding state-space model is given by bgba Rso Rso Rso Rso ηδ x F δ x G η (38) where ibi b ib b Rso (39) ib ib
Rso
C 00 CG 0 00 00 0 (40) The subscript
Rso denotes the involved error state is based on (3) formulation with inertial frame ( right ) attitude error definition. It will be shown in the following context that the inertial frame attitude error is just corresponding to the right error definition in (3) . Another attitude error form is the body frame attitude error, which is defined as iT ib b δ C C C (41) In the above definition, the attitude error is the difference between the body frame and calculated body frame, i.e. iT i bb b b δ C C C C (42) Similarly, with the small attitude error assumption, δ C can be approximated as b bb C I φ (43) Similarly, we can also give the error model with such attitude error definition as [35] b b b bib ib φ ω φ δω (44a) i i b b i bb b δ v C f φ C δ f (44b) i i δ r δ v (44c) Similarly, define the augmented state vector as TbT iT iT bT bT
Lso δ x φ δ v δ r ε (45) the state-space model for the left error definition is given by bgba Lso Lso Lso Lso ηδ x F δ x G η (46) where bibi b ib b Lso ω (47) ib Lso
I 00 CG 0 00 00 0 (48) 6 The subscript Lso denotes the involved error state is based on (3) formulation with body frame ( left ) attitude error definition. It will be shown in the following context that the body frame attitude error is just corresponding to the left error defined in (3) . It is shown in (44) that the velocity and position errors are still expressed in inertial frame. This is because that only the attitude has been specially treated and the other navigation parameters are all directly treated as vectors in Euclidean space. ib C and ib C are both obtained from SINS calculation, as will be shown in Algorithm 1 . The difference of the marked frames is only used to emphasize the expressed frames of the defined attitude errors. The (3) based error state models (35) and (44) are the basis for the derivation of the (3) based error state equations in the next section. It will be shown that, the inertial frame attitude error model (35) is corresponding to the right error model based on (3) and the body frame attitude error model (44) is corresponding to the left error model based on (3) . For initial alignment, the velocity provided by GNSS is always used as measurement. For the indirect filtering application, the filtering measurement is given by i i i y v v δ v (49) i v is given by (30) with known values provided by GNSS. The measurement transition equation corresponding to (49) is given by Rso Lso
Rso LsoH H y 0 I 0 δ x (50) Since the velocity error models in (35b) and (44b) are with the same definition form, the measurement transition equation (50) is suitable for both (37) and (45). V. SINS D AMPING E RROR S TATE M ODELS BASED ON (3) In this section, we will derive the error state models based on (3) . Both the right and left error definitions are considered. In contrast with the (3) based error models, the attitude matrix, velocity and position are formulated together as element of (3) , that is i i ib C v rT 0 I (51) A. Right Error State Model
Firstly, we consider the right error definition. The error-contaminated form of T is given by i i ib C v rT 0 I (52) The corresponding right error is defined as
12 3 2 2 2 3 2 22 3 2 2 i i i i T i T i i T ib b b bi i i i i i ii i i
Rse δ T TTC v r C C v C r0 I 0 IC v C v r C r0 I (53) The subscript
Rse denotes the involved error state is based on (3) formulation with right error definition. In second line of (53), we have made use of (2). Since the filtering is designed in an indirect manner, we would like to derive the vector form corresponding to Rse δ T , i.e. ln Rse dx δ T (54) Denote TiT iT iT dx φ dv dr . i φ is corresponding to ii C and its model can be readily obtained from (35a). According to (10) we have i i i il i dv J v C v (55) i i i il i dr J r C r (56) In this paper, we aim to derive the linear error model. In this respect, the attitude error is assumed to be small. With such assumption, i φ can be derived according to (34). Meanwhile, l J can be approximated as el J I φ (57) Substituting (34) and (57) into (55) and (56) gives i i i i dv δ v v φ (58) i i i i dr δ r r φ (59) It is shown that the velocity error in (58) is similar with the transformed velocity error in [36-38]. Actually, the transformed velocity error can be readily obtained through formulating the attitude and velocity as element of (3) . Actually, the geometric gyroscope drift bias error in [39-41] can also be derived in a similar manner as pointed out in [42]. The differential equation of i dv is given by i i i i i ii b i i b i b i i i i bb b b b ibi i i i b i bb ib b dv δ v v φ v φ C f φ C δ f C f g φ v C δω g φ v C δω C δ f (60) where i i nn g C g . The differential equation of i dr is given by i i i i i ii i i i i bb ibi i i bb ib dr δ r r φ r φδ v v φ r C δω dv r C δω (61) If we augment the drift biases into the state vector, that is TiT iT iT bT bT
Rse dx φ dv dr ε (62) the corresponding state-space model is given by 7 bgba Rse Rse Rse Rse η dx F dx G η (63) where ibi i i ib bi ib Rse (64) ibi i ib bi ib
Rse
C 0v C CG r C 00 00 0 (65) The velocity error measurement equation is given by i i i i i i y v v δ v v φ dv (66) The measurement transition equation corresponding to (66) is given by i Rse
RseH y v I 0 dx (67) B. Left Error State Model
Denote the error-contaminated form of T for left error definition as i i ib C v rT 0 I (68) The left error is given by i i iiT iT i iT i bb b bb i T i i i T i ib b b
Lse δ T T T C v rC C v C r 0 I0 IC C v v C r r0 I (69) The subscript
Lse denotes the involved error state is based on (3) formulation with left error definition. Denote TbT bT bT dx φ dv dr as the vector form corresponding to Lse δ T in (69). b φ is corresponding to bb C and its model can be readily obtained from (44a). According to (10) we have b i T i il b dv J C v v (70) b i T i il b dr J C r r (71) With the similar assumption in (57), (70) and (71) can be approximated as b i T i i i T ib b dv C v v C δ v (72) b i T i i i T ib b dr C r r C δ r (73) The differential equation of b dv in (72) is given by b i T i i T ib bb i T i i T i b b i bib b b b bb b b i bib dv C δ v C δ v ω C δ v C C f φ C δ ff φ ω dv δ f (74) The differential equation of b dr in (73) is given by b i T i i T ib bb b bib dr C δ r C δ rdv ω dr (75) Similarly, define the augmented state vector as TbT bT bT bT bT
Lse dx φ dv dr ε (76) the state-space model for the left error definition is given by bgba Lse Lse Lse Lse η dx F dx G η (77) where bibb bib bib Lse ω ω ω (78) Lse
I 00 IG 0 00 00 0 (79) The velocity error measurement equation is given by i i i i bb y v v δ v C dv (80) The measurement transition equation corresponding to (80) is given by ib Lse
LseH y 0 C 0 dx (81) VI.
SINS I NITIAL A LIGNMENT P ROCEDURES BASED ON (3) E RROR S TATE M ODELS
The explicit indirect SINS initial alignment procedures at time instant k are presented in Algorithm 1 . Algorithm1 : Indirect Initial Alignment based on KF Perform SINS calculation using inertial differential equations (22) , , 1 1 1 , ˆ ˆ, , SINS , , , , i i i i i i b bb k k k b k k k ib k k
C v r C v r ω f Construct the error state space model as in (38), (46) and (63), (77). Using the linear KF to estimate the error state ˆ ˆ, KF , , , , , , k k k k k k k k k dx P dx P F H y Q R Refine the SINS calculated results using the estimated error state , , ˆ ˆˆ ˆ, , , , 1: 9 i i i i i ib k k k b k k k k
C v r C v r dx Reset the error state estimate ˆ 1: 9 = k dx 0 Go to the next time recursion. In Algorithm 1 , the function
SINS is the SINS calculation algorithm corresponding to the inertial differential equations (22). One can apply the single-sample, two-sample or more advanced calculation algorithms to solve the differential equations [43]. For the SINS calculation, the known geographical velocity and position is used to calculate in C and to initialize i v and i r according to (30) and (23). The calculated velocity i v is also used as measurement for KF. The function KF is the general KF algorithm. k F and k H are the discrete-time forms of the transition matrices in (39), (47), (63), (78) and (50), (67), (81). k Q is the process noise covariance and k R is the measurement noise covariance. GPS, = i ik k k y v v is the measurement. The retraction operation denotes the modification process of the navigation parameters by the filtering estimate (in error form). Since the navigation parameters have been constructed as element of (3) , the state estimate ˆ ˆˆ ˆ1: 9 TT T Tk k k k dx φ dv dr should be firstly mapped into its (3) form. This can be done according to (6), i.e. ˆ ˆˆexpˆ k l k l kk φ J dv J dr δ T 0 I (82) For the right error definition (53), the refined navigation parameters are given by , ,2 3 2 2 2 3 2 2,2 3 2 2 ˆ ˆ ˆ ˆˆexp( )ˆ ˆ ˆ ˆˆexp( ) exp( ) exp( ) k k ki i i i i ik l k l k b k k ki i i i i i i ik b k k k l k k k l k
Rse T δ T T φ J dv J dr C v r0 I 0 I φ C φ v J dv φ r J dr0 I (83) The modified navigation parameters can be readily obtained from (83) as , , ˆ ˆexp i i ib k k b k C φ C (84a) ˆˆ ˆexp i i i ik k k l k v φ v J dv (84b) ˆ ˆ ˆexp i i i ik k k l k r φ r J dr (84c) Eqs. (84) are the closed-form modifications of the navigation parameters based on the error estimate. Based on the small attitude error assumption, which has been used for derivation of the error state model, (84b) and (84c) can be further approximated as ˆˆ ˆ i i i i ik k k k k v v dv v φ (85a) ˆ ˆ ˆ i i i i ik k k k k r r dr r φ (85b) One can check that (85a) and (85b) are just corresponding to (58) and (59). Similarly, for the left error definition (69), the refined navigation parameters are given by ,,2 3 2 2 2 3 2 2, , ,2 3 2 2 ˆ ˆ ˆ ˆˆexpˆ ˆˆexp k k k b b bi i i k l k l kb k k ki b i b i i b ib k k b k l k k b k l k k Lse
T T δ T φ J dv J drC v r0 I 0 IC φ C J dv v C J dr r0 I (86) The modified navigation parameters can be readily obtained from (86) as , , ˆ ˆexp i i bb k b k k C C φ (87a) , ˆ ˆ i i b ik b k l k k v C J dv v (87b) , ˆ ˆ i i b ik b k l k k r C J dr r (87c) Similarly, (87b) and (87c) can also be further approximated as , ˆ ˆ i i b ik b k k k v C dv v (88a) , ˆ ˆ i i b ik b k k k r C dr r (88b) which are just corresponding to (72) and (73). Algorithm 1 is mainly used to derive the attitude estimate , ˆ ib k C . The attitude estimate , ˆ nb k C can be readily obtained through (18). The initial alignment procedure with the decomposed model is shown in Fig. 1. In this paper, four different linear state-space models are investigated. It will be discussed in next section that only the left error state model based on (3) is independent on the SINS calculated results and this is just the rooted reason for its superior performance. 9 in t C k k bib ω b f bib ω b f SE/SO H SE/SO F GNSS y or dx δ x ˆ ib C ˆ nb C ib C i v i v Fig. 1. Diagram for the initial alignment method based on attitude decomposition
It should be noted that the right and left state error definitions are seemingly a little different with those in [22-28]. This is because that the definitions in this work are consistent with the traditional attitude error definitions as those in section IV for indirect initial alignment applications. However, the definitions in the presented work are essentially also consistent with those in [22-28]. Take the right attitude error for example, the attitude errors defined in this work and that in [22-28] are both formulated to represent the errors expressed in navigation frame. The only difference is that the attitude error in this work represents the attitude transformation from true navigation frame to calculated navigation frame, while that in [22-28] represent the attitude transformation from estimated navigation frame to true navigation frame. That is to say, there is a transposition relationship between the two definitions. Such difference does not affect the performance of the resulted error model. VII. V ALIDITY A NALYSIS OF (3) B ASED M ODEL FOR L INEAR I NITIAL A LIGNMENT WITH A RBITRARY M ISALIGNMENTS
In this section, the validity analysis of (3) based model for linear initial alignment with arbitrary misalignments is presented. With the group state definition in (51), the SINS model in inertial frame (22) can be rewritten as function of the group state as i b i b i n ib ib b n u C ω C f C g vT (89) In the following, we will demonstrate that the dynamic model (89) is a group affine system and its error model has an interesting log-linear property. Such striking property is just the fundamental of linear KF based initial alignment with arbitrary large misalignment.
Theorem 1 : The dynamics model (89) satisfy the relationship u u u u T T T T T T T I T (90) where , (3) T T are the realizations of the state (50). u is the input of the model (88), which include bib ω , b f and i nn C g . The explicit proof of
Theorem 1 is presented in Appendix. A dynamic model satisfying (90) is called group affine system. An attractive property of the group affine system is that its error dynamics are trajectory independent. As can be seen from (64) and (78) that, the error state models (only consider the navigation parameters) are indeed independent on the global state estimate. Such attractive property on one hand can remedy the inconsistency problem involved in inertial based applications and on the other hand, can make the vector error of the system satisfy a “log-linear” autonomous differential equation. Moreover, it is pointed out in [28] that the nonlinear group state error can be exactly recovered from such “log-linear” error state vector. This is just the theoretical basis for the linear initial alignment with arbitrary large misalignments.
Remark 1:
In the aforementioned discussion, we have not considered the inertial sensors’ errors. Unfortunately, if the inertial sensors’ errors are incorporated into the group state, the resulting model will not satisfy the group affine property. However, during the initial alignment, it is always desired to estimate the drift biases of the inertial sensors coupled with the attitude. Although under static or swaying conditions, some elements of the drift biases will not be observed, multi-position alignment procedures can be applied to remedy such problem. Fortunately, as pointed out in [28, 45], much of the advantages caused by the group affine property can be maintained in the augmented models.
Remark 2:
In order to estimate the nonlinear group state 10making use of the linear vector state model, another prerequisite is that the observation should also be with the invariant form. The invariant observations are defined as [28, 45]
Left-Invariant Observation: y χ b (91a) Rright-Invariant Observation: y χ b (91b) where b is a constant vector. With the invariant observation, the corresponding measurement transition matrix will also be independent on the global state estimate. According to (91), it can be verified that the velocity GPS i v is a left-invariant observation. In this respect, the left error state definition will be more preferred. Moreover, with the left error definition, the corresponding transition matrix will be independent on the global state estimate. However, as can be seen from (81) that the measurement transition matrix Lse H is the function of ib C . It is seemingly that Lse H is not independent on the global state estimate. Fortunately, [26] has pointed out that “ Applying a linear function to the innovation term of an EKF before computing the gains does not change the results of the filter ”. According to such statement, the coefficient ib C in (81) can be moved to the innovation and therefore making the measurement equation be trajectory independent. That is to say, the measurement is redefined as ,trans trans3 3 3 3 3 9 i T i i T i i bb b Lse
H Lse y C δ v C v v dv0 I 0 dx (92) It is shown that the redefined measurement transition matrix is now independent of the global state. In contrast, the measurement transition matrix Rse H is dependent on the global state i v and cannot be transformed into state-independent form through linear operation. Such dependence will degrade the initial alignment performance with large misalignment. For error state models based on (3) , the process models (39) and (47) are both functions of the SINS calculated attitude and therefore, their performance will also be degraded due to nonlinearity caused by large misalignments. Since the velocity is a left invariant observation, initial alignment with model (47) will outperform that with (39), although they have the same measurement model. Remark 3:
In the above discussion, we have presented the theoretical explanations for the fact that why the linear error state model can be used to accomplish the nonlinear initial alignment (even with extreme large misalignment). Next we will present another demonstration how the nonlinear attitude error can be exactly recovered from the linear attitude error model. Much of demonstration follows the works [28, 45]. With the left error definition (or body frame error for attitude), the corresponding attitude error equation is given by bib φ ω φ (93) In (93) we have ignored the gyroscope measurement error bib δω , which will be facilitated to the following demonstration. It is known that when deriving (93), the first-order approximation of the attitude error matrix δ C I φ has been used. The left error corresponding to ib C is given by i T ib b δ C C C (94) The corresponding differential equation of δ C is given by i T i i T ib b b bi T i b b i T ib b ib ib b bb bib ib δ C C C C CC C ω ω
C C δ C ω ω δ C (95) Let exp δ C φ be the initial left invariant error. It can be checked that , 0 , i T it b t b t δ C C δ C C (96) is a solution to the error dynamics equation (95) through , 0 , , , , 0 ,, , i T i b b i T it b t b t ib t ib t b t b tb bt ib t ib t t δ C C δ C C ω ω C δ C C δ C ω ω δ C (97) Denote the vector attitude error corresponding to t δ C as t φ . According to the Lie algebra for (3) , (96) can be reorganized as , 0 , , 0 exp exp exp i T i i Tt b t b t b t φ C φ C C φ (98) It can be extracted from (98) that , 0 i Tt b t φ C φ (99) The differential equation of (98) is given by , , 0 , b i T bt ib t b t ib t t φ ω C φ ω φ (100) which is just the equation (93). It is shown that from (94) to (100), we have not made the first-order approximation for the attitude error. That is to say if the initial error is known, the nonlinear error dynamics can be exactly recovered from this linear system. This property is known as Log-Linear property. In other words, when deriving the linear attitude error model, the small attitude error assumption surprising does not lost accuracy. It is shown that the aforementioned feature is also suitable for the traditional (3) based model (44a) with body frame attitude error. Unfortunately, in the traditional model (44), only the attitude error is expressed in body frame, the velocity and position errors are still expressed in inertial frame. It can be said that the expressed frame is not consistent for model (44). This is the main reason for the performance degradation of this model. VIII. S IMULATION R ESULTS
This section is devoted to numerically evaluating the initial alignment performance based on different error state models. Specially, the following four algorithms are evaluated for comparison: Initial alignment based on error state-space model (38) and (50), denoted as RSO-KF. Initial alignment based on error state-space model (46) and (50), denoted as LSO-KF. Initial alignment based on error state-space model (63) and (67), denoted as RSE-KF. Initial alignment based on error state-space model (77) and (8 no coalruunmwasinco φ onescaersareatco δ coalremertylaacwspmcanocoacmSIRstinpranstFithmpe81), denoted asThe SINS un that is equioise g Hz ). ondition and lgorithms are uns. The horizniform distrimisalignment iwithin
180 1 ssumed to benitial error staovariance is φ with φ dIt should be nnly used to stimated usinalculated attiturror state accoray, different efinement formttitude estimatomparison pre true esti ˆ α α α omparison of dThe attitude lgorithms acroespectively. Itmethods with rror models. Type observatioarge, RSO-KFccomplish theworse than LSOpeed and steameasurement malculated veloonlinear grouporresponding ccomplish thmisalignments,INS calculateSE-KF, whicate-dependencnitial alignmerocess model (nd LSE-KF eady-state estig. 5. It is clehan LSO-KF. model (46) stilerformance wis LSE-KF. nder investigaipped with a t h ) and accThe SINS its sampling evaluated byzontal misaligbution withiis assumed to . The i same for thates are all diagonal withdenoting the cnoted that diffdefine differng KF. Afterude results wrding to their attitude erms. After the tes by differeesentation, th imate directly. Tdifferent initiaestimate errooss 200 Montet is clearly sleft error moThis is becauseon. Moreover,F cannot cone initial alignO-KF and LSEady-state accmodel for RSocity. Accordp state cannolinear error sthe initial ali although its ped attitude. Itsh means thatce in measurent performan(46). From Fighave a very timate results arly shown thThat is to sayl has a negatiith large misaation is locatedtriad of gyroscelerometers (is assumed rate is H y implementinnments are asin
90 90 o satisfy the initial filteringhe four algoriassumed to bh attitude errorresponding ferent attitude rent state errr the KF eswill be refineddefinitions, rerror definitiorefinement, went initial alige attitude errThis is the comal alignment mors by the foe Carlo runs arshown that thodels outperfoe that the veloc, when the innverge. Althnment, its peE-KF in termcuracy. This SE-KF is depding to Lie ot be recovertate vector. Thignment evenprocess modes performancet for this simrement modence more sevg. 2-4, we cansimilar conby the two mhat LSE-KF py, the state-deive effect on alignment. In cd at medium scopes (drift (bias g to be under Hz . Firstly, thng 200 Montessumed to sati and the huniform distrg parameters ithms. Speciabe zero. Therors elementsmisalignmenterrors definitirors which wstimation, thed using the esespectively. Thons have dwe can get dgnment methorors are all gimmon procedumethods. our initial alire shown in Fihe initial aliorm those witcity is a left-initial misalignmhough RSE-Kerformance isms of both conis because thendent on thegroup theorred exactly frhe LSO-KF cn with veryl is dependente is even bettmulation scenal (67) degradverely than n see that the Lnvergent speemethods are shperforms muchependence in the initial alicontrast, the Llatitude h , , noiser static he four e Carlo isfy the heading ribution are all lly, the e initial s set to ts. ions are will be e SINS timated hat is to different different ods. For iven byures for gnment igs. 2-4, gnment th right nvariant ment is KF can s much vergent hat the e SINS ry, the rom the can also y large t on the ter than ario the des the that in LSO-KF ed. The hown in h better process gnment LSE-KF canextheusewithethe FigFigFig n accomplishtreme large eoretical analyed directly inith no prior knere is no neederefore, the to g. 2. Pitch angles g. 3. Roll angles eg. 4. Yaw angles e h the initial amisalignmentysis in Sectionn practical ininowledge on td to perform cotal alignment estimate errors acestimate errors acestimate errors ac alignment quts. This is cn VII. Therefitial alignmenthe initial attitcoarse alignmtime is expec cross 200 Monte cross 200 Monte Ccross 200 Monte uite well evenconsistent wifore, LSE-KF nt applicationtude. In other ent for LSE-Kcted to be redu
Carlo runs Carlo runs Carlo runs
11n with ith the can be ns even words, KF and uced.
Fig inman6 demalalcoanstofbawmot
Fig / ° g. 5. Steady-state In order to fnitial alignmenmisalignment angles estimateand 7. It is eteriorates mumisalignment. Tlignment staglignment. In converge even nd LSE-KF ceady-state estf LSO-KF. Thased on (3 which the nonlmisalignments)ther models. g. 6. Yaw angles z / e estimate errors b further evaluant algorithmsangles have bee errors by theshown that tuch faster witThese results ge for the ontrast, LSO-with large mconverge fasttimates of LSEhese results als is more like linear estimat) can be recov estimate errors bby LSO-KF and K ate the perfors, simulationseen carried oue four algorithmthe convergenth the increashighlight thetraditional R-KF, RSE-KFmisalignments. ter than RSEE-KF are morso indicate tha a log-linear etion error (forvered more ac by RSO-KF and R z / ° KSE-KF rmance of thes with differeut. The resultims are shownnce of the Rse of the initie necessary ofRSO-KF base and LSE-KFMoreover, LE-KF. Howeve accurate thaat the left errorerror system, tr the case witccurately than
RSE-KF ese four ent yaw ing yaw n in Fig. RSO-KF ial yaw f coarse ed fine all can LSO-KF ver, the an those r model through th large that by
Fig noThLSconoabcosimanresfasaccKFthacanminospe
Fig z / ° g. 7. Yaw angles e For initial aonlinear initiaherefore, one SE-KF with nnsideration, tonlinear Euler le to handle mparison. Thmulation evalund UKF acrossspectively. It ster convergecurate and coF and thereforat of UKF. Thn accomplishisalignments onlinear initialeed and steady g. 8. Pitch angles z estimate errors by alignment wl alignment mwould like nonlinear initithe unscentedangle error mlarge initiale simulation suation. The ins 200 Monte Cis clearly shoent speed andnsistent. Morere, its computhis MC evaluh the initial and its perfol alignment mey state accura estimate errors byy LSO-KF and LS with large mimethods haveto compare tial alignment d Kalman filmodel in [16] wl misalignmensetting is the sitial alignmenCarlo runs areown that the Ld the steady eover, LSE-Ktational burdeuation demonsalignment wormance is evethod in termsacy. y LSE-KF and U z / ° SE-KF isalignments, e been investthe performamethods. Wiltering (UKFwhich is arguents is evaluasame with thent results by L shown in FigLSE-KF has astate is alsoKF is based onen is much lesstrates that Lwith extremeven better ths of both conv
UKF some tigated. nce of ith this F) with ed to be ated as e above SE-KF g. 8-10, a much o more n linear ss than SE-KF e large han the vergent FigFig caevalspst wacgycoreclLSevac g. 9. Roll angles g. 10. Yaw angle
Besides the Married out wvaluation, the lso calculatedpecifications oatic initial alig φφφ where E and ccelerometers yroscopes drifoupled with tespectively. Thlearly observeSE-KF is alsoven with veccomplish the estimate errors byes estimate errors MC evaluatiowith misalignutmost precisid as comparof the inertial sgnment is give tan x Ny Ez E ggL φφφ N are the eadrift bias anft bias. The alithe utmost prhe superiorityed. Meanwhi very close to ery large minitial alignm y LSE-KF and Uby LSE-KF and U n, a single ininment
89 8 ion of the statrison referensensors, the uten by c E ie g ε ast and northnd E ε is the eagnment resultecision are shy of LSE-KF oile, the steadthe utmost premisalignment, ment effectivel UKF UKF itial alignmen
89 179 . Iic initial alignnce. For thetmost precisio cos L h components ast componentts by the two mhown in Fig. over UKF can dy-state estimecision. That iLSE-KF caly. nt run is In this nment is e given n of the (101) of the t of the methods 11-13, also be mate of is to say, an still FigmisFigmisFigmis alida g. 11. Pitch angle salignments g. 12. Roll angle salignments g. 13. Yaw angle salignments
In this sectignment algorata segments. O alignment errorsalignment errors alignment errors
IX. F IELD ion, the aforithms are furtOne was colle
100 200-0.02-0.015-0.01-0.005100 2005678910
100 200-10123 s by LSE-KF andby LSE-KF and s by LSE-KF andD T EST R ESULT orementioned ther evaluatedected from a n
150 200 t / s t / s -3
150 200 t / s d UKF with extremUKF with extremd UKF with extremTS four linear d using two fieavigation-grad UKFLSE-KFutmost precisio
UKFLSE-KFutmost precisio
UKFLSE-KFutmost precision me large me large me large initial eld test de ring n n n A. Ring Laser SINS Field Test
A navigation-grade SINS equipped with a triad of ring laser gyroscopes and accelerometers is fixed inside the car. The SINS sampling rate is 125 Hz . The dominating inertial sensor errors for the SINS include: the gyroscope constant drift: h , the accelerator bias: g . Since for the navigation-grade SINS, there is no attitude reference for comparison, we have designed the following experiment sequences: after the SINS is powered-on, some peoples get on and off the car frequently to enhance the disturbance intentionally and a test segment of about 300-seconds test data is collected to test the four linear initial alignment algorithms. The OBA in [8] is carried out using this data segment to obtain the attitude at the very start. The attitude obtained through the OBA is used as the initial value for the RSO-KF, denoted as “RSO-KF with small misalignment” hereafter. It should be noted that the performance of the traditional RSO-KF with small initial misalignment has been widely approved in commercial products. For a harsh comparison of four evaluated alignment algorithms, the initial misalignment
5 5 100 is intentionally added to the attitude estimate by OBA. The alignment results provided by the four methods using collected data under swaying condition are shown in Fig. 14-16, respectively. Parts of the three attitude results from 100s to 300s have been plotted in the right subfigures. For clarity, the yaw angle alignment result by RSO-KF has been omitted in the right subfigure of Fig. 16, because its result is far away from other results. It is shown that the LSE-KF can converge to the same value as the “RSO-KF with small misalignment” within a very short time period, although starting very far from the true value. In contrast, the RSE-KF needs more time to converge and its stead-state estimate is also apart from the results provided by “RSO-KF with small misalignment”, especially for pitch and yaw angles estimate. That is to say, we cannot recover the nonlinear estimation error from the linear model (63). This is because that the right error model (63) and (67) is dependent on the global state and therefore does not possess the Log-Linear property. So, in practical applications, coarse alignment is still necessary for RSE-KF in order to guarantee accurate steady-state estimate. The LSO-KF has a similar convergent speed with LSE-KF. The steady-state accuracy of LSO-KF is also nearly the same with LSE-KF for pith and roll angles. However, for yaw angle estimate, the steady-state accuracy of LSO-KF is worse than that of LSE-KF. In this respect, the LSE-KF is still the best method within the four evaluated methods, which is consistent with the theoretical analysis and the simulation test results.
Fig. 14. Pitch alignment results for ring laser SINS
Fig. 15. Roll alignment results for ring laser SINS
Fig. 16. Yaw alignment results for ring laser SINS B. Fiber Optic SINS Field Test
In this section, the four linear alignment algorithms are evaluated using dynamic data collected from an intermediate-grade fiber optic SINS. In this experiment, a POS, consisting of a navigation-grade fiber optic SINS and GNSS, can be used to provide attitude reference. The specifications of the navigation-grade fiber optic SINS are given as follows: drift bias of gyroscope is about h and drift bias of accelerometer is about g . The specifications of the intermediate-grade fiber optic SINS are given as follows: drift bias of gyroscope is about h and drift bias of accelerometer is about mg . The SINS sampling rate is 200 Hz . A data segment of 300 seconds was collected to evaluate the four linear alignment algorithms. The reference attitude is shown in Fig. 17. The reference velocity provided by the GNSS is shown in Fig. 18. t / s p i t c h / ° RSO-KFLSO-KFRSE-KFLSE-KFRSO-KF with small misalignment
100 150 200 250 300 t / s t / s r o ll / ° RSO-KFLSO-KFRSE-KFLSE-KFRSO-KF with small misalignment
100 150 200 250 300 t / s t / s -200-150-100-50050100150200 y a w / ° RSO-KFLSO-KFRSE-KFLSE-KFRSO-KF with small misalignment
100 150 200 250 300 t / s -91.4-91.2-91-90.8-90.6-90.4-90.2-90-89.8-89.6-89.4 Fig. 17. Reference attitude during the experiment
Fig. 18. Reference velocity during the experiment
In this field test, initial misalignment
89 ;89 ;179 is added for the four evaluated methods and the corresponding alignment results are presented in Fig. 19-21, respectively. It is shown that, under dynamic conditions, all the four methods have a very fast convergent speed. The methods with right error models can also accomplish the initial alignment with extreme large misalignments. This is because that under dynamic conditions, the observability the three attitude angles can be strengthened [46]. The initial alignment methods with left error state models converge faster than those with right error state models. This is because that the measurement is left-invariant. In this field test, LSO-KF and LSE-KF perform nearly identical with each other in terms of both convergent speed and steady-state accuracy.
Fig. 19. Pitch alignment results for fiber optic SINS
Fig. 20. Roll alignment results for fiber optic SINS Fig. 21. Yaw alignment results for fiber optic SINS X. C ONCLUSIONS
This paper investigates the (3) based KF for SINS initial alignment. Through introducing an artificially defined inertial frame, the desired attitude is decomposed into two parts, one of which can be calculated directly with accurate known initial values. The other part coupled with the velocity and position in the defined inertial frame are formulated as the SINS mechanization in inertia frame. Based on the reconstructed t / s -200-1000100200 V e l o c i t y / m / s t / s -0.500.511.5 x / ° RSO-KFLSO-KFRSE-KFLSE-KF y / ° t / s -200-150-100-50050100 z / ° RSO-KFLSO-KFRSE-KFLSE-KF
200 250 300-1-0.50 (3) based models treat the attitude, velocity and position together as element of (3) . Through such formulation, the defined group state model satisfies a group affine property and the corresponding linear error model satisfies a log-linear autonomous differential equation on the Lie algebra. It is proven that the nonlinear group error can be exactly recovered by such linear vector error model, which enables the linear KF based initial alignment with extreme large misalignments be possible. The explicit initial alignment procedures based on the derived error state models are also presented. Through extensive simulation and field test evaluations under different conditions, the results show that the LSE-KF can always performs quite well in terms of both convergent speed and steady-state estimate. Due to its ability to handle the large misalignments, the LSE-KF can be used directly without the traditional coarse alignment stage. In this respect, there will be no transitions between different alignment stages and can further improve the initial alignment speed. Moreover, the LSE-KF is still within the linear Kalman filtering framework and therefore it is very computational efficient. Therefore, the LSE-KF is promising in practical products. In this paper, the initial alignment is accomplished aided by GNSS. In such scenario, the geographical velocity and position are known, which is just the prerequisite for the model decomposition. However, for SINS initial alignment aided by Doppler velocity log or Odometer [47-49], the proposed method is no longer suitable, because these aided sources can only provide velocity expressed in body frame. In the future work, SINS linear initial alignment aided by Doppler velocity log or Odometer with large misalignments will be investigated by means of Lie group theory. A CKNOWLEDGMENT
The authors would like to thank the authors of [44] for providing the open source codes in https://github.com/CAOR-MINES-ParisTech/ukfm.
These programs have given the authors a more thoroughly understanding of the matrix Lie group based algorithms and accelerated the corresponding investigation in this paper. The first author would also like to thank Dr. Martin Brossard from MINES ParisTech, PSL Research University, for his kind guidance and help in investigating of the (3) based algorithms. The first author would also like to thank Yuanxin Wu and Wei Ouyang from Shanghai Key Laboratory of Navigation and Location-based Services, School of Electronic Information and Electrical Engineering, Shanghai Jiao Tong University, for their constructive suggestions in preparation of the paper. The authors would like to thank Dr. Miaomiao Wang from Western University for his guidance in the proof of Theorem 1. The first author would also like to thank Gongmin Yan from Northwestern Polytechnical University for providing the ring laser gyroscope based test data. A PPENDIX
A The proof of
Theorem 1 is given as follows. According to (51),
T T can be calculated as ,1 1 1 ,2 2 21 2 1 3 1 31 3 1 3,1 ,2 ,1 2 1 ,1 2 11 31 3 i i i i i ib bi i i i i i i ib b b b
C v r C v rT TC C C v v C r r (A1) Then u T T is given by i i b i i b i n i i ib b ib b b n b u T TC C ω C C f C g C v v (A2) u T T is given by ,1 ,1 1 ,2 2 21 2 3 1 3 13 1 3 1,1 2,1 ,2 ,1 2 1,13 13 1 i b i b i n i i i ib ib b n bi b ib ibi b i i b i ib ib b b ibi b i nb n u C ω C f C g v C v rT T C ω vC ω C C ω r vC f C g (A3) u T T is given by ,1 1 1 ,2 ,2 21 2 3 1 3 13 1 3 1,1 ,2 ,1 ,2 ,1 23 13 1 i i i i b i b i n ib b ib b ni i b i i b i n i ib b ib b b n b u C v r C ω C f C g vT TC C ω C C f C g C v (A4) u T I T is given by
00 1 0 0 0 0 0 1 00 0 1 0 0 0 0 0 10 0 00 0 0 b b i ni i i i i iib nb bi b ib ibi b i i b ib ib b b ibi b i nb n u T I T ω f C gC v r C v rC ω vC ω C C ω rC f C g (A5) According to (A2)-(A5), it can be easily demonstrated that u u u u T T T T T T T I T (A6) 17R
EFERENCES [1]
D. H. Titterton and J. L. Weston,
Strapdown Inertial Navigation Technology : the Institute of Electrical Engineers, London, United Kingdom, 2nd Ed., 2004. [2]
P. D. Groves,
Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems : Artech House, 2008. [3]
J. C. Fang, and D. J. Wan, “A fast initial alignment method for strapdown inertial navigation system on stationary base,”
IEEE Transactions on Aerospace and Electronic Systems , vol. 32, no. 4, pp.1501-1505, 1996. [4]
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. [5]
M. P. Wu, Y. X. Wu, X. P. Hu and D. W. Hu, “Optimization-based Alignment for Inertial Navigation Systems: Theory and Algorithm,”
Aerospace Science and Technology , vol. 15, pp. 1-17, 2011. [6]
Y. X. Wu and X. F. Pan, “Velocity/position integration formula, Part I: Application to in-flight coarse alignment,”
IEEE Transactions on Aerospace and Electronic Systems , vol. 49, no. 2, pp. 1006-1023, 2013. [7]
Y. X. Wu, J. L. Wang and D. W. 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-2654, 2014. [8]
L. B. Chang, J. S. Li and K. L. 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. [9]
F.O. Silva, E.M. Hemerly and W. C. L. Filho, “On the Error State Selection for Stationary SINS Alignment and Calibration Kalman Filters-Part II: Observability/Estimability Analysis,”
Sensors , vol. 17, no. 3, pp.1-34, 2017. [10]
F. O. Silva, E. M. Hemerly and W. C. L. Filho, “On the error state selection for stationary SINS alignment and calibration Kalman filters-part I: Estimation algorithms,”
Aerospace Science and Technology , vol. 61, pp.45-56, 2017. [11]
F.O. Silva, E.M. Hemerly and W. C. L. Filho, “On the measurement selection for stationary SINS alignment Kalman filters,”
Measurement , vol. 130, pp.82-93, 2018 [12]
H. S. Hong, J. G. Lee and C. G. Park, “Performance improvement of in-flight alignment for autonomous vehicle under large initial heading error,”
IEE Proc.-Radar Sonar Navigation , vol. 151, pp. 57-62, 2004. [13]
H. S. Ahn and C. H. Von, “Fast Alignment using Rotation Vector and Adaptive Kalman Filter,”
IEEE Transactions on Aerospace and Electronic Systems , vol. 42, no. 1, pp. 70-82, 2006. [14]
X. Y. Kong, “INS algorithm using quaternion model for low cost IMU,”
Robotics and Autonomous Systems , vol. 46, no. 4, pp. 221-246, 2004. [15]
H. J. Shao, L. J. Miao, W. X. Gao and J. Shen, “Ensemble Particle Filter Based on KLD and Its Application to Initial Alignment of the SINS in Large Misalignment Angles,”
IEEE Transactions on Industrial Electronics , vol. 65, no. 11, pp. 8946-8955, 2018. [16]
L. B. Chang, F. J. Qin and S. Jiang, “Strapdown Inertial Navigation System Initial Alignment based on Modified Process Model,”
IEEE Sensors Journal, vol. 19, no. 15, pp. 6381 - 6391, 2019. [17]
T. Zhang, K. Wu, J. Song, S. Huang, and G. Dissanayake, “Convergence and consistency analysis for a 3-D invariant-EKF SLAM,”
IEEE Robot. Autom. Lett. , vol. 2, no. 2, pp. 733–740, Apr. 2017. [18]
K. Wu, T. Zhang, D. Su, S. Huang, and G. Dissanayake, “An invariantEKF VINS algorithm for improving consistency,” in
Proc. Int. Conf. Intell. Robots Syst. , Sep. 2017, pp. 1578–1585. [19]
S. Heo and C. Park, “Consistent EKF-based visual-inertial odometry on matrix Lie group,” IEEE Sensors Journal, vol. 18, no. 9, pp. 3780–3788, May 2018. [20]
M. Brossard, S. Bonnabel, and J. Condomines, “Unscented Kalman Filtering on Lie Groups,” in
Proc. Int. Conf. Intell. Robots Syst. , 2017, pp. 2485–2491. [21]
S. Heo, J. H. Jung, and C. G. Park, “Consistent EKF-based visual inertial navigation using points and lines,”
IEEE Sensors Journal , vol. 18, no. 18, pp. 7638–7649, Sep. 2018. [22]
M. Brossard, A. Barrau, and S. Bonnabel, “Exploiting Symmetries to Design EKFs With Consistency Properties for Navigation and SLAM,”
IEEE Sensors Journal , vol. 19, no. 4, pp. 1572–1579, Sep. 2019. [23]
A. Barrau and S. Bonnabel, “An EKF-SLAM algorithm with consistency properties,” arXiv:1510.06263 , 2015. [24]
A. Barrau and S. Bonnabel, “Intrinsic Filtering on Lie Groups With Applications to Attitude Estimation,”
IEEE Transactions on Automatic Control , vol. 60, no. 2, pp. 436–449, 2015. [25]
A. Barrau and S. Bonnabel, “Invariant Kalman filtering,”
Annu. Rev. Control, Robot. Auton. Syst. , vol. 1, pp. 237–257, May 2018. [26]
A. Barrau,
Non-linear state error based extended Kalman filters with applications to navigation . PhD thesis, Mines Paristech, 2015. [27]
M. Brossard, A. Barrau, P. Chauchat and S. Bonnabel, “Associating Uncertainty to Extended Poses for on Lie Group IMU Preintegration with Rotating Earth,” arXiv:2007.14097v1 , 2020. [28]
A. Barrau and S. Bonnabel, “The Invariant Extended Kalman Filter as a Stable Observer,”
IEEE Transaction on Automatic Control , vol. 62, no. 4, pp. 1797–1812, 2017. [29]
J. Mao, J. X. Lian, and X. P. Hu, “Strapdown inertial navigation algorithms based on lie group,”
The Journal of Navigation , vol. 62, no. 4, pp. 1797–1812, 2017. [30]
F. J. Pei, H. Xu, N. Jiang, and D. R. Zhu, “A novel in-motion alignment method for underwater SINS using a state-dependent Lie group filter,”
NAVIGATION, Journal of the Institute of Navigation , vol. 67, no.3, pp. 451-470, 2020. [31]
F. J. Pei, S. Yang, and S. N. Yin, “In-Motion Initial Alignment Using State Dependent Extended Kalman Filter for Strapdown Inertial Navigation System,”
IEEE Transactions on Instrumentation and Measurement , DOI 10.1109/TIM.2020.3027405, 2020. [32]
T. D. Barfoot,
State Estimation for Bootics . Cambridge University Press, 2017. [33]
Q. Wang, G. Chang, T. Xu, and Y. Zou, “Representation of the rotation parameter estimation errors in the Helmert transformation model,”
Survey Review , DOI:10.1080/00396265.2016.1234806, 2016. [34]
G. M. Yan,
On SINS In-movement Initial Alignment and Some Other Problems . Post-doctor work report of Northwestern Polytechnical University. Xi’an, 2008. [35]
F. N. Li, and L. B. Chang, “MEKF with Navigation Frame Attitude Error Parameterization for INS/GNSS,”
IEEE Sensors Journal , vol. 20, no. 3, pp. 1536-1549, 2020. [36]
B. Scherzinger and D. B. Reid, “Modified strapdown inertial navigator error models,” in
Proc. IEEE Position, Location, Navigat. Symp. , 1994, pp. 426–430. [37]
M. S. Wang, W. Q. Wu, P. Y. Zhou and X. F. He, “State transformation extended Kalman flter for GNSS/SINS tightly coupled integration,”
GNSS Solutions , vol. 22, no. 4, pp.1-12, 2018. [38]
M. S. Wang, W. Q. Wu, X. F. He, Y. Li and X. F. Pan, “Consistent ST-EKF for Long Distance Land Vehicle Navigation Based on SINS/OD Integration,”
IEEE Transactions on Vehicular Technology , vol. 68, no. 11, pp. 10525-10534, 2019. [39]
M. S. Andrle and J. L. Crassidis, “Attitude estimation employing common frame error representations,”
Journal of Guidance Control and Dynamics , vol. 38, no. 9, pp. 1–11, Aug. 2015. [40]
M. Whittaker and J. L. Crassidis, “Inertial navigation employing common frame error representations,” in
Proc. AIAA Guidance, Navigation, Control Conference , Jan. 2017, vol. 1031, pp. 1–24. [41]
M. Whittaker and J. L. Crassidis, “Linearized analysis of inertial navigation employing common frame error representations,” in
Proc. AIAA Guidance, Navigation, Control Conference , Jan. 2018, vol. 1600, pp. 1–23. [42]
L. B. Chang, “SE(3) based Extended Kalman Filter for Spacecraft Attitude Estimation,” arXiv:2003.12978 , 2020. [43]
Y. X. Wu and X. F. Pan, “Velocity/position integration formula, Part II: Application to strapdown inertial navigation computation,”
IEEE Transactions on Aerospace and Electronic Systems , vol. 49, no. 2, pp. 1024-1034, 2013. [44]
M. Brossard, A. Barrau, and S. Bonnabel, “A Code for Unscented Kalman Filtering on Manifolds (UKF-M),” arXiv:2002.00878v2 , 2020. [45]
R. Hartley, M. Ghaffari, R. M. Eustice and J. W. Grizzle, “Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation,”
The International Journal of Robotics Research , 2020. [46]
Y. Wu, H. Zhang, M. Wu, X. Hu, and D. Hu, “Observability of SINS Alignment: A Global Perspective,”
IEEE Trans. on Aerospace and Electronic Systems , vol. 48, pp. 78-102, 2012. [47]
Q. Fu, Y. Liu, Z. Liu, S. Li and B. Guan, “High-Accuracy SINS/LDV Integration for Long-Distance Land Navigation,”
IEEE/ASME Transactions on Mechatronics , vol. 23, no. 6, pp. 2952-2962, Dec. 2018. [48]
X. Xu, Z. T. Guo, Y. Q. Yao and T. Zhang, “Robust Initial Alignment for SINS/DVL Based on Reconstructed Observation Vectors,”
IEEE/ASME Transactions on Mechatronics , vol. 25, no. 3, pp. 1659–1667, Jul 2020. [49]
W. Ouyang, Y. Wu, and H. Chen, “INS/Odometer Land Navigation by Accurate Measurement Modeling and Multiple-Model Adaptive Estimation,”
IEEE Trans. on Aerospace and Electronic Systems , doi:10.1109/TAES.2020.3011998, 2020.