Odometer Aided SINS in-Motion Alignment Method Based on Backtracking Scheme for Large Misalignment Angles

In-motion alignment of Strapdown Inertial Navigation Systems (SINS) without any geodetic-frame observations is one of the toughest challenges for odometer aided navigation system. It is difficult to obtain an accurate attitude within a short time using coarse alignment under intense maneuvering condition, so one of the main obstacles during in-motion alignment for SINS/OD is to estimate an accurate attitude during fine alignment process under large azimuth misalignment angle. However, most current in-motion alignment methods were based on linear error model. In this paper, an OD aided in-motion alignment method based on backtracking scheme for large misalignment angles is proposed. Three improvements were made as the following: (1) A continuous nonlinear 16-dimensional (16D) SINS/OD error model is derived. (2) We proposed a backtracking-based Extended Kalman filter (EKF) and (3) a strict reverse navigation method is utilized in the backward process. Experimental results illustrated that the proposed method can autonomously improve the estimated attitude accuracy under the condition of large misalignment angle.


I. INTRODUCTION
Strapdown Inertial Navigation Systems (SINS) estimate position, velocity and angular orientation upon signals acquired by inertial measurement unit (IMU) -three accelerometers and gyroscopes stiffly fixed on a vehicle. SINS is widely used in aviation, marine, land vehicle navigation and positioning because of its advantage of complete autonomy. As a deadreckoning navigation method, SINS requires to be aligned in the initial alignment stage in order to determine the initial conditions prior to navigational computation. Due to the fact that the demand for quick reaction and poor environment survivability capability in land vehicles, in-motion alignment has become a concern issue. Global Positioning System (GPS) and Odometer (OD) has been important external information to enhance the performance of SINS. However, the GPS information will suffer from attenuation and outage, such as blocked by some infrastructures or entering a tunnel, which The associate editor coordinating the review of this manuscript and approving it for publication was Lubin Chang . results a decrease in initial alignment accuracy. In contrast, OD aided SINS (SINS/OD) is expected to resist interference from surrounding environment [1].Therefore, it is an essential task to achieve an accurate alignment result using OD aiding within a short period of time.
The traditional initial alignment is often divided into two procedures: coarse alignment and fine alignment [2]. The coarse alignment is used to rapidly calculate a attitude roughly. With the roughly known initial attitude fine alignment can be utilized to compensate and correct the misalignment angle further [3]. Many works have been done in field of coarse alignment under the condition of moving base. Huang et al. proposed a kalman-filtering-based in-motion coarse alignment methods in order to compensate for the dynamic errors of gyroscope induced by severe maneuvering [4]. Since the complicated are contained in the outputs of the inertial sensors, a coarse alignment method based on reconstructed observation vectors [5] were presented to solve the problem. In order to cope with any large angular motions, the optimization-based alignment (OBA) method VOLUME 8, 2020 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see http://creativecommons.org/licenses/by/4.0/ has been proposed [13]- [15]. The OBA method equivalently transform the INS attitude alignment into a continuous attitude determination problem. Using GPS information as a aiding source there are many alignment methods [6]- [8], Under the condition where the GPS information is valid and stable, the precise position and velocity information will greatly improve the accuracy of coarse alignment [9]- [12].
In military applications, GPS is susceptible to interference, therefore autonomous in-motion alignment using OD has drawn wide attention. Chang et al. utilized attitude estimation based alignment method for OD aided SINS [14]. Sun et al. further improve the OBA [16] method on the basis of the known initial information including initial binding position and velocity. However, it is difficult to estimate the attitude to the accuracy of within a few degrees in a few minutes with the existing in-motion coarse alignment methods because of the randomness of the road conditions as well as the dynamics of the vehicle. In-motion SINS/OD alignment with large initial misalignments is always a challenge and thus needs to be researched. A lot of literature has been devoted to the linear model of SINS/OD system. In [17] and [18], taken the odometer scale factor error and the attitude matrix error into consideration, a linear error model of OD is established. Li et al. proposed an online calibration and compensation of total OD error in SINS/OD system [19]. In [16] a continuous linear 16-dimensional SINS/OD integrated model was utilized in fine alignment. Few researches were about the nonlinear model of SINS/OD system, therefore further improvements of the nonlinear model would be worthwhile. The other main effort to deal with the large misalignments problem is based on nonlinear filtering methods such as extended Kalman filter (EKF), unscented Kalman filter (UKF), and cubature Kalman filter (CKF). Li et al. used UKF techniques for DVL-aided SINS in-motion alignment [21] and Jiancheng and Sheng used EKF for in-flight alignment of airborne POS [22]. In [21] Li et al. utilized EKF in SINS/OD system among self-calibration and in-motion alignment. Under the condition that the navigation computer has a limited computing speed. We need to choose an appropriate solution of nonlinear filter.
With the rapid development of navigation computer, a large amount of data can be stored in real time and processed within a very short time. The backward navigation algorithm has been investigated to speed up the convergence rate of Kalman filter and improve the accuracy of alignment result [23]. Chang et al. proposed a novel backtracking scheme for initial alignment [24]. Lu et al. utilized a newly derived backward filter to in-motion fine alignment [25]. Nevertheless, few researches were about backtracking scheme based on nonlinear filter, which was considered as a necessary method for fast alignment under the condition of large misalignment angles.
In order to solve the above problems, an OD aided inmotion alignment method based on backtracking scheme for large misalignment angles is proposed. In this paper, (1) we derived a continuous nonlinear 16-dimensional (16D) SINS/OD error model; (2) In the process of coarse alignment, the IMU data and OD data were stored in real time in navigation computer memory. Using the stored data, a backtrakcingbased EKF filter is proposed in this paper; (3) In general applications, the signs of gyroscope measurement is opposite. In order to get a higher accuracy in the backtracking filtering process, we propose a method with strict reverse iteration based on the research on the iterative process of reverse navigation. Finally, three car-mounted field tests are carried out to verify the validity and applicability of investigated method.
The rest of the content is organized as follows. Section II presents the reference frame definitions. Section III derives the non-linear SINS/OD error model. In Section IV, the strict backtracking SINS algorithm, backtracking dead reckoning algorithm and backtracking EKF-based fine alignment algorithm and are derived. Section V we conducted three vehicle experiments to compare the proposed method and existing in-motion alignment methods. The conclusions are given in Section VI.

