A Fast SINS Initial Alignment Method Based on RTS Forward and Backward Resolution

For the strapdown inertial navigation system (SINS), the procedure of initial alignment is a necessity before the navigation can commence. On the quasi-stationary base, the self-alignment can be fulfilled with the high quality inertial sensors, and the fine alignment is usually executed to improve the alignment performance. Generally, fast estimating of heading misalignment is still a challenge due to the existence of gyro errors. An innovative data processing strategy called forward and backward resolution is proposed for INS initial alignment.The Rauch-Tung-Striebel (RTS) smoothing is applied to obtain the smoothed attitude estimates with the filter information provided by the forward data processing.The obtained attitudes are then treated as aidingmeasurements to implement the forward resolution with the repeated data set, the converged sensor biases are used as constraints, and the iterative processing is conducted to obtain the updated attitudes. Simulation studies have been conducted to validate the proposed algorithm. The results have shown that the alignment accuracy and convergence rate have been improved with the added RTS aided forward and backward resolution; more stable heading estimates can be obtained by calibrating with estimated gyro bias. A real test with a high quality inertial sensor was also carried out to validate the effectiveness of the proposed algorithm.


Introduction
Navigation can be referring to a technique that involves the determination of position and orientation of a moving object.Traditionally, the Global Navigation Satellite System (GNSS) has been widely used for navigation applications in the outdoor environment [1].However, the GNSS positioning performance quickly deteriorates in the challenging environments, like urban canyons and under foliage.On the other hand, the inertial navigation system (INS), which is a dead-reckoning based navigation system, is the only form of navigation that does not rely on external reference [2].The INS can successively supply position, velocity, and attitude information in any environment.Consequently, it has been widely used in the fields of navigation and positioning such as vehicles, ships, aircraft, and guided missiles.Before an INS is ready to navigate, the initial navigation parameters need to be provided, among which the initial position and velocity can be easily obtained from external reference, such as GNSS [3][4][5].The initial alignment, which refers to the attitude initialization, becomes the main factor that influences the navigation accuracy.
For the strapdown inertial navigation system (SINS), the purpose of alignment is to obtain the rotation matrix between the body frame and navigation frame.The initial alignment method usually involves two steps, namely, a coarse alignment utilizes analytic method followed by a fine alignment.The coarse alignment aims to obtain the initial attitudes with small misalignment angles; however, the coarse alignment is only feasible for the high quality inertial sensors.For the low cost INS, the initial alignment, especially for the heading, is still a challenge due to the existence of high sensor noise and bias.The heading information for the low cost INS is usually provided by the external reference, such as magnetic compass [6].
In broad sense, the fine INS initial alignment can be divided into three types [7], which are quasi-stationary alignment that assumes the INS is stationary with respect to the Earth [8,9], GNSS alignment that uses position and velocity information to aid INS alignment [10,11], and transfer alignment which uses position or velocity and sometimes attitude from the master INS [12,13].Generally, the Kalman filter (KF) is the most commonly used to calibrate position, velocity, and attitude based the difference of INS outputs and external reference [14].In the alignment process, the alignment accuracy can be improved if the misalignment angles and gyro bias error can be effectively estimated.Considering the large initial attitude error uncertainties derived when using the low cost sensors, the unscented Kalman filter (UKF) is commonly used to handle large attitude errors [15,16].In addition, the  ∞ filter has been introduced to improve the transfer alignment accuracy considering flexure deformation and lever-arm effect [17,18].
In real environment, the output from the inertial sensor, especially for low or medium cost sensors, suffers from large sensor noise.Efforts have been made to reduce the noise effect.El-Sheimy et al. utilized wavelet denoising method to eliminate sensor noise in the initial alignment [19].An IIR filter combined with a Kalman filter was used to attenuate the influence of sensor noise and outer disturbance [20].A Vondrak low-pass filter, with the ability to reduce the high frequency noise in the force observation accurately, was applied to improve attitude accuracy [21].When using the low-pass filter, the difficulty is to find universal filter parameters applicable for dynamic environment, and the online processing is another problem.
In order to fulfill the requirements of high accuracy and short time for initial alignment, the same set of sensor data can be used repeatedly to conduct initial alignment.The SINS mathematical platform is adjusted accordingly with updated compensation parameters [22], and a fast compass alignment is feasible based on iterative calculation [23].The forward and backward processes can be effectively applied in INS alignment, and the analysis of forward and backward compass processes has been carried out using the error equations in frequency domain [24].In this research, a quasistationary condition is considered, which is characterized as having bounded position and attitude movement such as that produced by wind gusts and passenger [7].This is a typical situation for land vehicles.An innovative forward and backward data processing method is proposed to improve the alignment performance, an easily implemented Rauch-Tung-Striebel (RTS) smoother is applied to obtain smoothed attitudes in the backward resolution, and the obtained attitudes together with converged sensor biases are used as constraints in the repeated resolution process.
The rest of the paper is organized as follows.In Section 2, the alignment model on the quasi-stationary base is constructed, and the effect of sensor bias on alignment accuracy is analyzed.In Section 3, after the INS alignment process is studied, an improved RTS aided forward and backward alignment method is developed to shorten the alignment time.In Section 4, two simulation tests and a real test are conducted for verification.Finally, some conclusions are given.

