Rapid Transfer Alignment of MEMS SINS Based on Adaptive Incremental Kalman Filter

In airborne MEMS SINS transfer alignment, the error of MEMS IMU is highly environment-dependent and the parameters of the system model are also uncertain, which may lead to large error and bad convergence of the Kalman filter. In order to solve this problem, an improved adaptive incremental Kalman filter (AIKF) algorithm is proposed. First, the model of SINS transfer alignment is defined based on the “Velocity and Attitude” matching method. Then the detailed algorithm progress of AIKF and its recurrence formulas are presented. The performance and calculation amount of AKF and AIKF are also compared. Finally, a simulation test is designed to verify the accuracy and the rapidity of the AIKF algorithm by comparing it with KF and AKF. The results show that the AIKF algorithm has better estimation accuracy and shorter convergence time, especially for the bias of the gyroscope and the accelerometer, which can meet the accuracy and rapidity requirement of transfer alignment.


Introduction
Mirco-Electronical Mechanical System Inertial Measurement Unit (MEMS IMU), as shown in Figure 1, has been widely used in the Strapdown Inertial Navigation Systems (SINS) field in recent years because it has low cost, small size, and low power consumption [1]. In airborne MEMS SINS application, before the operation of the slaved SINS, it must be initially aligned using the master SINS and Kalman filter [2]. However, MEMS IMU has poor bias repeatability and bias drift level owing to the low-cost fabrication process, which may lead to large attitude error of the SINS in long time navigation cases [3,4]. A gyroscope with hundreds of deg/h of bias may lead to large nonlinear error in the progress of SINS transfer alignment. In the meantime, the uncertainty of noise, the lever arm effect, elastic distortion, and other disturbance during some violent maneuvers in practical application will make the transfer alignment more complex [5]. Besides, in airborne transfer alignment, the system errors of the measurement equation are usually unknown and the model parameters are also uncertain, which may lead to large error and bad convergence of the Kalman filter. Based on these studies on the alignment algorithm, we proposed an improved transfer alignment algorithm based on AIKF. The paper is organized as follows. First, the model of SINS transfer alignment is defined, which includes the SINS mechanized arrangement, the SINS error model, the flexural deflection error model, and the state and measurement equations of the transfer alignment. Then, the algorithm of AIKF is presented with detailed description. The calculation amount and the performance of AKF and AIKF are also compared. Finally, a digital simulation test based on MATLAB is designed. The results are compared with traditional methods to test its performance.

SINS Mechanized Arrangement
The local geographical coordinate is chosen as the navigation reference frame (n-frame), and the SINS mechanized arrangement equations are: where is the direction cosine matrix of the body frame with respect to the n-frame. is the velocity of the vehicle with respect to the earth in the navigation frame.
is the specific force of the vehicle.
is the rotation angular rate of n-frame with respect to the inertial frame (i-frame). is the earth rotation angular rate.
is the rotation angular rate of n-frame with respect to the earth.
is the local gravity acceleration in n-frame [11].

SINS Error Model
The attitude error equation of SINS is, where is the attitude error angle of the slave SINS between the calculated n-frame and the real n-frame.
is the rotation angular rate of n-frame with respect to the i-frame, and is the error of . is the direction cosine matrix of the body frame with respect to the n-frame. is the equivalent bias drift error of the gyroscope.
The velocity error equation of SINS is, In order to improve the performance of the filter, many researchers are involved in developing improved filtering algorithms. In [6] an H∞ filter and Unscented Transformation (UT) algorithm are introduced. When applied to a nonlinear model, a model with colored noise, or an unmatched model, H∞ filter is still robust. An adaptive Unscented Particle filter is introduced in [7] to solve the initial alignment of SINS with large misalignment. In [8], a robust adaptive filter algorithm based on local observability analysis is developed for rapid transfer alignment. Simulation results indicate that the proposed method has better performance and shorter alignment time. In [9], a new multiple fading factors Kalman filtering algorithm is presented to solve the problem that the Kalman filter cannot give the optimal solution when the accurate system model and stochastic information are unknown. It uses the innovation sequence to compute multiple fading factors to scale the predicted covariance matrix. A similar conclusion can be drawn in [10]. By carefully designing a filter, we can develop an algorithm that is less sensitive to uncertain noise and has a better estimation effect, which is important for transfer alignment. From these references, we can see that the proper filter algorithm for MEMS SINS transfer alignment should be more robust and adaptive. This is the main idea of our research.
Based on these studies on the alignment algorithm, we proposed an improved transfer alignment algorithm based on AIKF. The paper is organized as follows. First, the model of SINS transfer alignment is defined, which includes the SINS mechanized arrangement, the SINS error model, the flexural deflection error model, and the state and measurement equations of the transfer alignment. Then, the algorithm of AIKF is presented with detailed description. The calculation amount and the performance of AKF and AIKF are also compared. Finally, a digital simulation test based on MATLAB is designed. The results are compared with traditional methods to test its performance.