II. THE REFERENCE FRAME DEFINITIONS
The reference coordinate frames involved in this paper are defined as follows: i frame: Earth-Centred Initially Fixed (ECIF) orthogonal reference frame.
b frame: Orthogonal reference frame aligned with Right-Forth-Up (RFU) axes.
n frame: Orthogonal reference frame aligned with actual East-North-Up (ENU) geodetic axes.
n frame: Orthogonal reference frame aligned with computational East-North-Up (ENU) geodetic axes. n 0 frame: Orthogonal reference frame nonrotating relative to the e frame. It's formed by fixing the n frame at the beginning of the in-motion alignment.
i 0 frame: Orthogonal reference frame nonrotating relative to the i frame. It's formed by fixing the i frame at the beginning of the in-motion alignment.
i b0 frame: Orthogonal reference frame nonrotating relative to the i frame. It's formed by fixing the b frame at the beginning of the in-motion alignment.

III. ALIGNMENT MODEL A. NON-LINEAR INS ERROR MODEL
Let the platform attitude errors be represented by Euler rotation angles.The derivation of the nonlinear INS erorr model for small tilt and large azimuth misalignment angles is given as follows: Then adopting ENU rotation sequence, if the tilt error angles φ E ,φ N are small then following approximations can be made the transformation matrix C n n in terms of Euler rotation angles will be if ω n nn is the angular velocity of n-frame, then the propagation of Euler rotation angles is computed aṡ For local geographic(ENU)frame Using (3) and (8) in (7) and expanding gives following nonlinear attitude error model for small tilt error angles and large azimuth error angle.
The specific force euation expressed in the n frame is presented as where v n = v n E v n N v n U T is the velocity projected in the n frame and g n = 0 0 −g T is the gravity in the n frame.Its error equation,ignoring second and higher order terms will be Using We spread out the (12) without consideration of δv n U then we can obtain the non-linear velocity error equation as follows: The position (Latitude,longitude) differential equation is written asL Regarding the slow changing of R N and R M , we can take them as constants, so the position error equation without the consideration of height can be written as (15) As for the gyro bias and accelerometer bias error equation, we figure them as constants. With no consideration of the accelerometer uppper bias the differential equations were given as follows:ε In real application, it is impossible to install the INS's body frame perfectly parrel to the vehicle's body frame. Taken the odometer scale factor error and the attitude matrix error into considerations, the actual vehicle velocityṽ D can be written as follows.ṽ where C n b is the attitude matrix determined by the deadreckoning updating. C m b denotes the misalignment matrix between the SINS and odometer.write the misalignment angle as α = α θ α γ α ψ T .Thus we can write: The α θ , α γ and α ψ are the misalignment error of pitch, roll and heading. Otherwise, taken the odometer scale factor error into consideration, the actual vehicle velocityṽ n D was given as follows: where δk D denotes the error of the odometer sclae factor and α denotes the attitude error in navigation frame. Thus we can obtain the velocity error function: The differential equation of the odometer position can be written as:ṗ Using (20) into (19), we can get: where Expanding (22) and eliminating the products of error quantities yield where v D is the odometer line velocity, (v n D ×) is the skew symmetric matrix of the odometer velocity with respect to the navigation frame. The error function of the scale factor error and misalignment error can be written as follows: With no consideration of the vertical direction, The effect of the misalignment error α θ can be transformed into the odometer scale error because the height channel are analyzed in another way. Meanwhile, the altitude error and vertical velocity can be restrained by barometer altimeter. Therefore, in order to reduce the computational load and enhance the observability of the filter states. Based on the previous formula derivation, we establish the state vector X as: While under moving base, the position errors can be acquired by deducting the dead-reckoning output positionp D from the SINS output positionp. Thus the linear measurement variables are chosen as position errors: where p real is the actual position. Therefore, based on the previous analyze, the continuos Extended Kalman filter of fine alignment is desgned as follows:Ẋ where f [X(t), t] can be shown on the basis of the formula (9),(13), (15), (16), (24) and (26). G(t) is the noise matrix; w(t) is the system process white noise with the power spectral density Q(t). H(t) is the measurement matrix. The nosie matrix G(t) and the measurement matrix H(t) can be expressed as follows: 7940 VOLUME 8, 2020

