Semi-Analytic Method for SINS Attitude and Parameters   Online Estimation Lubin Chang Department of Navigation Engineering, Naval University of Engineering, Wuhan, China changlubin@163.com  Abstract —In this note, the attitude and inertial sensors drift biases estimation for Strapdown inertial navigation system is investigated. A semi-analytic method is proposed, which contains two interlaced solution procedures. Specifically, the attitude encoding the body frame changes and gyroscopes drift biases are estimated through attitude estimation while the attitude encoding the constant value at the very start and accelerometers drift biases are determined through online optimization.  Index Terms— Strapdown inertial navigation system, initial alignment, attitude estimation, online optimization.  I. Introduction  The past ten years have seen the prosperity of initial alignment methods development for   the strapdown inertial navigation system (SINS). This is mainly promoted by the optimization based alignment (OBA) methods which are proposed and popularized by Wu, Yan, et al. [1-9]. As the greatest contribution, OBA formulates the SINS attitude alignment into an attitude determination problem using vector observations, which " reveals an interesting link between these two individual problems that has been studied in parallel for several decades   [1]". In OBA, the attitude is decomposed into separate Earth motion, inertial rate, and constant attitude at the very start. The attitude determination just refers to the determination of the constant attitude at the very start. Within the OBA framework, there are two main research aspects, that is, construction of the vector observations based on specific forces equation and extension of attitude determination model through formulating parameters other than attitude into the cost function for optimization. Regarding to the first aspect, Silson proposes an interleaved vector observations construction method, which can make full use of information inherent in the specific forces equation [9]. This procedure has also been used in the attitude
estimation based alignment that will be discussed in the following right now. Regarding to the second aspect, Chang et al. formulate the attitude alignment into an attitude estimation method, which can estimate the gyroscopes drift biases coupled with the attitude [10, 11]. However, this method is powerless in estimating accelerometers drift biases. In contrast, Wu et al. propose an online constrained optimization method which can simultaneously estimate the attitude and other related parameters [4]. From the numerical test results, it is shown that this method is only validate in estimating accelerometers drift biases and is compromised in estimating gyroscopes drift biases. It is therefore desired to combine the advantages of Wu and Chang's methods, which is just the consideration of the presented work. In this note, a semi-analytic alignment (SAA) method is proposed, which is intended to realizing the aforementioned combination. In the proposed SAA, the attitude encoding body frame changes and gyroscopes drift biases are estimated using attitude estimation methods as in [10] and constant attitude at the very start and accelerometers drift biases are determined through optimization based method as in [4].The attitude estimation and optimization are interlaced with each other in the SAA framework. The contents are organized as follows. Section II mathematically formulates the SAA framework. Section III presents the solution procedures for the two interlaced sub-problems in SAA. Conclusions are drawn in Section IV. Finally, some of my comments on existing initial alignment methods are presented in Section V.  II. Problem Formulation  All the quantities involved in this note uses the same set of symbols as in [4] for brevity and, if not stated, their definitions and meanings are omitted for brevity. The attitude matrix       n b   t C   can be decomposed as                            0 0   0 n t   b n   n b   b n   b t t    C   C   C   C   (1) where          0  n t n  C   can be calculated according to                    0   0 n   n   n in n t   n t      C   C   ω    (2)      0 n b C   can be determined according to the procedures in [4] as
       0 n b   a     C   α   χ b   β   (3) where                    2 1 0   0 3   2 k m M t   b   b b   b   b ib b   b t t   k   m t d   t                                    α   C   f   C   I   ω   f   (4)                         2 1 0   0 3 3   2 k m M t   b   b   b ib b   b t t   k   m t d   t                                       χ   C   C   I   ω   (5)                                              0   0   0   0  m   m   m t   t n   n   n   n n   n   n   n   n m   ie n t   n   n n t   t   t t   t   d   d                       β   C   v   C   v   C   ω   v   C   g   (6) The first integral on the right hand of (6) is given by                                                           1 1 0   0 2   2 1   0 3 3   3 3   1 2   6   2   3  k   k k m   k k M t   t   n t n   n n   n   n   n ie   ie n   n n t t   t k   m M   n   n   n   n   n   n   n in   ie   k   in   ie   k n t k   m d   d t   t   t   t t   t                                                                                                          C   ω   v   C   C   ω   v C   I   ω   ω   v   I   ω   ω   v  (7) The second integral on the right hand of (6) is given by                                        1 2 1   1 0   0   0 3 3   2  k   k k   k m   k M   M t   t   n t n   n   n n   n   n   n in n   n n t   n t t   t k   m   k   m t d   d   t                                                  C   g   C   C   g   C   I   ω   g   (8) In (3) the accelerometers drift biases   a b   has also been jointly estimated associated with the attitude.        0 b b t  C   and the gyroscopes drift biases   g b   can be estimated according to the procedures in [10] as                    0   0 b   b   b ib   g   gv b t   b t            C   C   ω   b   η       (9)        0 b m   m b t    C   α   β   (10) where (9) is the process model and (10) is the measurement model. The vector observations in (10) are given by                                            0   0 0   0  m   m t   t b t   b   b t   b b   b m   b   b   b   b t   t d   d                    α   C   C   f   C   C   f   (11)      0   T n m   b  β   C   β   (12) The integration in (11) can be calculated according to (4). The   estimation   of         0 b b t  C   necessitates   the   constant   attitude       0 n b C   to   construct   the   vector observations as shown in (12). The determination of       0 n b C   necessitates the attitude         0 b b t  C   to construct the vector observations as shown in (4). This is just an interlaced procedure as shown in Fig. 1.