SINS Mechanized Arrangement
The local geographical coordinate is chosen as the navigation reference frame (n-frame), and the SINS mechanized arrangement equations are: where C n b is the direction cosine matrix of the body frame with respect to the n-frame. V n e is the velocity of the vehicle with respect to the earth in the navigation frame. f n is the specific force of the vehicle. ω n in is the rotation angular rate of n-frame with respect to the inertial frame (i-frame). ω n ie is the earth rotation angular rate. ω n en is the rotation angular rate of n-frame with respect to the earth. g n is the local gravity acceleration in n-frame [11].

SINS Error Model
The attitude error equation of SINS is, where ϕ is the attitude error angle of the slave SINS between the calculated n-frame and the real n-frame. ω n in is the rotation angular rate of n-frame with respect to the i-frame, and δω n in is the error of ω n in . C n b is the direction cosine matrix of the body frame with respect to the n-frame. ε b is the equivalent bias drift error of the gyroscope.
The velocity error equation of SINS is, .
δV n e = f n × ϕ n − (2ω n ie + ω n en ) × δV n e + V n e × (2δω n ie + δω n en ) where δV n e is the velocity error of the vehicle in n-frame. ∇ b is the equivalent bias drift error of the accelerometer. δω n ie and δω n en are the angular rate error of ω n ie and ω n en , respectively [12].

State and Measurement Equations of the Transfer Alignment
In 1989, Kain and Cloutier proposed the "Velocity and Attitude" matching method, which combined the advantage of velocity matching with attitude matching [14]. This algorithm has better alignment accuracy and costs less alignment time so it has been used in practical applications. Thus, in this paper we propose a new filter algorithm based on this matching method.
Based on the study of SINS error model, IMU error, misalignment error, and flexural deflection, we select the attitude error of the slave SINS, velocity error, bias drift of gyroscope and accelerometer, the misalignment error, the flexural deflection angular, and its angular rate as the state of the filter: where ϕ n and δV n e are the attitude error and velocity error of the slave SINS. ε bs b and ∇ bs b are the bias drift of gyroscope and accelerometer. µ b f is the misalignment angle of the body with respect to the theoretical mounting location. θ b f and ω b f are the flexural deflection angle and angular rate.
Combined with Equations (2), (3) and (7), the state function of transfer alignment can be written as follows: In this equation, where V N , V E , and V D are the velocity of the vehicle in north, west, and down directions. L and h are the latitude and altitude of the vehicle. R M and R N are the meridian circle radius and prime vertical circle radius of the earth. ω ie is the earth rotation rate. f n x , f n y , f n z are the output specific force of the slave SINS. ε bs w and ∇ bs w are the noise of the gyroscope and accelerometer, which are supposed as the white noise.
The velocity measurement equation can be written as: where δV n e,s is the velocity error of the slave SINS. V n V is the measurement noise of velocity, V n V = V n w − δV n e,m − δV n LA . V n w is the speed caused by the vibration and flexural deflection. δV n e,m is the velocity error of the main SINS. δV n LA is the compensation value of the lever arm effect [15]. The attitude measurement value Z θ can be obtained by matching the attitude matrixes of the main SINS and slave SINS. Considering the flexural deflection equation: where µ is the error angle between the main SINS and slave SINS. v n θ is the measurement noise of attitude error.
Combined with (11) and (12), the measurement equation can be written as: where According to Equations (9) and (13), we have the transfer alignment equation of "Velocity and Attitude" matching algorithm.
where X k is the 21-dimension state vector. Z k is the 6-dimension measurement vector. W k−1 is the 21-dimension state noise variance vector. V k is the 6-dimension measurement noise variance vector. Φ k,k−1 is the 21 × 21 state transfer matrix of the system.

The Improved Adaptive Incremental Kalman Transfer Alignment Algorithm
The measurement noise V k of the "Velocity and Attitude" matching method is mainly caused by the distortion of flexural deflection. However, the prior knowledge of V k may be different from a practical situation because of the change of slaved SINS location and the maneuvers of the aircraft. The state noise variance vector W k−1 can also be different from its prior knowledge because the character of the MEMS IMU noise is easily affected by the temperature and vibration. Traditional Kalman filter is only available when the prior knowledge of the state noise and measurement noise are known and their character do not change, which may lead to large alignment error. Although conventional adaptive Kalman filter is able to estimate the system noise on-line, its estimation accuracy is still not good when the system noise changes greatly. In order to improve the accuracy of the filter, the adaptive incremental Kalman filter is proposed.