IV. BACKTRACKING ALIGNMENT SCHEME FOR LARGE MISALIGNMENT ANGLE A. BACKTRACKING SINS ALGORITHM
The backtracking SINS algorithm is based on the forward SINS algorithm. Assuming that SINS navigates from position A to position B from time t 0 to t, the backtracking discreted SINS algorithm from position B back to position A can be written as: T s is the sampling time of gyroscopes and accelerometers in SINS, k is each discretized time point of the navigation process (k=1,2,3,...). The backtracking attitude and velocity update algorithm can be expressed as: Combining equation (31a) and (32a), we can write: Defining p = m−k + 1, we can get: Using (34) into (31), the backtracking algorithm formula can be written as: It can be seen that the backtracking descreted SINS algorithm is similar to the forward discreted SINS algorithm except that the sign of initial velocity and earth rotation are opposite. The skew symetric matrix of backtracking gyro rotating angular velocity can be written as:

B. BACKTRACKING DEAD RECKONING ALGORITHM
The process of fine alignment can be recognized as a Dead-Reckoning system, assuming the sampling time of OD is T s as well. the velocity v b sk measured by odometer with the reference in the b frame can be calculated as: where s b k denotes the mileage increment expressed in the b frame.
Defining the p = m−k + 1. The backtracking dead reckoning formulas can be written as: where ← L Dp , ← λ Dp and ← h Dp denote the latitude, longitude and height calculated by DR. Respectively, R MD and R ND calculated by DR latitude are the radius of curvature in meridian and prime vertical.
Similarly, the sign of OD measurements and earth rotation angular velocity are opposite in discreted backtracking DR algorithm. The skew symetric matrix of gyro ratotating angular velocity in the backtracking attitude update algorithm is written as (36).