Error Propagation Model for
the Quasi-Stationary Alignment , where    is the rotation matrix from the navigation frame (n-), which is denoted as north, east, and down (NED) frame, to the body frame (b-),  is heading angle, and  is local gravity constant, which can be derived from the gravity model.
Equation (1) then can be written as The pitch and roll angles can be calculated as where the subscripts , , and  represent forward, right, and down direction of the body frame.
The accuracy of the attitude estimates is limited by the accelerometer error; the resulting attitude errors can be described as where ∇ represents accelerometer bias and  is the misalignment error.
The INS tilt error is caused by the accelerometer error, for example, a milli-g accelerometer bias will induce 1 mrad error for roll and pitch estimates.Actually, promising attitude accuracy can be obtained for most of inertial sensors because the gravity is a relatively large quantity.
However, accurate self-alignment of the heading requires high quality inertial sensors.For the low cost inertial sensors, the heading is usually initialized using a magnetic compass.In the gyrocompassing process, the high quality gyros are used to sense the Earth's rotation.The gyro angular rates (   ) are related to the Earth's rate by with where Ω and  denote Earth's rate and latitude, respectively; the latitude can be provided by GNSS.For quasi-stationary conditions,    is a zero vector.Equation ( 6) can be written as The heading can be calculated as where the pitch and roll have already been calculated from leveling.The heading error is affected by gyro and accelerometer errors, which can be expressed as where  is gyro bias.
It can be seen that the heading accuracy is largely affected by the east gyro errors.We have known that the Earth rate is about 15 ∘ /h; for the low cost gyros, it cannot align itself because the noise levels are near or higher than the Earth rate.The sensor data actually reflects the magnitude of misalignment; for example, at latitude 45 ∘ , in order to obtain a 1 mrad heading initialing accuracy, the gyro should be accurate to about 0.01 ∘ /h.During the coarse alignment process, the inertial measurements are smoothed to reduce the adverse effects from the random noise.