The Adaptive Kalman Filter (AKF)
The state equation and measurement equation of the conventional adaptive Kalman filter: where X k is the n-dimension state vector, Z k is the m-dimension measurement vector, and W k is the p-dimension state noise variance vector. V k is the m-dimension measurement noise variance vector. Φ k,k−1 is the n × n state transfer matrix of the system. Supposing that W k and V k satisfy the following relations: The adaptive Kalman filter can be obtained by recursion. The estimated value of state vector X k represented byX k at the time of t k is: , and the mean value of system noise q k and its variance Ω k , the mean value of measurement noise r k and its variance R k are estimated using maximum verified method: The adaptive Kalman Filter is able to estimate the system and measurement noise. However, in practical application, such as airborne transfer alignment of SINS, the disturbance is very complex and the character of the system and measurement noise may change fast with time, which may lead to large estimation error by using the adaptive Kalman Filter. In order to improve the performance, we can apply the adaptive incremental Kalman Filter to airborne SINS transfer alignment.

The Adaptive Incremental Kalman Filter (AIKF)
In real flight, the change of two adjacent measurement values Z k and Z k−1 is small. If we choose the incremental of two successive measurement values as the measurement value, which is represented by ∆Z k , the system error can be reduced. Based on this idea, we can obtain the state equation and measurement equation of the incremental Kalman filter [16,17]: where ∆Z k is the incremental of the m-dimension measurement vector, and According to the principles of the independent incremental stochastic progress, the ∆Z k and ∆Z k−1 are independent. The adaptive incremental Kalman filter can be obtained by recursion. The estimate value of state vector X k , represented byX k at time t k is: where In the progress of SINS transfer alignment, the mean value of system noise q k and its variance Q k , the mean value of measurement noise r k and its variance R k are all unknown time-varying parameters, which needs to be estimated using the maximum verified method.
The estimation valueQ k andR k are biased using the equation above. In order to obtain the unbiased estimation, we set the innovation of ε k = ∆Z k − ∆Z k,k−1 and we have: According to Equation (19), we have: Then we can see that the estimation of the mean value is unbiased. Also, we have: So, theR k is biased. Then we introduce the suboptimal unbiased maximum a posteriori (MAP) estimatorR k :R Similarly, we have: The recurrence formulas of adaptive incremental Kalman filter together with the suboptimal unbiased MAP estimator are: When the noise parameters are unknown and time-varying [18,19], the estimator is: is the forgetting factor. Usually 0.95 < b < 0.995. When b → 1 , d k−1 → (1/t) , which is in accord with the Sage and Husa adaptive algorithm. The forgetting factor should be adjusted according to the character of the noise. If the frequency band of the noise is low, the value should be close to 1, otherwise the value should be reduced. The forgetting factor b can control the memory length of the filter to strengthen the estimation of the lately measurement data and reduce percentage of the old data.

Comparison of AKF and AIKF
To compare the estimation accuracy, the two methods of AKF and AIKF are simulated under the same condition. The results are presented as Figure 2. In Figure 2, we compare the performance of AKF and AIKF. During the steps 200~400 and steps 600~800, the measurement noise becomes worse abruptly. We can apparently see that AIKF has better estimation accuracy. The estimation error of AIKF is less than 0.5 while the error of AKF can be larger than 2.5. Besides, we can see that the estimation value of AIKF is still very smooth in the whole progress when the measurement noise changes. On the contrary, AKF is more easily affected by the disturbance of the measurement noise. worse abruptly. We can apparently see that AIKF has better estimation accuracy. The estimation error of AIKF is less than 0.5 while the error of AKF can be larger than 2.5. Besides, we can see that the estimation value of AIKF is still very smooth in the whole progress when the measurement noise changes. On the contrary, AKF is more easily affected by the disturbance of the measurement noise. To compare the two methods in a more comprehensive way, the calculation amount of the different filtering algorithms are also considered, as is shown in Table 1. Table 1. Calculation amount of each algorithm.
From the above comparison of conditional AKF and AIKF, we can obviously see that AIKF has better estimation accuracy, which is also more robust to the disturbance of system noise compared with traditional AKF. Though AIKF has a larger calculation amount, the formulas of AIKF are still simple enough to satisfy the real-time requirement in practical application.

Simulation Test and Results
In order to test the validity and accuracy of this algorithm, a simulation test is designed using MATLAB, which is shown in Figure 3. simple enough to satisfy the real-time requirement in practical application.