X(t) = f [X(t), t] + G(t)w(t) Z(t) = H(t)X(t) + v(t)
For the state model, set the sample interval at t, and expand X (t + t) in the Taylor series at t.
( t) 2 2! + · · · (40) Set X k = X(t) and X k+1 = X(t + t), and two higherorder terms are ignored since they have little effect on the VOLUME 8, 2020 accuracy but will bring a heavy computational burden. Then the discrete equation can be written as: where A (X k ) = ∂f (X, t)/∂X|X = X k is the Jacobian matrix and w k is the system noise in discrete form with covariance Q k . For the measurement model, it can be written in descrete form directly where Z k is the observation vector, H k is the observation matrix, and v k is the measurement noise in discrete form with covariance R k . Forward Extended Kalman filter process is organized as follows time update: measurement update: X k/k−1 is the a priori state estimation. P k/k−1 is the a priori error covariance matrix, and k/k−1 is the state transformation matrix, which can be calculated as K k is the filter gain at step k, andZ k is the innovation at step k, which can be calculated as The backtracking Extended Kalman filter process can be obtained by modifying the forward Extended Kalman filter process. The state and measurement vector of backtracking Extended Kalman filter are the same with the forward filter. The dynamic model of backtracking filter has some different elements compared with the forward filter, the gyroscope biases and the earth rotation angular velocity in the (27) are oppositite. And the skew symetric matrix of backtrakcing gyro rotating angular velocity can be wriiten as (36). As for the time backtracking management, it requires that the system state update to be performed in the reverse order. In time update equation and measuremennt update equation, the time update process is performed fromX k toX k−1 , transfer matrix k,k−1 changes to k−1,k . In measurement update process the prediction process is from P k to P k−1 . So the 16D backtracking Extended Kalman filter process is organized as follows time update: measurement update: According to (50) to (54), we can estimate the attitude error by employing backtracking Kalman filter and forward Kalman filter within a certain time period. During the calculation period, the SINS is still working. In this respect, time delay t D will exist during the implementation of forward process at t coarse . So extra memory is essential to record the measurement information. Then the forward process will be end at t fine and follow by the navigation process. It should be pointed out that the delay time t D is short due to the enormous computing power of modern navigation computer. Thus the investigated backtracking algorithm can be calculated within a very short time.
Given the above, the diagram of backtracking scheme process is shown in Fig.1, and the alignment algorithm procedure are summarized in the following.
1) The coarse alignment algorithm is developed to derive the attitude, velocity and position during the moving process. In the meantime. the gyro data, the accelerometer data and the odometer data are stored in the computer memory during coarse alignment. 2) The backward process of fine alignment is executed.
The initial velocity is set as opposite. Meanwhile, the gyroscope measurement were stored as skew symetric matrix of backtrakcing gyro rotating angular velocity. The gyro biases, earth rotation angular velocity are set as opposite according to the backtracking algorithm. During this process the gyro data, the accelerometer data and the odometer data are stored in the computer memory.
3) The forward process of fine alignment is carried out when the backward process ends. The signs of gyroscope output angular velocity, gyroscope biases, earth rotation angular velocity are back to normal. The known initial binding position and the velocity are set at the start of forward process, while other parameters of the EKF are not reset, aiming to further refine the velocity and position estimation. The computing interval is much shorter than the raw data sampling interval, thus the computer will catch up with the stage of realtime sampling stage within a short time. Futuhermore, the convergent odometer errors estimated by Extended Kalman filter are compensated to obtain the velocity and position with higher precision. 4) Initialized with the attitude, velocity and position provided by forward process, the navigation process can be conducted during every sampling interval time.