Fine Alignment.
After the coarse alignment has been completed on the stationary base, the attitude misalignments converge to small values.To achieve a more accurate initial attitude estimate, sequential measurements are used to carry out a self-alignment over a period of time.In the quasistationary condition, zero-velocity measurements are used in a Kalman filter during the initial alignment phase, which is used to calibrate velocity and attitude.Inertial sensor errors, such as accelerometer bias and gyro bias, are usually estimated together.However, the effects of inertial sensor errors cannot be fully separated from the attitude errors in a stationary condition.
The dynamic model for the small orientation errors can be expressed as In addition, the velocity error model can be written as The state vector is denoted by where the subscripts N, E, and D denote the direction in the local navigation frame and V is velocity error.
Then the system state equation can be expressed as where the dynamic matrix is and  is the process noise,   and   are meridian radius and normal radius, respectively, h is the height of INS, and the inertial sensor errors are treated as random walk process.On a stationary base, the measurement equation can be constructed as where the measurement matrix  is where  denotes the difference between the INS output and external measurements, and it can be expressed as  = [      ]  .The zero-velocity update (ZUPT) is conducted for stationary INS.The symbol V is the measurement noise vector and  is identity matrix.The error states are predicted by where x+ −1 is the a posteriori state vector at epoch  − 1 and  −  is the a priori state vector at epoch k; Φ −1 is the state transition matrix from epoch  − 1 to epoch k, and it can be approximated as Φ =  + Δ; Δ is the time increment,  −  is the a priori covariance matrix of  −  , and  −1 is the process noise covariance matrix.The correction step is provided as [25] where   is the Kalman gain matrix,  +  is the a posteriori state vector at epoch k,  +  is the a posteriori covariance matrix of  +  , and   is the measurement noise covariance matrix.The estimated error states are fed back to update attitude estimation and compensate sensor output.

Improved RTS Forward and Backward Based Alignment
After analyzing the model combined with ( 13) and ( 15), we can conclude that velocity errors are directly observed, which results in rapid estimation of horizontal orientation errors for that the accelerometer biases have little effects.The down orientation error, however, is indistinguishable from the east gyro bias; the two error states are correlated with each other.In other words, the misalignment angles are directly related with sensor data.It requires a period of time to obtain stable attitudes.
For SINS, when the vehicle body is stationary, the zerovelocity updates are carried out continuously.The calculated misalignment angles are used to update the rotation matrix periodically.For the ideal error-free inertial sensors, the measured sensor data stays the same.However, in real applications, the inertial sensors are usually contaminated by a variety of error sources, such as bias, scale factor, and random noise.A set of sensor data and reference measurements are required to conduct the initial alignment.In order to improve the alignment accuracy, the repeated SINS resolution can be conducted using the same data set.
In the quasi-stationary conditions, the INS is disturbed by external loadings, such as mechanical vibration, wind effects, and human activity; the attitudes are varied at a relative low level.In the repeated resolution process, a normal forward SINS resolution is applied first, then a backward SINS resolution will be applied, the obtained smoothing results are used as external constraints to conduct forward SINS resolution again, and finally the more accurate alignment estimates are expected to be achieved.The forward-backward SINS resolution process is shown in Figure 1.
In tradition, the Rauch-Tung-Striebel (RTS) has been widely used as a smoother; the maximum likelihood estimates can be obtained using RTS with the forward solution [26].The main advantage is that the RTS is easily implemented and highly reliable.The RTS can be used to obtain the smoother solution for models (13) and (15) given that The RTS mean (   ) and covariance (   ) are calculated by the following equations: The RTS is initialized using the estimates and covariance derived from the standard Kalman filter at epoch  + 1; then a backward filter is performed using the initial estimates and its error covariance.Actually, the RTS can be implemented in an online manner; when the measurement update is finished using KF, the RTS smoother performs smoothing from the current epoch to the previous updating epoch using the latest updated estimates and predicted information.
For the quasi-stationary INS alignment, the smoothed attitude estimates and updated sensor errors can be obtained after RTS smoothing.In order to improve the alignment convergence rate and accuracy, the obtained attitude estimates can be used as external measurements to perform forward KF again due to the low dynamics encountered.On the other hand, the converged sensors error estimates can be used as external constraints.The measurement vector then becomes where , , and  are the roll, pitch, and heading difference in Euler angle between INS outputs and smoothed estimates.Note that Euler angle error in ( 21) is different from where    represents the rotation matrix from the misalignment error in the navigation frame to the Euler angle and is given by In the forward INS resolution process, the velocity errors and misalignment angles will be fed to INS resolution.The estimation of accelerometer and gyro bias is a slow process before converging to the stable value.By adding the forwardbackward resolution process, the converged sensor biases will be used to compensate inertial output.In the repeated forward resolution process, since more measurements are available in the Kalman filter, the observability can be improved and better navigation parameters can be obtained.The estimation procedure of the KF using the RTS derived attitude estimates is shown in Figure 2.