Simulation Test and Results
In order to test the validity and accuracy of this algorithm, a simulation test is designed using MATLAB, which is shown in Figure 3.  Suppose the aircraft is moving straightly at 180 m/s and the yaw is −30 • . During the flight, the aircraft makes two 20 • swing maneuvers at the 10th second and 33rd second, respectively. In the meantime, the dynamic flexural deflection variance is set to 6 , 10 , and 7 , respectively. The correlation time parameters are 0.5, 0.4, and 10 s, respectively. The parameters of the MEMS IMU are shown in Table 2. The aircraft attitude, velocity, the flexural deflection angle and its angular rate are generated by the maneuvers simulator. The transfer alignment progresses are calculated using KF and AIKF, respectively. The results are shown in Figures 4 and 5.
Comparing the Figure 5g with Figure 5h, we can see the estimation error of accelerometer bias has been reduced to less than 1 mg rapidly during the first swing maneuvers using AIKF, while the estimation of accelerometer bias is much slower and its error is still around 7 mg using KF.
Comparing Figure 5e with Figure 5f, we can see the estimation error of gyroscope bias has been reduced to 20 • /h within 2 s using AIKF, while KF takes 10 s. If we magnify the error curve of the estimation error, as we show in Figure 6, we can see that the estimation error of gyroscope bias has been reduced from 20 • /h to 13 • /h in 2.5 s using KF while the AIKF reduced the error from 9 • /h to 1.5 • /h. The final accuracy is 5 • /h by KF and 0.6 • /h by AIKF, respectively. The AIKF costs less convergence time and has much better accuracy than KF.

Bias drift
250 °/h 10 mg Noise 0.5 °/√h 1 mg/√Hz The aircraft attitude, velocity, the flexural deflection angle and its angular rate are generated by the maneuvers simulator. The transfer alignment progresses are calculated using KF and AIKF, respectively. The results are shown in Figures 4 and 5.      Comparing the Figure 5g with Figure 5h, we can see the estimation error of accelerometer bias has been reduced to less than 1 mg rapidly during the first swing maneuvers using AIKF, while the estimation of accelerometer bias is much slower and its error is still around 7 mg using KF.
Comparing Figure 5e with Figure 5f, we can see the estimation error of gyroscope bias has been reduced to 20 °/h within 2 s using AIKF, while KF takes 10 s. If we magnify the error curve of the estimation error, as we show in Figure 6, we can see that the estimation error of gyroscope bias has

Conclusions
In this paper, we develop the transfer alignment model based on the "Velocity and Attitude" matching method and the flexural deflection model. An improved AIKF algorithm is proposed to solve the problem that the state noise and measurement noise parameters cannot be accurately measured. Also, the methods of conditional AKF and AIKF are compared in terms of estimation accuracy and calculation amount, AIKF has better estimation accuracy and is more robust to the disturbance of system noise compared with traditional AKF. Though AIKF has a larger calculation amount, it can still satisfy the real-time requirement in practical application. A simulation system is designed to compare AIKF with KF. The results show that the estimation error of the gyroscope bias is less than 1°/h and the estimation error of the accelerometer bias is less than 1 mg by using AIKF, which is five times better than KF and takes less alignment time. This method can estimate the bias of the IMU, initial attitude, and velocity of SINS in a short time. In Tables 3 and 4, we can see that the estimation error of gyroscope bias is less than 1 • /h, the accelerometer bias is less than 1 mg, the attitude error is less than 2.5 and the velocity error is less than 0.002 m/s. The forgetting factor b can control the memory length of the filter. The old knowledge of measurement data can be forgotten so that the disturbance of new measurement data will not affect the filter. So AIKF can achieve better accuracy, especially in a complicated environment. The AIKF has almost five times better accuracy than KF, which means AIKF can satisfy the requirement of airborne transfer alignment.

Conclusions
In this paper, we develop the transfer alignment model based on the "Velocity and Attitude" matching method and the flexural deflection model. An improved AIKF algorithm is proposed to solve the problem that the state noise and measurement noise parameters cannot be accurately measured. Also, the methods of conditional AKF and AIKF are compared in terms of estimation accuracy and calculation amount, AIKF has better estimation accuracy and is more robust to the disturbance of system noise compared with traditional AKF. Though AIKF has a larger calculation amount, it can still satisfy the real-time requirement in practical application. A simulation system is designed to compare AIKF with KF. The results show that the estimation error of the gyroscope bias is less than 1 • /h and the estimation error of the accelerometer bias is less than 1 mg by using AIKF, which is five times better than KF and takes less alignment time. This method can estimate the bias of the IMU, initial attitude, and velocity of SINS in a short time.