Optimization MEKF  g b        0 b b t  C      0 n b C    n b   t C  α  m β  m α β  a b        0 n n t  C  Fig. 1. Interlaced framework of SAA  III. Problem Solutions  Through the formulation in last section, the attitude alignment and parameters estimation have been decomposed into two interlaced sub-problems. One refers to constrained-optimization and the other refers to attitude estimation. The solution procedures can be derived directly through lessons learned from the references [4, 12]. The constrained-optimization problem (3) is actually a simplified form of (27) in [4]. In this respect, the time-recursive Newton-Lagrange algorithm developed in [4] can be readily used to solve (3). However, due to the simplicity of (3), an efficient solution learned from the  Point-Cloud   Alignment   Problem   for   robotics   can   be   applied   directly.   The   following procedures   are   the   representation   of   that in   section   8.1   of   [13],   where   only   notation modifications are made to adapt to problem (3). Problem (3) can be reorganized as                   1 1 3 0   0 1   0   1 1  j   j n a b                                                                                              p   r   y q   q  α   χ b   β C 0    (13) where   q   is the attitude quaternion corresponding to       0 n b C   with definition as   T T            q   ρ   . The quaternion left-hand compound operator "+" and the right-hand compound operator " ⊕ " are defined as      3 3  T                               I   ρ   ρ  q   ρ   (14)      3 3  T                               I   ρ   ρ  q   ρ   (15)
Given the corresponding definitions in the subscripts of (13), an error quaternion can be formed as          1  j   j   j              e   y   q   p   r   q   (16) The total objective function to be minimized can be defined as             1 1   1 , ,   1 2   2  M   T   T j   j   j j J   w             q r   e   e   q q   (17) where   j w   is the weight corresponding to   j e   . Taking the derivative of the objective function with respect to   q   ,   r   and     gives                    1  M   T j   j   j   j   j T j J   w                                      y   p   r   y   p   r   q   q q   (18)          1 1  M j   j   j T j J   w                         q   y   p   r   q r   (19)      1   1 2  T J              q q   (20) Setting (19) to zero, we find 1            r   q y q   p   (21) where   y   and   p   are given in the following. Substituting (21) into (18) and setting the resulting equation to zero, we have    Wq   q   (22) where                                1 1   M   T j   j   j   j   j j w w                                 W   y   y   p   p   y   y   p   p   (23) 1 1   M j   j j w w        y   y   ,   1 1   M j   j j w w        p   p   ,   1  M j j w   w         (24) It is shown that (22) is just an eigenvalue   problem, which can be solved using some existing well-known   methods,   such   as   Davenport   q-method   as   in   [1].   The   accelerometer   drift bias   a b   can be readily identified from (21). The   attitude   estimation   problem   (9)   and   (10)   can   be   solved   using   the   representative algorithms, such as multiplicative extended Kalman filter (MEKF) or UnScented QUaternion Estimator (USQUE) [13]. If the constant attitude       0 n b C   is initialized using the eigenvalue  problem (3) ignoring sensors biases, the MEKF will be adequate for the attitude estimation problem (9) and (10). This is because that the initial value of the attitude is accurately known,
that is           0 3 3 0  b b     C   I   . The explicit procedures for estimation of         0 b b t  C   coupled with   g b   by MEKF are summarized in   Algorithm 1 .  Algorithm 1 : MEKF for estimation of         0 b b t  C   and   g b  Initialization            0   0 ˆ   0 0 0 1   T b b   t    q   ,       0   3 1 g   t     b   0   and       0   0 t    P   P  Kalman Gain                    1 1   1   1   1   1 ˆ   ˆ   ˆ T   T k   k   k   k   k k k   k k   k k   k k   k k                              K   P   H   x   H   x   P   H   x   R            0   ,   3 3 1   ,   1 ˆ ˆ   b k   m k k k   b   k k                  H   x   C q   β   0  Update      6 6   1   1 ˆ k   k   k   k k   k k                     P   I   K H   x   P      ,   1 ˆ   ˆ k   k   m k   k   k k h                    x   K   α   x            0   , 1   ,   1 ˆ ˆ   b k   m k k k   b   k k h        x   C q   β  , ˆ ˆ ˆ   T   T k   k   g k                    x   α   b      1   1 ˆ   ˆ   ˆ   ˆ 0.5 k   k k k   k k               q   q   q   α  ,   , ,   1 ˆ   ˆ   ˆ  g   k   g k g k k         b   b   b  Propagation            ˆ ˆ   b   b ib   ib   g t   t   t     ω   ω   b             0   0 1 ˆ   ˆ   ˆ 2  b   b   b b   b   ib t   t   t             q   q   ω                                 T   T t   t   t   t   t   t   t   t        P   F   P   P   F   G   Q   G           3 3 3 3   3 3 ˆ   b ib   t t                                         ω   I F   0   0   ,       3 3   3 3 3 3   3 3  t                          I   0 G   0   I  In   Algorithm 1 ,          q   is defined as            3 3  T                            I   ρ  q   ρ   (25)  Algorithm 1   is a continues-discrete form of MEKF. The explicit discrete form of MEKF for on-board application is not presented here for brevity. The readers can refer to [12] for more information about this algorithm.
IV. Conclusions  This note has traced the thread of optimization-based approach for SINS through exploring a new technique for joint estimation of attitude and inertial sensors drift biases. The joint estimation problem is posed as an interlaced procedure which contains dynamic attitude estimation and online optimization. The outputs of the two procedures are the necessary inputs for each other to construct the corresponding vector observations.  V. My Comments  Initial alignment has been one of the difficult and attractive topics for SINS. To design practical initial alignment method, one should master SINS calculation, error analysis, state estimation and so on. If one has mastered initial alignment, he can also master inertial navigation technologies, so to speak. The initial alignment is the starting stage of SINS and studying of initial alignment can be viewed as the first step of studying inertial navigation technologies. This can be viewed as the reasons why so many papers related to initial alignment has been published. Unfortunately, most of these papers are only suitable for theoretical investigation and there is still a long way from becoming practical, however. Taking the nonlinear filtering based alignment for example, it is expected to unify the coarse and fine alignment stages through modeling nonlinear errors equations for SINS. However, the initial state covariance is a crucial factor for the attitude alignment performance. If the covariance is too small, the filtering may be diverging while if it is too big, the filtering steady state may be too large, which can not research the same accuracy level as the traditional two-stage method. The appropriate covariance setting is corresponding to the initial attitude misalignment which unfortunately, is unknown for initial alignment. In this respect, I don't prefer to make use of nonlinear filtering based alignment method in practice. The OBA method is an attractive method for SINS attitude alignment and it has been applied in practice to the best of my knowledge. The aforementioned applications actually refer in particular to the original form of OBA in [5, 8]. The subsequent "modification" or "extension" are also limited to theoretical investigation. For the presented method in this note, I have not carried out the numerical and experimental tests. This is because that I know that the corresponding results will be not so satisfactory as expected, which is due to the coupling of errors sources. Actually, the method in [4] is also capable of estimating all the involved parameters theoretically. The unsatisfactory gyroscopes drift biases estimate is also due to the
coupling of errors sources. The superiority of attitude alignment in [4] compared with traditional EKF based method is just derived through the optimization procedure which contains a special vector observations construction procedure. This superiority has also been demonstrated   in   our   attitude   estimation   based   alignment   method   [10].   Based   on   the aforementioned discussion, my choice of initial alignment in practice is making use of OBA method in [5, 8] to perform coarse alignment and making use of Kalman filtering to perform fine alignment.  Acknowledgment  The author would like to thank Prof. Yuanxin Wu and Dr. Yulong Huang for the discussions on the SINS initial alignment problem.  References  [1]   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. [2]   Y. X. Wu, H. L. Zhang, M. P. Wu, X. P. Hu and D. W. Hu, “Observability of SINS alignment: A global perspective,”   IEEE Transactions on Aerospace and Electronic Systems , vol. 48, no. 1, pp. 78–102, 2012. [3]   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. [4]   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. [5]   Y. Y. Qin, G. M. Yan, D. Q. Gu and J. B Zheng, “A clever way of SINS coarse alignment despite rocking ship,”   Journal of Northwestern Polytechnical University , vol. 23,no. 5, pp. 681–684, 2005.  （秦永元，严恭敏，顾冬晴，郑吉兵，摇摆基座上基于信息的捷联惯导粗对准研究，西北工业 大学学报，第 23 卷，第 5 期， 681-684 ， 2005 年 10 月）  [6]   G. M. Yan, Y. Q. Qin, and Y. X. Wei, et al., "New initial alignment algorithm for SINS on moving base,"   Systems Engineering and Electronics , vol. 31, no. 3, pp.634-637, 2009. （严恭敏，秦永元，卫
育新，张立川，徐德民，严卫生，一种适用于   SINS   动基座初始对准的新算法，系统工程与电 子技术，第 31 卷，第 3 期， 634-637 ， 2009 年 3 月）  [7]   G. M. Yan, J. Weng, L. Bai, and Y. Q. Qin, "Initial in-movement alignment and position determination based on inertial reference frame,"   Systems Engineering and Electronics , vol. 33, no. 3, pp.618-621, 2009. （严恭敏，翁浚，白亮，秦永元，基于惯性参考系的动基座初始对准与定位导航，系统工 程与电子技术，第 33 卷，第 3 期， 618-621 ， 2011 年 3 月）  [8]   G. M. Yan, On SINS In-movement Initial Alignment and Some Other Problems,   Post-doctoral Research Report of Northwestern Polytechnical University , 2008. （严恭敏，捷联惯导系统动基座初 始对准及其它相关问题研究，西北工业大学博士后工作报告， 2008 年 11 月 ）  [9]   P. M. G. Silson, “Coarse Alignment of a Ship's Strapdown Inertial Attitude Reference System Using Velocity Loci,” IEEE Transactions on Instrumentation and Measurement, vol. 60, no. 6, pp. 1930-1941, 2011. [10]  L. B. Chang, J. S. Li and S. Y. Chen, “Initial Alignment by Attitude Estimation for Strapdown Inertial Navigation Systems,”   IEEE Transactions on Instrumentation and Measurement , vol. 64, no. 3, pp. 784-794, 2015. [11]  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. [12]  F. L. Markley, and J. L. Crassidis, Fundamentals of Spacecraft Attitude Determination and Control. Microcosm Press and Springer, 2014. [13]  T. D. Barfoot, State Estimation for Robotic, Cambridge University Press, 2017.