Simulation Test.
A static INS was considered in the simulation.The inertial sensor errors are listed in Table 1.The INS is assumed to be located at north latitude 34.2218 ∘ and east longitude 117.1404 ∘ , and the height is 70 m.The initial attitudes for the INS are 1 ∘ , 2 ∘ , and −30 ∘ for pitch, roll, and heading, respectively.The sensor sampling rate is 100 Hz.The INS resolution cycle is 0.01 s, while the ZUPT resolution cycle is set as 1 s.A total of 600 s sensor data are generated for alignment analysis.
In the first step, the coarse alignment should be conducted to obtain the initial estimates using the sensor output.The online smoothing strategy was applied to reduce the negative effect from the sensor random noise.The obtained attitude estimates are shown in Figure 3; it can be concluded that horizontal misalignment angles quickly converge to stable values because low level biases are simulated for the accelerometer.However, larger variations can be seen for the heading estimates in the initial period due to the existence of gyro biases.After the convergence period, the obtained INS attitude estimates are −36.6560∘ , 1.0417 ∘ , and 1.9559 ∘ for heading, pitch, and roll, respectively.It can be seen that the heading error is the most significant for the simulated INS.
After the coarse initial alignment has been completed, the misalignment angle converges to a small value.Under the condition of small misalignment, including small tilt misalignment and small heading misalignment, the ZUPT based fine alignment can be applied to improve the alignment accuracy.A twelve-state closed-loop Kalman filter is implemented to accomplish the fine alignment.
The initial settings for the Kalman filter are given as follows: The alignment results from the fine alignment are shown in Figure 4.It can be seen that the convergence rate for the heading is slow, it takes about 250 s to obtain stable value, and it still has small variations until the end of the data.The heading at the end epoch is −29.7381∘ ; the alignment accuracy is 0.2619 ∘ .On the other hand, the variations for the pitch and roll angles are smaller.The obtained mean value and standard deviation of the estimated pitch and roll are shown in Table 2.The alignment accuracy of pitch and roll is 0.0400 ∘ and 0.0403 ∘ , respectively, and it corresponds  to the limit alignment accuracy as derived from model (5).The estimates of accelerometer bias and gyro bias curves are shown in Figures 5 and 6, respectively.The curves in Figure 5 show that the horizontal accelerometer biases are hardly estimable for that they cannot be fully separated from the tilt errors.However, the down accelerometer bias is more readily estimated since the INS is almost leveled (within ±2 ∘ ) relative to the local horizontal.It can be seen that the estimation accuracy of tilt angle is limited to the accelerometer bias for the stationary INS.To separately observe these errors, maneuvers must be performed.On the other hand, we have known that the north gyro bias can be estimated since it is coupled to the east velocity error through the north orientation error.It can be seen that the horizontal gyro biases in the body frame can both be estimated since the system is not aligned to the local frame.The estimated gyro biases are used to compensate sensor data, thus improving the heading accuracy, the tendency of convergence of horizontal gyro biases corresponds to that of heading estimation, and the estimation curves of gyro biases converge to setting value at about 250 s.As expected, the down gyro bias is hardly estimable because it is directly coupled with down orientation error.
After the forward fine alignment process has been completed, the RTS smoother will be implemented with the knowledge of state estimates, state covariance, and state transition matrix derived from the forward ZUPT based Kalman filter.During the RTS smoothing process, the obtained error states are used to correct the attitude estimates again.In the repeated forward resolution process, the obtained attitude estimates from RTS smoothing plus zero velocity will be used as the external measurements, and the converged sensor biases will be used as constraints.The measurement covariance is derived from the smoothing covariance, as the tilt estimates obtain the limit accuracy, and the improvement is marginal.Here only the heading estimating performance will be analyzed.The heading estimates derived from the forward KF, RTS smoother, and forward-backward KF are shown in Figure 7.It can be seen that the accuracy of heading estimates has been improved after the RTS backward smoothing, especially in the initial period.However, relatively large oscillations of heading estimates still exist before 550 s.When applying the proposed repeated forward and backward resolution technique, more smooth heading estimates can be obtained be concluded that alignment accuracy and convergence rate have been improved using the repeated resolution scheme.
The previous analysis has confirmed that the proposed RTS aiding approach can improve the INS alignment performance by using a tactical IMU.In order to provide a comparison analysis for the real data, we have simulated IMU data of navigation grade, and the initial conditions are the same.The simulated accelerometer bias is 0.25 mg, and the velocity random walk is set as 20 g/ √ Hz.The simulated gyro bias is 0.05 ∘ /h, and the angular random walk is selected as 0.005 ∘ / √ h.The initial covariance for the heading misalignment is set as (1 ∘ /h) 2 .After a 60 s coarse alignment period, the obtained attitudes converge to −30.1725 ∘ , 1.0137 ∘ , and 1.9849 ∘ for heading, pitch, and roll, respectively, which are evidently better than simulation test 1, especially for the heading estimates.
The estimated IMU sensor biases are shown in Figures 8 and 9.It can be seen that similar convergence behavior is observed, and better estimation accuracy is obtained.The converged sensor biases are used as constraints during the RTS aiding process, which will benefit the alignment process.
The obtained heading estimates using different alignment schemes are shown in Figure 10.In the forward fine alignment period, the heading estimates slowly converge to the real heading; however the disturbance is obvious.By applying the RTS smoothing strategy, the accuracy of the heading estimates is improved; however the unstable estimation is also observed.When applying the forward-backward alignment scheme, the filter can quickly converge to stable value.It takes 17 s to converge to the stable value, and a slight degradation is observed during 150∼500 s, which is due to the inaccuracy of RTS observations.The obtained heading estimate in the final period using the RTS aided forward-backward scheme is almost identical to the true value, which proves the effectiveness of proposed algorithm.quality fiber optical gyros (bias repeatability ±0.05 ∘ /h) and three accelerometers (bias repeatability ±0.25 mg), was used to collect data at the rate of 100 Hz.The data length is around one hour.The raw data was recorded with a laptop and then used for postprocessing.As it was not possible to obtain a true value of the heading with the necessary precision, the attitudes obtained after one hour of convergence were used as reference.The experiment setup was shown in Figure 11.