V. FIELD TEST A. TEST ARRANGEMENTS
Field navigation tests were carried out to verify the effectiveness of the proposed EKF-based in-motion alignment method. A navigation-grade ring laser SINS, odometer, barometric altimeter and differential GPS are equipped on the car, as is shown in the figure 2. The SINS consisted of three Ring Laser Gyros with an accuracy of 0.01 • /h (1σ ) and three quartz accelerometers with a bias of 50 µg (1σ . The sampling frequency was 100 Hz. A DPGS with an accuracy of 3 cm was utilized as the reference position information. The scale factor and installations of OD are accurately calibrated before the field tests. Three car-mounted field tests were carried out. The corresponding trajecotries are shown in Fig. 3. The descriptions of three field tests are summarized in Table 1. In order to provide a precise attitude and position reference, we use normal static alignment method during the car stationary data for 300 seconds and then utilizing the static alignment attitude results as the initial attitude, during the maneuvering environments we use the SINS/DGPS integrated algorithm. The initial alignment accuracy utilizing the static data is within 1 arcmin, which is accurate enough to utlized as the reference. The navigation results (attitude and postion) were utilized as the reference data. In order to compare the KF-based backtracking algorithm, EKF-based backtracking algorithm and EKF-based backtracking algorithm with strict reverse navigation, three in-motion alignment schemes are  carried out using the 400 seconds maneuvering data. The descriptions including alignment method, algorithm organization are summarized in Table 2. The two schemes were executed without the aid of GPS. The algorithm are coded with C in TMS320C6748 Fixed/Floating Point Digital Signal Processor and draw wtih MATLAB.

B. RESULTS AND ANALYSIS
In order to compare the three fine alignment algorithm, thee in-motion alignment schemes are carried out using manuevering data. The descriptions including Scheme number, alignment method and filter parameters are summarized in Table 2. The coarse alignment method and alignment time are the same in three schemes to provide the same initial misalignment angles before three different fine alignment methods. Three schemes are executed without GPS aiding. The algorithms are coded with Visual Studio 2017 and draw with MATLAB on a computer with Intel Core i7-6700HQ CPU and the Windows 10 operating system.
In the backward process, the heading-estimate errors of three field tests are shown in Figs. 4-6, the process lasts 400 seconds. As is shown in the figures, the convergence of   scheme 1 which uses KF is faster than the other two schemes which uses EKF. Because of the large initial misalignment at   the begining of the filtering process, we need to zoom in on the convergence curve for the last few seconds. In the enlarge view of Figs. 4-6, although the convergence rate of scheme 1 is really fast, the accuracy is obviously not as high as the other two schemes ( larger more than 1 arcmin). The reason is that under the condition of large misalignment, the SINS/OD linear error model is no longer satisfied. The differences between scheme 2 and scheme 3 are as following: the signs of gyroscope measurements are opposite in scheme 2, otherwise in scheme 3 the skew symetric matrix of backtracking rotating angular velocity can be written as (36). Therefore the scheme 3 is more accurate than the scheme 2 during the backward process. For the purpose of providing an accurate initial value when the forward process started, scheme 3 is the best solution among three schemes.
In the forward process, the heading-estimate errors of three trajectories are shown in Figs. 7-9 and the process lasts the VOLUME 8, 2020   same as the backward process time. Due to the fast convergent speed of KF, the heading error of scheme 1 is approaching a constant value. On the contrary, the errors of the other two schemes are still converging rapidly. The alignment algorithm are the same in scheme 2 and scheme 3 in the forward process, the difference between the two schemes are the initial misalignment angle and EKF parameters. Obviously, the heading error of scheme 1 is much larger than that of scheme 2 and 3, therefore we mainly compare scheme 2 and scheme 3. In order to have a clear sight on the last few seconds, the enlarge view are also shown in the figure.
The convergence curve trends of scheme 2 and scheme 3 are very similar, because the initial value of scheme 3 in the forward process is more accurate, the estimated heading value of scheme 3 is more accurate than scheme 2. Both the linear model and nonlinear model are based on the assumption of small horizontal misalignment angle. For the    convenience of analysis, we put the forward process and the reverse process in one figure. Figs. 10-12 shows the whole process of pitch errors and Figs. 13-15 shows the whole process of roll errors. The duration lasts 800 seconds in total. Different from the heading error curve, the horizontal attitude error curve converges very fast. In order to have a clear sight on the last few seconds, this paper enlarges the curve of the last few seconds. We can see the pitch error of scheme 1 is obviously large than the other two schemes, and the pitchestimate of scheme 3 is more accurate than scheme 2. The roll errors in Fig. 13 are no big differnece, because the trajectory of the vehicle is nearly a straight line. However in trajectory 2 the error of scheme 2 and scheme 3 are nearly the same but much smaller than scheme 1. In Fig. 15, The manuevering process of trajectory 3 are more intense than trajectory 1 and 2. The figure shows that the roll error estimated by scheme 3 is smaller than scheme 2 and scheme 1.

VI. CONCLUSION
The in-motion alignment has drawn increasing attentions due to its necessity for many military applications. In this paper, an OD aided in-motion alignment method based on backtracking scheme for large misalignment angles is proposed. We derived a continuous nonlinear 16-dimensional (16D) SINS/OD error model to solve the problem of large initial misalignment. In the investigated backtracking scheme, the three gyros' outputs were written as a skew symmetric matrix of backtracking gyro rotating angular velocity, and we derived backtracking EKF-based fine alignment algorithm in order to achieve a higher attitude accuracy within a short time. The recorded data is reversed and connected with the forward data. The field test experiments show that the proposed alignment algorithm can autonomously improve the attitude accuracy under large azimuth misalignment angle.
ZEYANG WEN received the B.S. degree in integrated circuit from Xidian University, Xi'an, Shaanxi, China, in 2017. He is currently pursuing the Ph.D. degree with the School of Instrumentation and Optoelectronic Engineering, Beihang University.
His research interest includes the error mitigation method of long-term independent inertial navigation.
GONGLIU YANG received the Ph.D. degree in precision instrumentation and mechanism from Tsinghua University, Beijing, China, in 2004.
He is currently a Professor with the School of Instrumentation and Optoelectronic Engineering, Beihang University. His current research interests include inertial navigation and integrated navigation. VOLUME 8, 2020 QINGZHONG CAI received the Ph.D. degree in optical engineering from Beihang University, Beijing, China, in 2014.
He is currently a Research Associate with the School of Instrumentation and Optoelectronic Engineering, Beihang University. His current research interests include inertial navigation and integrated navigation.
YIDING SUN received the B.E. degree in measurement and control technology and instrumentation from Harbin Engineering University, Harbin, China, in 2015. He is currently pursuing the Ph.D. degree with the School of Instrumentation and Optoelectronic Engineering, Beihang University.
His current research interests include inertial navigation and integrated navigation.