IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 1    Abstract — In this paper, the optimization-based alignment (OBA) methods are investigated with main focus on the vector observations construction procedures for the strapdown inertial navigation system (SINS). The contributions of this study are twofold. First the OBA method is extended to be able to estimate the gyroscopes biases coupled with the attitude based on the construction process of the existing OBA methods. This extension transforms the initial alignment into an attitude estimation problem which can be solved using the nonlinear filtering algorithms. The second contribution is the comprehensive evaluation of the OBA methods and their extensions with different vector observations construction procedures in terms of convergent speed and steady-state estimate using field test data collected from different grades of SINS. This study is expected to facilitate the selection of appropriate OBA methods for different grade SINS.  Index   Terms — Attitude   estimation,   inertial   navigation,   initial   alignment,   velocity   integration formula  I.   INTRODUCTION As a dead-reckoning navigation method, the performance of the strapdown inertial navigation system (SINS)  Manuscript received December 19, 2013; revised April 25, 2014, August 23, 2014; released for publication May 10, 2016. DOI. No. 10.1109/TAES.2016.130824. Refereeing of this contribution was handled by A. Dempster. This work was supported, in part, by the National Natural Science Foundation of China (61304241, 61374206, and 41404002).  Authors’ addresses: Department of Navigation Engineering, Naval   University of Engineering, China, Jiefang Road 717, Qiaokou District,Wuhan,  430033 People’ s Republic of China. Corresponding author is L.Chang, E-mail: (changlubin@163.com).  Optimization - based   A lignment for   S trapdown  I nertial   N avigation   S ystem:   C omparison   and  Extension  Lubin Chang ,   Member, IEEE , Jingshu Li , and   Kailong Li
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 2  relies largely on the accuracy of the initial conditions which are determined by the so called initial alignment [1, 2]. Up to this day, the initial alignment has been one of the most researched topics for SINS and various methods have been proposed to address this problem [3]. Typically, the initial alignment can be achieved through two consecutive stages: coarse alignment and fine alignment. Regarding the fine alignment, consensus has been reached that the Kalman filtering based optimal estimation scheme is the standard method. The Kalman filtering based fine alignment is founded on the linearized system error models and necessitates the coarse alignment to provide roughly known initial attitude to guarantee the validity of such linearization [4, 5]. Unfortunately, the traditional analytic coarse alignment methods are no longer applicable when the carrier is moving or maneuvering. Recently, an attitude matrix decomposition based coarse alignment method has been proposed for the carrier under swaying or maneuvering conditions [6-11]. By introducing some   “ fixed ”   reference frames known as inertial frames, the attitude matrix can be decomposed into three parts: attitude matrix as a function of the carrier ’ s angular rate, attitude matrix as a function of the Earth and transport rates and a constant attitude matrix encoding the transformation between the body and navigation frames at the very start of the initial alignment. The first two attitude matrices can be obtained through the attitude update procedure according to the corresponding angular rates. The constant attitude matrix can be derived using solutions to  Wahba’s   problem based on the constructed vector observations. To this respect, the attitude alignment problem has been transformed into a continuous attitude determination problem [6]. In [6, 8], such initial alignment method is termed as   “ optimization-based alignment (OBA) ”   because the   Wahba’s   problem is virtually a constrained least square problem and all the solutions are devoted to minimizing the corresponding cost function in a optimal manner. Since many fruitful algorithms can be readily used to address the attitude determination problem, say   Davenport’s q method   used in [6, 8, 10], the key of determining the constant attitude matrix has been the construction of the vector observations. Since the constant attitude matrix encoding the transformation between the   “ fixed ”   body and navigation frames at the very start of the initial
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 3  alignment, the vector observations expressed in the corresponding reference frames are constructed based on the specific force equation. The specific force equation encodes the relationship between different kinds of acceleration information and therefore, acceleration information based vector observations are firstly constructed and used in the OBA method in [6]. However, there is much external disturbance inherent in the acceleration information, which can degrade the performance of the OBA method significantly. Although the low-pass filter designed in [6] can restrain the disturbance to a certain extent, the parameters design for the low-pass filter based on different maneuvering conditions is a troublesome process. Meanwhile, the disturbance of the accelerometer outputs has not been taken into account in [6]. A straightforward and effective method to restrain disturbance is to integrate the acceleration information over certain time interval, which is just the case in [8, 10].Regarding the length of the integral time interval, there are mainly two integration procedures: one is the integration procedure over the whole alignment time interval in [8] and the other is the integration procedure over certain time interval with fixed length in [10]. It is well known that restraining effect on the disturbance becomes better and better when the integral time interval increases, which seemingly favors the integration procedures in [8]. Moreover, making use of more acceleration information can accelerate the convergent speed of the OBA method. However, when the integral time interval increases, the inertial sensors biases will also cause accumulative errors in the constructed vector observations and in turn, degrade the performance the OBA method. To this respect, the integral time interval should not be too long, which favors the integration procedures in [10] on the other way. The aforementioned conflicting conclusions demonstrate that many issues need to be further fully addressed for the OBA methods. Motivated by the aforementioned discussion, this paper is devoted to evaluating the performances of the OBA method with different vector observations construction procedures using field test data collected from different grades of SINS. Since the inertial sensors biases, especially the gyroscopes biases, have a much negative effect on the precision of the calculated attitude, the existing OBA methods will be no longer applicable for the low-grade SINS, say micro-electromechanical system (MEMS)
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 4  technology based low-cost SINS [12, 13], due to their inability to estimate anything other than the attitude. With this consideration, this paper extends the existing OBA methods and investigates an attitude estimation based alignment method which can estimate the gyroscopes biases coupled with the attitude. Since the investigated attitude estimation based alignment method is used to estimate the attitude at current time that is not constant for the in-motion alignment, we term the investigated alignment method as   “ dynamic OBA ”  method. In contrast, the traditional OBA methods are used to determine the constant attitude matrix at the very start of the initial alignment and can be viewed as   “ static OBA ”   method. The aforementioned different integration procedures have also been investigated and evaluated in the construction of the vector observations for the developed dynamic OBA method. The rest of the paper is organized as follows. Section II mathematically makes the static OBA method statement with main focus on the vector observation construction methods with different integration procedures. In Section III, the dynamic OBA method able to estimate the gyroscopes biases coupled with the attitude is developed. Meanwhile, the explicit filtering procedure is also presented to solve the established attitude estimation problem. Section IV evaluates the aforementioned OBA methods comprehensively in terms of convergent speed and steady-state estimate using field test data collected from different grades of SINS. Finally, conclusions are drawn in Section V. II.   S TATIC   OBA   M ETHOD  A.   Formulation of the Static OBA Method  For the OBA method, the attitude kinematics equation is used directly and there is no need to derive the corresponding angle error models. Therefore, the OBA method can be seen as an analytical method. The ingeniousness of the OBA method mainly manifests in the reapplication of the navigation (attitude, velocity and position) rate equations in a new manner. For the traditional application, the attitude kinematics equation in terms of attitude matrix is given by
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 5 n   n   b b   b   nb C   C        (1) where     b   b   nT   n   b   nT   n   n nb   ib   b   in   ib   b   ie   en C   C                              (2) The definitions of the involved reference frames in this paper are the same with those in [8]. n b C   denotes the attitude matrix from the body frame to the navigation frame, determining the values of which prior to the navigational computation is just the purpose of the initial alignment. b ib     denotes the body angular rate measured by gyroscopes in the body frame, n ie     the earth rotation rate expressed in the navigation frame and n en    the angular rate caused by the linear motion of the carrier. Denote by     0 n   the inertially non-rotating frame that is aligned with the navigation frame at 0 t   , by     0 b   the inertially non-rotating frame that is aligned with the body frame at 0 t   , by   n b t   the navigation/body frame at t   . According to the chain rule of the attitude matrix, the attitude matrix   n b C   t   can be decomposed as                   0 0   0  n t   b n   n b   b n   b t  C   t   C   C   C   (3) Since the attitude matrices       0 n n t  C   and       0 b b t  C   can be determined through the attitude update procedure based on the angular rates n in     and b ib     , respectively as                          0   0 0   0  b   b   b ib b t   b t n   n   n in n t   n t  C   C C   C             (4) the heart of determining   n b C   t   is transformed into determining the constant matrix     0 n b C   . According to [6, 8, 10], the calculation of     0 n b C   is virtually a continuous attitude determination problem using infinite vector observations which are constructed based on the specific force equation. The specific force equation expressed in the navigation frame is given by     2 n   n   b   n   n   n   n b   ie   en v   C f   v   g                    (5)
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 6  where n  v   denotes the ground velocity, b  f   the specific force measured by accelerometers in the body frame and n  g   the gravity vector. Substituting (3) into (5) gives                     0 0   0   2  n t   b n   n   b   n   n   n   n b   ie   en n   b t  v   C   C   C   f   v   g                    (6) Multiplying       0 n n t  C   on both sides and recognizing the resulting equation yield                       0   0  0   2  b   n n   b   n   n   n   n   n b   ie   en b t   n t  C   C   f   C   v   v   g                    (7) which can also be rewritten in a compact form as   0 n b   a   a C        (8) with     0 b   b a   b t  C   f       (9a)               0   2  n   n   n   n   n   n a   ie   en n t  C   v   v   g                     (9b) The determination of the constant matrix     0 n b C   in (8) based on the vector observations in (9) is well known as the   Wahba’s problem   [14]. Accordingly, many fruitful algorithms have been developed for such problem, such as the three axis attitude determination (TRIAD) based method,   Davenport’s q method , the quaternion estimator (QUEST) based method and so on. More details about these attitude determination methods can be found in the excellent survey paper [14] and the references therein. In [9], the TRIAD based method is applied to determine the constant matrix     0 n b C   while in [6, 8, 10, 11], the   Davenport’s q method   is applied. Since many effective attitude determination methods are readily to be used, the construction methods for the vector observations are crucial to such problem. Although the vector observations in (9) can be used directly, the severe disturbances inherent in the acceleration information, say b  f   and n  v   , have a much negative effect on the performance of the attitude determination. A straightforward way to attenuate the negative effect of external disturbances is to integrate the acceleration information through certain time interval. According to the
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 7  different integration procedures, there are mainly two different vector observations constructions methods for the developed attitude determination problem, which will be presented detailedly in the following section.  B.   Velocity Integration Formula for Vector Observations Construction  Denote by t   the current time instant during the initial alignment process. In [8], the vector observations in terms of acceleration in (9) are integrated over the time interval     0, t   . The resulting form of the attitude determination problem associated with the vector observations in terms of velocity is given by   0 n b   v   v C        (10) with       0 0  t   b   b v   b  C   f d           (11a)               0 0   2  t   n   n   n   n   n   n v   ie   en n  C   v   v   g   d                         (11b) Similarly to [8, 15], denote the current time instant in its discrete-time form as t   M   t    with t    denoting the time duration of the update interval     1 , k   k t   t      , 0,1, 2,   ,   1 k   M       . Accordingly, the integration of (11) can be calculated using the velocity integration formula developed in [8, 15] as                      1  1   0 0  k   k k   k  M   t   b t b   b v   b t b t   t k  C   C   f   t dt                  (12a)                                               1 0 1  1 0   0 0 1   0 0  k   k k   k k   k k   k  M t   t   n t n   n n   n   n v   ie n   n t n t   t t   k M   t   n t n   n n t n t   t k  C   v   C   C   ω   v dt C   C   g   t dt                                 (12b) The illustration of such integrating procedure is shown in Fig. 1. It can be seen from Fig. 1 that, as the time goes on, the length of the integration time interval is increasing and, in turn, the attenuating effect of the external disturbances will be more remarkable. Moreover, making use of such integration procedure can guarantee the numbers of the constructed vector observations without loss of intermediate GPS data samples. Since the acceleration information of the accelerometer and the GPS at certain time instant can be used
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 8  repeatedly for many times, the convergent speed of the attitude determination is expected to be much faster. The downside of such integration procedure is that, the sensor biases will cause accumulative errors in the constructed vector observations as is shown in (12a). In [8, 10], the OBA method is regarded as a coarse alignment method which needs much shorter time interval, so the accumulative errors have an ignored effect on the navigational or more precise SINS. However, for the low-cost SINS, say MEMS based SINS, the negative effect of these accumulative errors will be much remarkable and even invalidate the OBA method. ...... ...... ...... ...... ...... 0  b  f 1  b  f 2  b  f 1  b  M f    b  M f 0 1 2 1 M    M  Fig. 1. Integration procedure in (11) A straightforward way to attenuate the negative effect of these accumulative errors is to integrate the acceleration based vector observations in (9) over time interval     , m t   t   with     0, m t   t    . The so called interleaved integrating method proposed in [10] is just with such category. In this case, the corresponding attitude determination formulation with the associated vector observations in terms of velocity increment is given by   0 n b   v   v C            (13) with     0  m  t   b   b v   b t   C   f d              (14a)               0   2  m  t   n   n   n   n   n   n v   ie   en n t   C   v   v   g   d                            (14b) Here, the subscript v    is used to denote the velocity increment over time interval     , m t   t   and to sign difference with the velocity denotation v   used in (11). Although the interval of the interleaved integration is set to be the GPS-sample time in [10], it can be extended to a more general form with arbitrary length. We can
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 9  determine the length of the interleaved integration interval based on the consideration of the trade-off between the convergent speed and the attenuating the negative effect of accumulative errors. That is to say, the time interval should be as long as possible to guarantee the convergent speed of the OBA method on one hand, while on the other hand, the time interval should be as short as possible to attenuate the negative effect of accumulative errors. The interleaved integrating procedure can also guarantee the numbers of the constructed vector observations without loss of intermediate GPS data samples, which is just the motivation of the derivation of the interleaved integration in [10]. In [10], there is no explicit integration formula to calculate the integration in (14). However, the velocity integration formula developed in [8, 15] can be readily applied as                      1  1   0   k   k k   k  M   t   b t b   b v   b t b t   t k   m  C   C   f   t dt                   (15a)                                               1 1  1 0   0 1   0  k   k k   k m k   k k   k  M t   t   n t n   n n   n   n v   ie n   n t n t   t t   k   m M   t   n t n   n n t n t   t k   m  C   v   C   C   ω   v dt C   C   g   t dt                                  (15b) where m t   m t       . The illustration of the interleaved integration procedure is shown in Fig. 2. ......   ......   ...... ......   ......   ...... k 2 k    k   m  2 k   m     0 0  b  f b  k f 2  b  k f    b  k   m f    2  b  k   m f        Sliding Fixed-interval integration  Fig. 2. Integration procedure in (14) It should be noted that all the existing OBA methods are all unable to estimate anything other than the attitude. Since for the low-cost SINS the sensors biases have much negative effect on the attitude calculation, it is desired to extend existing OBA methods and make them be able to estimate the sensors biases coupled with the attitude for the initial alignment problem. With this consideration, a dynamic OBA method is
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 10  developed in this paper, which is used to estimate the attitude at current time rather than the constant attitude at the very start of the initial alignment. III.   D YNAMIC   OBA   M ETHOD  A.   Alternative Perspective on Interleaved Integration  Before getting into the development of the dynamic OBA method, we first give an alternative perspective on the interleaved integrating procedure, which will facilitate the derivation of the developed dynamic OBA method. Specifically, according to the chain rule of the attitude matrix, we have                                                 0   0 0   0  m m m m  b t b   b b t   b t b t n t n   n n t   n t n t  C   C   C C   C   C     (16) Substituting (16) into (7) and integrating the resulting equation over time interval     , m t   t   yield                                                      0 0  0 2  m m m m m m  t   b t b n   b b   b b t t t   n t n   n   n   n   n   n ie   en n n t t  C   C   C   f d C   C   v   v   g   d                            (17) For the integrating operator d     , the time instant m t   is constant and so as the corresponding attitude matrices          0  m  b b t  C   and          0  m  n n t  C   . To this respect, (17) can be rewritten as                                                      0 0  0 2  m m   m m m   m  t   b t b n   b b   b b t   t t   n t n   n   n   n   n   n ie   en n n t   t  C   C   C   f d C   C   v   v   g   d                            (18) Multiplying           0  m n t n  C   on both sides and incorporating the corresponding attitude matrices yield                                    2  m m m m  t   b t n   b b   m   b t t   n t   n   n   n   n   n ie   en n t  C   t   C   f d C   v   v   g   d                            (19) where
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 11                           0 0   0 m m  n t   b n   n b   m   b n   b t  C   t   C   C   C   (20) Eq. (19) can also be written in the form of attitude determination problem as     n b   m   v   v C   t            (21) with        m m  t   b t   b v   b t   C   f d              (22a)                  2 m m  t   n t   n   n   n   n   n v   ie   en n t   C   v   v   g   d                            (22b) The relationship between (14) and (22) is given as                     0 0  m m  b v   v b t n v   v n t  C C                   (23) Regarding (21),     n b   m C   t   can be viewed as a   “ dynamic   constant”   matrix. Here, the term   “constant”   in  “ dynamic   constant”   means that the attitude matrix     n b   m C   t   at time instant m t   is regarded as a constant for the vector observations in following time interval     , m t   t   . In this respect, the determination of     n b   m C   t   based on (21) shares the same procedure as that of     0 n b C   based on (10) and (13). On the other hand, the term  “ dynamic ”   in   “ dynamic   constant”   means that the attitude matrix     n b   m C   t   is variational on the whole time interval of the initial alignment as the time instant m t   is changed from 0   to t   . The illustration of the integration procedure in (22) is shown in Fig. 3. All the attitude matrices at the marked time instant are just the aforementioned   “ dynamic   constant”   matrices. The navigation and body frames at time instant m t   , i.e.     m n t   and     m b t   , can also be viewed as inertial frames for the corresponding frames at the following time interval     , m t   t   just the same as the     0 n   and     0 b  for the corresponding frames at time interval     0, t   . Since the time instant m t   is changed from 0   to t   , the corresponding frames     m n t   and     m b t   are only the   “temporary   inertial ”   frames. In the following section, the
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 12  concept of   “te mporary inertial ”   frames will also be used to derive the dynamic OBA method. ......   ...... ...... ......   ...... ...... 0  b  f 1  b  f b  m f 0 1 m  ...... ...... ...... ...... ...... ...... 0  b  f 1  b  f b  m f 0 1 m  ...... ...... 1 m    1  b  m f     ...... ...... 0  b  f 1  b  f b  k   m f    0 1 k   m   ...... ...... 1 k   m     1  b  k   m f        ...... k 1 k    b  k f 1  b  k f     ...... ......  Fig. 3. Integration procedure in (22) After the   “ dynamic   constant”   matrix     n b   m C   t   being derived, the attitude matrix at the current time instant can be readily obtained as                           m m  b t n t n   n b   b   m   b t n t  C   t   C   C   t   C   (24) where                                                  0 0 0 0  m   m m   m  b t   b t   b b t   b   b t n t   n t   n n n t   n t  C   C   C C   C   C     (25) All the attitude matrices at the right hand of (25) can be calculated according to (4). It should be noted that the attitude determination procedure (21)-(25) is not a new OBA method and it is the same as that of the interleaved integration based method.  B.   Dynamic Attitude Estimation Model  In this section part, we will develop the dynamic OBA method with consideration of estimating the sensors
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 13  biases. The derivation of such method is based on the aforementioned concepts   “ dynamic   constant”   matrix and   “temporary   inertial ”   frames. For the alternative perspective on the interleaved integration, the frames at the time instant m t   are treated as inertial for the corresponding frames at the following time interval     , m t   t   , while in this section, the frames at time instant t   are treated as inertial for the corresponding frames at the previous time interval     , m t   t   . Specifically for any time instant     , m t   t      , according to the chain rule of the attitude matrix, we have                   n   b t n   n b   b n t   b  C   C   C   t C            (26) Substituting (26) into (5) yields                     2  n   b t n   n   b   n   n   n   n b   ie   en n t   b  v   C   C   t C   f   v   g                         (27) It should be noted that all the quantities above are functions of time    and their time dependences on are omitted for brevity. Multiplying       n t n  C     on both sides of (27) and reorganizing the resulting equation yield                       2  b t   n t n   b   n   n   n   n   n b   ie   en b   n  C   t C   f   C   v   v   g                       (28) Integrating (28) on both sides over the time interval     , m t   t                           2  m m  t   b t n   b b   b t t   n t   n   n   n   n   n ie   en n t  C   t C   f d C   v   v   g   d                            (29) For the integrating operator d     ,the time instant t   is constant and so as the corresponding   n b C   t   . To this respect, (29) can be rewritten as   n b   dv   dv C   t        (30) with      m  t   b t   b dv   b t   C   f d           (31a)               2  m  t   n t   n   n   n   n   n dv   ie   en n t   C   v   v   g   d                         (31b)
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 14  It is shown that we have made use of the similar   “temporary   inertial ”   concept in going from (17) to (18) and (29) to (30). The reference frame   n b C   t   corresponding to the   “temporary   inertial ”   frames can also be viewed as a   “ dynamic   constant”   matrix. In last section, the constant characteristic of the   “ dynamic   constant”  matrix     n b   m C   t   is used for the static attitude determination. In contrast, the dynamic characteristic of the  “ dynamic   constant”   matrix   n b C   t   will be used for the following dynamic attitude estimation. When the attitude matrix   n b C   t   is treated as a dynamic value, the gyroscopes biases can be easily incorporated into the dynamic attitude update model. Considering the contaminated bias and noise, the direct output of the gyroscopes   b ib   t     is modeled as              b   b   b ib   ib   gv b gu  t   t   t t                         (32) where b     is the gyro drift derived from a random walk process. gv     and gu     are independent zero-mean Gaussian white-noise processes with spectral densities. Reorganizing (32) and substituting the result into (1) give the following attitude model                           T n   n   b   b   n   n b   b   ib   b   in   gv C   t   C   t   t   t   C   t   t                        (33) Eq. (33) and (30) constitute the typical dynamic attitude estimation model with (33) being the process model and (30) the measurement model. With the developed dynamic model, many existing dynamic attitude estimation methods known as filtering methods can be readily applied. Before getting involved in explicit details of filtering algorithms that will be presented in next section, we first extend the velocity integration formula developed in [8, 15] to solve the integration (31). The velocity integration formula developed in [8, 15] can not be applied in (31) in its current form. Next, we will show how the integration in (31) can be calculated using the velocity integration formula. The integration in (31a) can be expressed as
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 15                     0   0 0   0  m   m   m  t   t   t b t   b t   b   b t   b b   b   b b   b   b   b   b t   t   t  C   f d   C   C   f d   C   C   f d                     (34) As it is shown, the integration on the right hand of (34) is the same as that in (14a). Therefore, the integration in (31a) can be calculated as                             1  1   0 0  k   k k   k  M   t   b t b t   b   b dv   b   b t b t   t k   m  C   C   C   f   t dt                  (35) The first part of the integration (31b) can be developed as                                              m   m m m   m  t t   t n t   n t   n t n   n   n   n in n   n   n t   t t t n t   n t n   n   n   n m   in n n t   t  C   v d   C   v   C   v d v   t   C   v   t   C   v d                                        (36) Substituting (36) into (31b) yields                                m m   m  n t n   n dv   m n t t   t n t   n t n   n   n ie n   n t   t  v   t   C   v   t C   v d   C   g d                             (37) Similar to the procedure in (34), the first integration on the right hand of (37) can be expressed and calculated as                                            1  0 0 1   0 0  m   m k   k k   k  t   t n t   n t   n n   n   n   n ie   ie n   n   n t   t M   t   n t n t   n   n   n ie n   n t n t   t k   m  C   v d   C   C   v d C   C   C   ω   v dt                                        (38) The second integration on the right hand of (37) can be expressed and calculated as                                                 1  0 0 1   0 0  m   m k   k k   k  t   t n t   n t   n n   n n   n   n t   t M   t   n t n t   n   n n   n t n t   t k   m  C   g d   C   C   g d C   C   C   g   t dt                         (39) Substituting (38) and (39) into the right side of (37) yields                                                               1   1  1   1 0   0 0  m k   k k   k k   k k   k  n t n   n dv   m n t M   M t   t n t   n t n t   n   n n   n   n ie n   n t   n t n t   n t t   t k   m   k   m  v   t   C   v   t C   C   C   ω   v dt   C   C   g   t dt                                                   (40)
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 16  The illustration of the developed dynamic attitude determination problem and the associated vector observations (31) are shown in Fig. 4. In Fig. 4, k x   is the state of the dynamic model, which includes attitude and gyroscopes biases. It should be noted that, although the vector observations are constructed based on the interleaved integration procedure, they can also be constructed based on the integration procedure in (11). Let 0 m t     , the integration procedure in (31) is just the same as that in (11). The properties of the two integration procedures discussed in the last section for the OBA method are also suitable for the dynamic attitude estimation problem. k x 1 k x    2 k x    2 k y    1 k y    k y ......   ......   ...... ......   ......   ...... 2 k   m     k   m  2 k    k 2  b  k   m f       b  k   m f    2  b  k f    b  k f  Fig. 4. Integration procedure in (31) for the dynamic model (32)  C.   Modified UnScented QUaternion Estimator  Since the attitude is usually parameterized in terms of quaternion for on-computer application, most of the existing filtering methods for attitude estimation are devoted to handle the quaternion normalization constraint in the filtering recursion, such as the multiplicative extended Kalman filter [14], the UnScented QUaternion Estimator (USQUE) [16, 17] and its modified version (MUSQUE) [18, 19]. In this paper, the MUSQUE is used. Considering clarity and completeness, the explicit filtering procedure of MUSQUE for the investigated dynamic attitude estimation problem is presented in this section part. Parameterizing the attitude in terms of quaternion, the attitude kinematics equation (33) in its discrete-time form is given by
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 17     1   1 ,   ,   , b   n k   k   k   ib   in q   ff   q               (41) Eq. (41) is a general form of the discrete-time attitude kinematics equation in terms of quaternion. The discrete-time attitude kinematics equation is virtually the on-computer calculation procedure of the attitude. Various fruitful algorithms have been developed for the on-computer attitude calculation, say single sample algorithm, double sample algorithm and so on. We can select the appropriate one based on the consideration of the algorithm precision and complexity. Since this paper focuses on the initial alignment method development, the explicit procedures of these calculation algorithms are not presented and only a general form as shown in (41) is presented. The MUSQUE is virtually an extension of the unscented Kalman filter (UKF) [20, 21] with consideration of the quaternion norm constraint. Since the norm constraint of the quaternion can be easily destroyed by the weighted-mean computation inherent in the UKF, the attitude quaternion can not be used directly as the filtering state. In order to overcome this problem the MUSQUE uses the quaternion for global nonsingular attitude   representation   and   unconstrained   generalized   Rodrigues   parameter   (GRP)   for   local   attitude representation and filtering. Given the error quaternion 4  T T  q   q                    , where    is the vector part of the quaternion q     , the corresponding GRP representation p     is given by 4  p   f a   q            (42) where a   is a parameter from 0 to 1, and f   is a scale factor. In the MUSQUE, the state is selected as     ; k   k   k x         . Given the state estimate 1  ˆ k x      at time instant 1 k      , the corresponding state covariance 1 k P      and the quaternion estimate 1  ˆ k q      , the aim of MUSQUE is to determine the state estimate ˆ k x   and quaternion estimate ˆ k q   at time instant k   . The explicit filtering procedure is given as follows  Time Update
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 18  Generate the sigma points as     1 1   1   1 1  ˆ  p k k   k   k k  x   n   P                                           (43) where    is a tune parameter which is usually set to 3   n    in the UKF to capture some higher order information of the distribution with n   denoting the state dimension [19]. The   i -th component of 1  p k          is denoted as   1  p k   i          and its quaternion form is denoted as           1   1   4,   1  T T k   k   k q   i   i   q   i                          which can be calculated according to the reverse form of (42) as                    2   2 2   2 1   1 4,   1   2 2 1  1 p   p k   k k   p k  a   i   f   f   a   i q   i f   i                               (44a)           1 1   4,   1   1  p k   k   k i   f   a   q   i   i                            (44b) The corresponding quaternion based sigma points   1  q k   i        are given as       1   1   1  ˆ q k   k   k i   q   i   q                  (45) Propagate the quaternion and gyroscopes bias based sigma points through the process model (41)               1   1 1   ,   ,   , q   q   b   n k   k   ib   in k k   i   ff   i   i                        (46) Since the gyroscopes bias is viewed as constant, the propagated gyroscopes bias based sigma points are given as       1 1   k k k   i   i               (47) The prediction of the attitude quaternion is given by the quaternion averaging algorithm as [22] 3 1  ˆ   arg max   T k k   q   S  q   q Aq        (48) where        2 1   1 1  1 2  n   T q   q k k   k k i  A   i   i n                  (49)
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 19  The predicted quaternion error based sigma points are given by         1 1   1   1  ˆ q k k   k k   k k  q   i   i   q                            (50) Denote           1   1   4,   1  ˆ   T T k k   k k   k k  q   i   i   q   i                        , the corresponding predicted GRP based sigma points are given by          1 1 4,   1  k k p k k k k  i i   f a   q   i                 (51) Denote 1   1   1  T pT   T k k   k k   k k                               , the predicted mean and covariance of the state are given by    2 1   1 1  1  ˆ   2  n k k   k k i  x   i n              (52a)        2 1 1   1   1   1   1 1  1   ˆ   ˆ  2  n   T k k k   k k   k k   k k   k k i  P   i   x   i   x   Q n                                                  (52b) where 1 k Q      is the covariance of the process noise.  Measurement Update  Regenerate the sigma points as      * 1 * 1   1   1 * 1  ˆ  p k k k k   k k   k k k k  x   n   P                                               (53) Denote the quaternion form of   * 1  p k k   i          as           *   *   * 1   1   4,   1  T T k k   k k   k k  q   i   i   q   i                       which can be calculated according to the reverse form of (42) as                    2   2 *   2   2   * 1   1 * 4,   1   2 2   * 1  1 p   p k k   k k k k   p k k  a   i   f   f   a   i q   i f   i                               (54a)           *   1   *   * 1   4,   1   1  p k k   k k   k k  i   f   a   q   i   i                             (54b)
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 20  The quaternion based sigma points corresponding to   * 1 k k  q   i        is given by       *   * 1   1   1  ˆ q k k   k k   k k  i   q   i   q                 (55) Propagate the quaternion based sigma points through the measurement model (30) as           * , 1  q k   dv k k k  i   C   i             (56) where       * 1  q k k  C   i        denotes the attitude matrix corresponding the quaternion   * 1  q k k   i        . The mean and covariance of the measurement are given by    2 1  1  ˆ   2  n k   k i  y   i n             (57a)        2 , 1  1   ˆ   ˆ  2  n   T y k   k   k   k   k   k i  P   i   y   i   y   R n                                      (57b) where k R   is the covariance of the measurement noise. The cross-correlation covariance of the state and measurement is given by        2 ,   1   1 1  1   ˆ   ˆ  2  n   T xy k   k   k k k   k k i  P   i   x   i   y n                                     (57c) The state estimate and corresponding covariance are given by     , 1  ˆ   ˆ   ˆ k   k   dv k   k k k  x   x   K   y             (58a) , 1  T k   k   y k   k k k  P   P   K P   K        (58b) where       1 ,   , k   xy k   y k K   P   P       (58c)  Attitude Update  Denote ˆ ˆ   ˆ   T T   T k   k   k x   p                 . The quaternion corresponding to ˆ k p     is denoted as 4,  ˆ   T T k   k   k q   q                    which can be calculated as
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 21     2   2 2   2 4,   2 2  ˆ   ˆ 1  ˆ  k   k k k  a   p   f   f   a   p q   f   p                     (59a)   1 4,   ˆ k   k   k f   a   q   i   p                     (59b) The quaternion estimate is given by 1  ˆ   ˆ   ˆ k   k   k k  q   q   q            (60) Reset the first three elements (GRP) of ˆ k x   to zero and go to the next filtering cycle. IV.   PERFORMANCE   EVALUATION   THROUGH   FIELD   TEST  In this section, the vector observations construction procedures for both the static and dynamic OBA methods are evaluated using field test data. Specifically, the following four initial alignment methods are evaluated:  q method: full integration q method: partial integration filter: full integration filter: partial integration  The term   “ full integration ”   means the integration procedure in (11) and   “ partial integration ”   the interleaved integration procedure in (14) or (31). The term   “ q method ”   is corresponding to the OBA method as the  Davenport’s q method   is used to calculate the constant attitude matrix. The term   “ filter ”   is corresponding to the dynamic OBA method as the MUSQUE algorithm is used to calculate the dynamic attitude matrix. In the partial integration procedure, 100 interleaved samples are used, i.e. 100 m t   t   t          for (14) and (31). As is discussed in the last section, the two different integration procedures mainly affect the convergent speed and steady-state estimate of the corresponding attitude determination methods. In this section, we will evaluate the two characteristics of the aforementioned four initial alignment methods using car-mounted field test data collected on SINS with different levels of precision. One is the navigation-grade SINS equipped with
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 22  a triad of ring laser gyroscopes and accelerometers. The other is the low-grade SINS equipped with a triad of MEMS gyroscopes and accelerometers. The specifications of the navigation-grade and low-grade inertial sensors are listed in Tables I and II, respectively.  TABLE I ． SPECIFICATIONS OF THE NAVIGATION-GRADE INERTIAL SENSORS  Gyro   Accelerometer  Dynamic  range 400   s  3 g  Update rate   100Hz   100Hz  Bias 0.05   h  50   g     Bias st a bility 0.01   h  50   g     TABLE II ．   SPECIFICATIONS OF THE LOW-GRADE INERTIAL SENSORS  Gyro   Accelerometer  Dynamic  range 150   s  10 g  Update rate   100Hz   100Hz  Bias 0.5   s  0.005 g   Bias st a bility 0.02   s  0.001 g   There is also a GPS antenna installed outside the cabin on the top of the car and the SINS/GPS integrated navigation result is used as the reference data to compare with. The raw measurements (1   Hz ) of GPS were linearly interpolated to obtain the velocity and position at both ends of the update interval.  A.   Testing Results Using the Navigation-grade SINS  According to [8], the OBA method can reach a satisfactory performance for the navigation-grade SINS after only 100s, therefore, we also make use of 100s collected data to test the involved initial alignment methods in this paper. The testing results of the four initial alignment methods using the navigation-grade SINS data are presented in Fig. 5-10, respectively. Fig. 5 and 6 plot the pitch angles estimates and estimate error, Fig. 7 and 8 the roll angles, Fig. 9 and 10 the yaw angles, respectively. The stead-state attitude error
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 23  summary of the four initial alignment methods is listed in Table III for comparison. It is shown that the full integration procedure can reach a much faster convergent speed and better stead-state estimate performance than the partial integration procedure. This is because that in the full integration procedure the sampling data collected from the inertial sensors is used more times and, in turn, more adequately than that in the partial integration procedure. The improvement in terms of convergent speed and steady-state precision by the full integration procedure is guaranteed by the high-grade specifications of the navigation-grade inertial sensors. That is to say, making use of the sampling data collected from the navigation-grade inertial sensors repeatedly will not cause accumulative errors. Therefore, for the OBA of the high-grade SINS, the full integration procedure will be more celebrated. Moreover, with the full integration procedure the dynamic method developed in this paper is a little better than the static method. However, the computational cost of the dynamic method is much larger than that of the static method. All the initial alignment methods are implemented using Matlab on a computer with a 2.66G CPU, 2.0G memory and the Windows7 operating system. In our experiment, the cost time of processing the 100 s   data is about 52 s   for dynamic OBA method with full integration and 8 s   for static OBA method with full integration. If the OBA methods are only regarded as coarse alignment methods which are used to provide rough attitude for the following fine alignment, the static OBA method will be more celebrated due to its less computational cost. The proposed dynamic OBA method shows no much improvement over the traditional method is mainly due to the inability of the proposed method to estimate the accelerometer biases. The accelerometer biases mainly determine the ultimate precision of the initial alignment. If the negative effect of the gyroscopes biases is not so obvious as for the navigation-grade SINS, the steady-state alignment precision is mainly determined by the accelerometer biases. Since both the static and dynamic OBA methods are unable to estimate the accelerometer biases, their performance is similar to each other in this case. Advanced method that is able to estimate the accelerometer biases can be expected to further improve the alignment performance of the navigation-grade SINS.
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 24 0   20   40   60   80   100 -8 -6 -4 -2 0 2 4 Pitch / deg  t   / s  True q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 5. Pitch angles estimates 0   20   40   60   80   100 -30 -25 -20 -15 -10 -5 0 5 Pitch / arcmin  t   / s  q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 6. Pitch angles estimate error
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 25 0   20   40   60   80   100 -4 -3 -2 -1 0 1 2 3 4 Roll / deg  t   / s  True q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 7. Roll angles estimates 0   20   40   60   80   100 -5 0 5 10 15 Roll / arcmin  t   / s  q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 8. Roll angles estimate error
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 26 0   20   40   60   80   100 40 60 80 100 120 140 160 Yaw / deg  t   / s  True q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 9. Yaw angles estimates 0   20   40   60   80   100 -50 0 50 100 150 200 250 300 350 400 450 Yaw / arcmin  t   / s  q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 10. Yaw angles estimate error  Table III. ATTITUDE ERROR SUMMARY USING NAVIGATION-GRADE INERTIAL SENSORS (unit: arcmin)  Pitch   Roll   Yaw  q method: full integr a tion   - 0.18   0.62   14.41  q method:   partial   integr a tion   - 0.23   1.92   21.15  filter : full int e gration   - 0.07   0.48   5.88
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 27  filter :   p artial   i n tegration   - 2.51   0.99   104.4  The main advantage of the developed dynamic method is its ability to estimate the gyroscopes biases coupled with the attitude. The gyroscopes biases estimate results with different integration procedures are presented in Fig. 11 and 12, respectively. Meanwhile, the gyroscopes biases estimate results using the SINS/GPS integration are also plotted in Fig. 11 and 12 to compare with. For the SINS/GPS integration, the initial gyroscopes biases are assumed to be zero, which is the same as that in the dynamic OBA methods. However, all the attitude, velocity and position are properly initialized. That is to say, the SINS/GPS integration is actually carried out after the attitude alignment. As it is shown, the gyroscopes biases estimate results using the full integration procedure are more close to the results of the SINS/GPS integration than that of the partial integration procedure. However, the results of all the three estimate procedure are not so well. This is because that for the high-grade SINS, the observability of the biases errors is feeble and much longer time is needed to converge the estimate results [6, 7]. 0   20   40   60   80   100 -0.1 -0.05 0 0.05 0.1 0.15 0.2 0.25 0.3  t   / s    /    /h  SINS/GPS X SINS/GPS Y SINS/GPS Z  partial X partial Y partial Z  Fig. 11. Gyroscopes biases estimates of the   “ filter: partial integration ”   and SINS/GPS
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 28 0   20   40   60   80   100 -0.05 0 0.05 0.1 0.15 0.2 0.25 0.3  t   / s    /    /h  SINS/GPS X SINS/GPS Y SINS/GPS Z  full X full Y full Z  Fig. 12. Gyroscopes biases estimates of the   “ filter:full integration ”   and SINS/GPS  B.   Testing Results Using the Low-grade SINS  In this test, the car with low-grade SINS was moving in a large circle around the sports grounds with a good sky view of the GPS antennas. Two circles of test data with different velocities are collected to evaluate the alignment methods. In the first circle, 250s were taken for the car to make one circuit around the sports grounds. The testing results of the four initial alignment methods using the first data segment are presented in Fig. 13-18, respectively. Fig. 13 and 14 plot the pitch angles estimate and estimate error, Fig. 15 and 16 the roll angles, Fig. 17 and 18 the yaw angles, respectively. The stead-state attitude error summary of the four initial alignment methods is listed in Table IV for comparison. It is shown that both the two static OBA methods are no longer applicable. This is because that the gyroscopes biases have a much negative effect on the accuracy of the attitude and the static OBA methods are unable to estimate the biases during the attitude alignment. For the dynamic OBA methods developed in this paper, the partial integration procedure is effective while the full integration procedure is noneffective just as the static methods. This is because that the inertial sensors biases will cumulate in the constructed vector observations and when the integrating interval increases with time just as the full integration procedure the cumulated errors will be larger and larger, resulting in much degraded vector observations and, in turn, much degraded attitude estimate. The
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 29  effectiveness of the dynamic OBA method with partial integration procedure lies in its ability to estimate the gyroscopes biases. The gyroscopes biases estimate results with partial integration procedure along with the results of the SINS/GPS integration are presented in Fig. 19. It is shown that biases estimate results of the dynamic OBA method with partial integration procedure are comparable with that of the SINS/GPS integration. For comparison, the gyroscopes biases estimate results with full integration procedure along with the results of the SINS/GPS integration are presented in Fig. 20. It is shown that biases estimate results of the dynamic OBA method with full integration procedure are much degraded. Therefore, the dynamic OBA method with partial integration procedure is most celebrated for the low-grade SINS. It is shown that the estimate of the gyroscopes bias in the vertical direction has a much slower convergence speed than that of the gyroscopes biases. This is mainly due to the different degrees of observability. For more thorough details about the observability of the initial alignment, the reader is referred to the systematical and comprehensive study [7] for this problem. Although it appears that the test time is a little short to obtain a clear result for the vertical gyroscopes bias, it does not affect the demonstration of the validity of the proposed dynamic OBA method. This is because that the performance of the proposed dynamic OBA method in estimating the gyroscopes biases is comparable with that of the SINS/GPS integration which is the accredited method in estimating the sensor biases. However, it should be noted that the SINS/GPS integration is founded on the linearized system error models and necessitates the initial alignment to provide roughly known initial attitude to guarantee the validity of such linearization. If the attitude can not be aligned appropriately, the performance of the SINS/GPS integration will be much degraded. Therefore, the investigated initial alignment method is still necessary in practical application. It can be seen in Fig. 13-18 and Tab. IV that the dynamic OBA method with partial integration procedure can align the attitude quite well. Therefore, the subsequent stage of SINS/GPS integration can be readily carried out after the dynamic OBA. Meanwhile, the attitude alignment time should not be too long due to some special military requirements in terms of maneuverability. In this respect, the test time is appropriate for the attitude alignment. The calibration of the
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 30  sensor biases can be further carried out in the subsequent SINS/GPS integration stage. 0   50   100   150   200   250 -8 -7 -6 -5 -4 -3 -2 -1 0 1 2 Pitch / deg  t   / s  True q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 13. Pitch angles estimates (first circle) 0   50   100   150   200   250 -2 -1 0 1 2 3 4 5 6 Pitch / deg  t   / s  q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 14. Pitch angles estimate error (first circle)
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 31 0   50   100   150   200   250 -6 -5 -4 -3 -2 -1 0 1 2 Roll / deg  t   / s  True q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 15. Roll angles estimates (first circle) 0   50   100   150   200   250 -4 -3 -2 -1 0 1 2 3 4 5 Roll / deg  t   / s  q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 16. Roll angles estimate error (first circle)
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 32 0   50   100   150   200   250 0 50 100 150 200 250 300 350 400 Yaw / deg  t   / s  True q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 17. Yaw angles estimates (first circle) 0   50   100   150   200   250 -200 -150 -100 -50 0 50 100 150 200 250 Yaw / deg  t   / s  q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 18. Yaw angles estimate error (first circle)  Table IV. ATTITUDE ERROR SUMMARY USING LOW-GRADE INERTIAL SENSORS (first circle, unit: deg)  Pitch   Roll   Yaw  q method: full integr a tion   1.77   2.97   207.8 1  q method:   partial  integr a tion  2.42   2.99   - 57.58  filter : full int e gration   2.87   2.35   29.69
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 33  filter :   partial   i n tegration   0.11   - 0.18   - 0.2 4 0   50   100   150   200   250 -1000 -800 -600 -400 -200 0 200 400 600 800 1000  t   / s    /    /h  SINS/GPS X SINS/GPS Y SINS/GPS Z  partial X partial Y partial Z  Fig. 19. Gyroscopes biases estimates of the   “ filter: partial integration ”   and SINS/GPS (first circle) 0   50   100   150   200   250 -2000 -1500 -1000 -500 0 500 1000 1500  t   / s    /    /h  SINS/GPS X SINS/GPS Y SINS/GPS Z  full X full Y full Z  Fig. 20. Gyroscopes biases estimates of the   “ filter: full integration ”   and SINS/GPS (first circle) In the second circle, 150s were taken for the car to make one circuit around the sports grounds. The testing results of the four initial alignment methods using the second data segment are presented in Fig. 21-26, respectively. Fig. 21 and 22 plot the pitch angles estimate and estimate error, Fig. 23 and 24 the roll angles, Fig. 25 and 26 the yaw angles, respectively. The stead-state attitude error summary of the four initial alignment methods is listed in Table V for comparison. The gyroscopes biases estimate results of the dynamic
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 34  OBA method with partial integration procedure along with the results of the SINS/GPS integration are presented in Fig. 27. The gyroscopes biases estimate results of the dynamic OBA method with full integration procedure along with the results of the SINS/GPS integration are presented in Fig. 28. We can draw the same conclusions from these results with those from the last experimental results. It should be noted that the gyroscopes biases in the two experiments are not the same with each other. This is because that the gyroscopes biases are always not the same after each application of power, which is known as the bias instability. Admittedly, this observation suggests that the gyroscopes biases should be estimated in each application, which can further give prominence to the significance of the proposed dynamic OBA method for the attitude alignment of the low-grade SINS. 0   50   100   150 -5 -4 -3 -2 -1 0 1 2 3 4 Pitch / deg  t   / s  True q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 21. Pitch angles estimates (second circle)
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 35 0   50   100   150 -4 -3 -2 -1 0 1 2 Pitch / deg  t   / s  q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 22. Pitch angles estimate error (second circle) 0   50   100   150 -2 -1 0 1 2 3 4 5 Roll / deg  t   / s  True q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 23. Roll angles estimates (second circle)
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 36 0   50   100   150 -4 -3.5 -3 -2.5 -2 -1.5 -1 -0.5 0 0.5 Roll / deg  t   / s  q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 24. Roll angles estimate error (second circle) 0   50   100   150 0 50 100 150 200 250 300 350 400 Yaw / deg  t   / s  True q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 25. Yaw angles estimates (second circle)
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 37 0   50   100   150 -150 -100 -50 0 50 100 150 200 250 300 350 Yaw / deg  t   / s  q method: full integration q method: partial integration filter: full integration filter: partial integration  Fig. 26. Yaw angles estimate error (second circle)  Table V. ATTITUDE ERROR SUMMARY USING LOW-GRADE INERTIAL SENSORS (second circle, unit: deg)  Pitch   Roll   Yaw  q method: full int e gration   - 2.81   - 0.38   - 124 . 41  q method:   partial   integr a tion   - 2. 5 2   - 1.13   - 32.75  filter : full integr a tion   - 2. 4 7   - 1.01   - 51.71  filter :   partial   integr a tion   0. 07   0.18   - 1 . 47 0   50   100   150 -2500 -2000 -1500 -1000 -500 0 500 1000  t   / s    /    /h  SINS/GPS X SINS/GPS Y SINS/GPS Z  partial X partial Y partial Z  Fig. 27. Gyroscopes biases estimates of the   “ filter: partial integration ”   and SINS/GPS (second circle)
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 38 0   50   100   150 -2000 -1500 -1000 -500 0 500 1000 1500 2000  t   / s    /    /h  SINS/GPS X SINS/GPS Y SINS/GPS Z  full X full Y full Z  Fig. 28. Gyroscopes biases estimates of the   “ filter: full integration ”   and SINS/GPS (second circle) It should be noted that the 100 interleaved samples used in the partial integration is only a special case. The length of integration time interval has a remarkable effect on the performance of the developed dynamic OBA method. For the navigation-grade SINS, making use of the sampling data more adequately can reach a better performance in terms of convergent speed and steady-state precision. For the low-grade SINS, however, the situation is a little complex. On the one hand, too less sampling data incorporated into the integration can not guarantee the appropriate convergent speed. On the other hand, too more sampling data incorporated into the integration may cause accumulative error due to the inherent inertial sensors ’   error. In this respect, it is desired to determine the length of integration time interval under different conditions. However, there is no universal method to achieve such end and it can only be determined through experiment study. Regarding the utilizing procedure of the sampling data over certain time intervals, the investigated method has a reasonable similarity to the finite impulse response (FIR) filter. Recently, Shmaliy and Simon have investigated unbiased FIR filter for the state-space model based problems and presented a unified forms for Kalman and FIR filtering [24, 25]. In [24], it is pointed out that the optimal window size can be easily estimated experimentally by minimizing an elaborate cost function without any   “ true ”   reference. The similarity between the Kalman and FIR filtering motives us to determine the length of integration time interval according to the proposed method in [24],
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 39  which is the further research regarding the investigated dynamic OBA method. V.   C ONCLUSION  The newly developed OBA method has been proven to be an ingenious method for the SINS initial alignment under swaying or maneuvering conditions. Typically, there are mainly two integration procedures for the construction of the vector observations used in the OBA methods. However, the conflicting properties of the two integration procedures necessitate a further investigation of the OBA methods. Meanwhile, the existing OBA methods are all unable to estimate anything other than the attitude and can be viewed as static methods, which stultifies them for the low-grade SINS. Based on the construction process of the static OBA methods, a dynamic OBA method is developed to estimate the gyroscopes biases coupled with the attitude, which is expected to be applicable for the low-grade SINS. The existing static OBA methods and the developed dynamic OBA method with different integration procedures are comprehensively evaluated in terms of convergent speed and steady-state estimate using field test data from both navigation-grade and low-grade SINS. The comparison analysis shows that for the navigation-grade SINS the static OBA method is most celebrated due to its less computational cost and appropriate steady-state estimate precision. In this case, making use of more acceleration information can result in a much faster convergent speed with no degradation in steady-state estimate due to the high-grade specifications of the navigation-grade inertial sensors. For the low-grade SINS, the static OBA methods are no longer applicable due to the accumulative errors in the constructed vector observations caused by the large inertial sensors biases. In this case, the developed dynamic OBA method with interleaved integration procedure is most celebrated since it can estimate the gyroscopes biases appropriately and thus result in a much better attitude alignment performance. Making use of more acceleration information from the low-grade inertial sensors will cause much accumulative errors in the constructed vector observations, which can even stultify the dynamic OBA method. A disadvantage of the proposed algorithm is that it can not estimate the accelerometer biases. Quite recently,
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 40  Wu et al. [23] propose an online constrained-optimization method to simultaneously estimate the attitude and the inertial sensor biases. This online constrained optimization method performs quite well in estimating the attitude and the accelerometer biases. However, its performance of estimating the gyroscopes biases is compromised due to the severe noise and disturbance. Therefore, this online constrained optimization method is confined to those accurate applications with navigation-grade SINS measurements [23]. In this respect,   the   online   constrained   optimization   method   and   the   proposed   dynamic   OBA   method   is complementary to each other in joint estimation of the gyroscopes/ accelerometer biases. In the future work, we will try to handle the gyroscopes/ accelerometer biases simultaneously by combining the virtue of the online constrained-optimization method and the proposed dynamic OBA method. A CKNOWLEDGMENT  The authors would like to thank Gongmin Yan, and Zongwei Wu for providing the field test data. R 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. L.   Crassidis, “Sigma -Point Kalman Filtering for Integrated GPS   and Inertial Navigation,”   IEEE Transactions on Aerospace and Electronic Systems , vol. 42, no. 2, pp. 750 – 756, 2006. [4]   Y. F. Jiang, “Error analysis of analytic coarse alignment methods,”   IEEE Transactions on Aerospace and Electronic Systems , vol. 34, no. 1, pp. 334 – 337, Jan. 1998. [5]   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,
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 41  1996. [6]   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. [7]   Y. X. Wu, H. L. Zhang, M. P. Wu, X. P. Hu, and   D. P. Hu, “Observability of SINS alignment: A global perspective,”   IEEE Transactions on Aerospace and Electronic Systems , vol. 48, no. 1, pp. 78 – 102, 2012. [8]   Y. Wu, and X. 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 [9]   D. Q. Gu, N. El-Sheimy, T. Hassan and Z.   Syed, “Coarse alignment   for marine SINS using gravity in the inertial frame as a reference ,”   in Proc. IEEE/ION PLANS , Monterey, CA, May 6 – 8, 2008, pp. 961 – 965. [10]  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. [11]  T. Z. Kang, J. C. Fang and W. Wang , “Quaternion -Optimization-Based In-Flight Alignment Approach  for Airborne POS,”   IEEE Transactions on Instrumentation and Measurement , vol. 61, no. 11, pp. 1916-2923, 2012. [12]  Z. Wu, M. Yao, H. Ma and W. Jia,   “Improving Accuracy of the Vehicle Attitude   Estimation for Low-Cost INS/GPS Integration Aided by the GPS-Measured Course Angle, ”   IEEE Transactions on Intelligent Transportation Systems,   vol. 14, no. 2, pp. 553-564, 2013. [13]  Z. Wu, M. Yao, H. Ma, W. Jia and F. Tian,   “Low -Cost Antenna Attitude Estimation by Fusing Inertial Sensing and Two-Antenna GPS for Vehicle-Mounted Satcom-on-the-Move, ”   IEEE Transactions on vehicular technology,   vol. 62, no. 3, pp. 1084-1095, 2013. [14]  J. L. Crassidis, F. L. Markley, and Cheng Y,   “ Survey of Nonlinear Attitude Estimation , ”   Journal of Guidance, Control, and Dynamics , vol.30, no. 1, pp. 12-28, 2007.
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 42  [15]  Y. Wu and X. Pan,   “ Velocity/Position Integration Formula (II): Application to Strapdown Inertial Navigation Computation, ”   IEEE Transactions on Aerospace and Electronic Systems , vol. 49, no. 2, pp. 1024-1034, 2013 [16]  J. L. Crassidis and F. L. Markley, “Unscented   filtering for spacecraft   attitude estimation,”   Journal of Guidance, Control, and Dynamics , vol. 26, no. 4, pp. 536 – 542, 2003. [17]  J.   L. Crassidis and F. L. Markley, “Unscented filtering for spacecraft attitude estimation,”   Journal of Guidance, Control, and Dynamics , vol. 26, no. 4, pp. 536 – 542, 2003. [18]  L. B. Chang, B. Q. Hu, and G. B. Chang,   “ Modified UnScented QUaternion Estimator based on Quaternion Averaging, ”   Journal of Guidance, Control, and Dynamics , vol.37, no. 1, pp. 305-309, 2014. [19]  L. B. Chang, B. Q. Hu, S. Y. Chen and F. J. Qin,   “Comments on “A Quaternion -Based Method for SINS/SAR Integrated Navigation Sys tem” , ”   IEEE Transactions on Aerospace and Electronic Systems , vol. 42, no. 2, pp. 750-756, 2013. [20]  S. J. Julier,   and J. K. Uhlmann, “Unscented filtering and nonlinear est i mation,”   Proc. IEEE , vol. 92, no. 3, pp. 401 – 422, 2004. [21]  L. B. Chang, B. Q. Hu, A. Li, and   F. J. Qin, “Transformed unscented Kalman filter,”   IEEE Transactions on Automatic Control , vol. 58, no. 1, pp. 252 – 257, 2013. [22]  F. L. Markley, Y. Cheng, J. L. Crassidis, and Y. Oshman, “Averaging Quaternions,”   Journal of Guidance, Control, and Dynamics , vol. 30, no. 4, pp. 1193 – 1196, 2007. [23]  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-2655, 2014. [24]  Y. S. Shmaliy,   “Suboptimal FIR filteri ng of nonlinear models in additive white Gaussian noise, ”   IEEE Transactions on Signal Processing , vol. 60, no. 10, pp. 5519 – 5527, 2012. [25]  D. Simon, and Y. S. Shmaliy,   “Unified forms for Kalman and finite impulse response filtering   and
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 43  smoothing, ”   Automatica , vol. 49, no. 10, pp. 892 – 1899, 2013.  Lubin Chang   (M’13)   received B.Sc. and Ph.D. degrees in navigation from the Department of Navigation, Naval University of Engineering, Wuhan, P.R. China in 2009 and 2014, respectively. He is currently a Lecturer with the Naval University of Engineering. His current research interests include inertial navigation systems, inertial-based integrated navigation systems, and state estimation theory.  Jingshu Li   received the B.Sc. and M.Sc. degrees in navigation from the Department of Control Engineering, Aviation University of Air Force, Changchun, P.R. China, in 2008 and 2010, respectively, and the Ph.D. degree in navigation from the Department of Navigation, Naval University of Engineering, Wuhan, P.R. China in 2014.
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 44  He is currently a Lecturer with the Naval University of Engineering. His current research interests include inertial navigation systems, and control algorithms.  Kailong Li   received B.Sc. and Ph.D. degrees in navigation from the Department of Navigation, Naval University of Engineering, Wuhan, P.R. China in 2011 and 2015, respectively. He is currently a Lecturer with the Naval University of Engineering. His current research interests include inertial navigation systems, attitude estimation, and integrated navigation.
IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 52, NO. 4 AUGUST 2016 45  1