Real Data
The attitudes estimated from coarse alignment are shown in Figure 12.It can be seen that the attitudes converge fast; it takes only several seconds to obtain the stable values, which is owing to the high quality inertial sensor that has been used.Larger variations can also be found for the heading estimates.After a 60 s convergence, the obtained attitudes are 0.7449 ∘ , 0.2050 ∘ , and 0.2376 ∘ for heading, pitch, and roll, respectively.
The same fine alignment process should be applied after the coarse alignment obtains the initial attitudes.The parameters for Kalman filter are determined based on the sensor specifications, the velocity random walk is set as 20 g/ √ Hz, and the angular random walk is set as 0.005 ∘ / √ h.The same is done with the simulation test: the heading initial standard deviation is set as 1 ∘ , which is used for the higher end  sensor.The attitude curves from the fine alignment are shown in Figure 13.It can be seen that that estimation of tilt angles is rather stable.The statistical characteristics of the pitch and roll for the real test are provided in Table 3; very small changes have been found compared to the reference values.On the other hand, the heading slowly converges to stable values.The heading estimate obtained at the end epoch is 0.6971 ∘ compared to the reference value 0.6200 ∘ .The obtained accelerometer biases and gyro biases are shown in Figures 14 and 15.Similar results can be seen for accelerometer bias estimates, and the down accelerometer bias quickly converges to stable value, while the horizontal accelerometer biases are hardly estimable.On the other hand, the north gyro bias is readily estimated.However, the east gyro bias and down gyro bias are hardly estimable in this case; this prevents further improvement of heading estimation.
When the forward KF alignment has been completed, the backward RTS smoothing is applied to obtain the smoothed attitude results.The repeated forward resolution is then applied with the aiding measurements from the RTS smoothing results.The headings estimated from the three strategies are shown in Figure 16.In can be seen that the heading estimates obtained from the repeated resolution strategy are smoother, and the convergence rate is faster.It takes about 250 s to converge to stable value, and obtained heading at the end epoch is 0.6822 ∘ , which is slightly better than forward KF.Compared to the simulation test, we can see that similar results can be obtained; the alignment accuracy is limited due to the sensor configuration and unavailable of accurate filter parameters.

Conclusions
In summary, this paper proposes an innovative data processing method to improve the SINS initial alignment performance on the quasi-stationary base.In this method, the forward fine alignment is first conducted to generate attitude estimates and sensor biases, and the RTS smoothing is then applied to obtain smoothed attitudes using the saved filter information.In the third step, the iterative processing is carried out with external attitude reference provided by RTS smoothing, and the converged sensor biases are used as additional constraints.By adopting the repeated forward and backward resolution, better navigation parameters are expected to be obtained.For verification, simulation analyses were conducted.The results have shown that initial alignment can be improved by iterative processing on the same set of sensor data.The estimation of horizontal gyro biases becomes feasible when the INS is not aligned to local frame; the improvement of heading is readily achieved with calibrated east gyro bias.The alignment time is shortened with repeated forward and backward resolution; for the tactical IMU data set, it takes about 200 s for the heading to converge to stable value; the final heading accuracy has also been improved from 0.2619 ∘ to 0.1522 ∘ .For simulated navigation grade IMU, by applying the RTS aided iterative processing scheme, it takes 17 s to converge to the stable value and the final obtained heading estimate is almost identical to true value.Meanwhile, the results from a real INS alignment test, which consisted of a high quality inertial sensor, have proved the improvement of initial alignment with the added forward and backward resolution process.

Figure 2 :Figure 3 :
Figure 2: The operation of RTS aided KF for initial alignment.

Figure 4 :
Figure 4: Estimated attitudes from fine alignment using the simulated data.

Figure 7 :
Figure 7: Estimated heading using the RTS aided forward and backward resolution scheme with the simulated data (Test 1).

Figure 10 :
Figure 10: Estimated heading using the RTS aided forward and backward resolution scheme with the simulated data (Test 2).

Figure 11 :
Figure 11: The setup of the test system for initial alignment.

Figure 12 :
Figure 12: Estimated attitudes from coarse alignment for the real test.

Figure 13 :Figure 14 :
Figure 13: Estimated attitudes from fine alignment for the real test.

Figure 15 :Figure 16 :
Figure 15: Estimated gyro bias for the real test.
sin  + sin  sin  cos  cos  cos  + sin  sin  sin  sin  cos  sin  sin  + cos  sin  cos  −sin  cos  + cos  sin  sin  cos  cos 2.1.Coarse Alignment.Here we are mainly discussing the practical aspects of alignment with regard to land vehicle navigation.For the land vehicles, when the INS is in quasistationary condition, that is, low-vibration environment, the self-alignment can be fulfilled with inertial measurements.The coarse alignment usually involves two steps.In the first step, the INS is leveled by initializing the pitch () and roll ( arctan 2 (  sin  −   cos ,   cos  +   sin  sin  +   sin  cos ) ,

Table 2 :
The statistical characteristics of the estimated pitch and roll from fine alignment using simulated data.
Test.A real test has been carried out to test the INS quasi-stationary alignment performance.The INS was set up on the roof of the School of Environment Science and Spatial Informatics (SESSI) building, on the campus of China University of Mining and Technology (CUMT).In order to provide a heading reference, a baseline aligned to local north was established with GNSS.During the test, the inertial sensor was aligned to make the -axis pointed to the north with the optical line-of-sight reference provided by the total station.Considering the positioning precision of 1-2 cm provided by GNSS, the obtained heading accuracy of the baseline is about ±0.1 ∘ .However, the heading of the INS is biased due to the imperfect installation.During the test, a high quality inertial sensor, which consists of three high

Table 3 :
The statistical characteristics of the estimated pitch and roll from fine alignment for the real test.