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.

1. 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.

Figure 1

MEMS inertial measurement units.

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.

2. Model of SINS Transfer Alignment

2.1. 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].

2.2. 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, where is the velocity error of the vehicle in n-frame. is the equivalent bias drift error of the accelerometer. and are the angular rate error of and , respectively [12].

2.3. Flexural Deflection Error Model

When the vehicle is mounted on the aircraft, the airframe may suffer from flexural deflection caused by the disturbance of airflow or some maneuvers such as turns, climbs, and swings. This problem will lead to instability or large error of the alignment. Therefore, it is necessary to take the dynamic flexural deflection into consideration during the process of the transfer alignment [13].

The flexural deflection of the aircraft tends to be a stochastic progress, which can be modeled as a second-order Markov progress actuated by white noise. Supposing the deflection between the main SINS and the slave SINS in three axes are independent, the flexural deflection can be described as: where is the angular vector of the flexural deflection and its variance is . is the white noise with PSD of , which means . is a constant value. The relationship between , , and can be shown as . is the correlation time of the stochastic progress. The relationship between and can be shown as .

Suppose that is the flexural deflection in the body frame (b-frame) and is the deflection angular rate of slave SINS with respect to the main SINS. Then we have: where , , and are the white noise. , where is the correlation time of the three axes.

The equations above can be rewritten as: where , ,

2.4. 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 and are the attitude error and velocity error of the slave SINS. and are the bias drift of gyroscope and accelerometer. is the misalignment angle of the body with respect to the theoretical mounting location. and 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 , , and are the velocity of the vehicle in north, west, and down directions. and are the latitude and altitude of the vehicle. and are the meridian circle radius and prime vertical circle radius of the earth. is the earth rotation rate. are the output specific force of the slave SINS. and are the noise of the gyroscope and accelerometer, which are supposed as the white noise.

The velocity measurement equation can be written as: where is the velocity error of the slave SINS. is the measurement noise of velocity, . is the speed caused by the vibration and flexural deflection. is the velocity error of the main SINS. is the compensation value of the lever arm effect [15].

The attitude measurement value 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. 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 is the 21-dimension state vector. is the 6-dimension measurement vector. is the 21-dimension state noise variance vector. is the 6-dimension measurement noise variance vector. is the 21 × 21 state transfer matrix of the system.

3. The Improved Adaptive Incremental Kalman Transfer Alignment Algorithm

The measurement noise of the “Velocity and Attitude” matching method is mainly caused by the distortion of flexural deflection. However, the prior knowledge of 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 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.

3.1. The Adaptive Kalman Filter (AKF)

The state equation and measurement equation of the conventional adaptive Kalman filter: where is the n-dimension state vector, is the m-dimension measurement vector, and is the p-dimension state noise variance vector. is the m-dimension measurement noise variance vector. is the n × n state transfer matrix of the system.

Supposing that and satisfy the following relations: where is p × p dimension symmetric non-negative definite matrix of variance. is m × m dimension symmetric positive definite matrix of variance. is Kronecher-δ function.

The adaptive Kalman filter can be obtained by recursion. The estimated value of state vector represented by at the time of is: where

Suppose , and the mean value of system noise and its variance , the mean value of measurement noise and its variance 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.

3.2. The Adaptive Incremental Kalman Filter (AIKF)

In real flight, the change of two adjacent measurement values and is small. If we choose the incremental of two successive measurement values as the measurement value, which is represented by , 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 is the incremental of the m-dimension measurement vector, and , where is the measurement vector.

According to the principles of the independent incremental stochastic progress, the and are independent.

The adaptive incremental Kalman filter can be obtained by recursion. The estimate value of state vector , represented by at time is: where

In the progress of SINS transfer alignment, the mean value of system noise and its variance , the mean value of measurement noise and its variance are all unknown time-varying parameters, which needs to be estimated using the maximum verified method.

The estimation value and are biased using the equation above. In order to obtain the unbiased estimation, we set the innovation of 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, the is biased. Then we introduce the suboptimal unbiased maximum a posteriori (MAP) estimator :

Similarly, we have:

The recurrence formulas of adaptive incremental Kalman filter together with the suboptimal unbiased MAP estimator are: where .

When the noise parameters are unknown and time-varying [18,19], the estimator is: where , , b is the forgetting factor. Usually . When , , 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.

3.3. 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.

Figure 2

Comparison of AKF and AIKF. (a) Estimation result of AIKF and AKF; (b) Estimation error of AIKF and AKF.

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.

		AIKF	AKF	KF
Noise estimator	q^_k	2n2 + 3n	2n2 + 3n	-
	Q^_k	2n2 + m2 + 2nm2 − nm + 2n2m + 4n3	4n3 + 2n2m + 2n2 + 3mn − n	-
	r^_k	4mn + 3m	2mn + 3m	-
	R^_k	12mn2 + 8m2n + 3m2 − 6mn	4m2 + 2mn2 + 2m2n − mn	-
Prediction update	X^_k,k−1	2n2	2n2	2n2
	Pk,k−1	4n3 − n2	4n3 − n2	4n3−n2
Measurement update	X^_k	10mn + 2m − n	4mn + m	nm + n + m
	Pk	8mn2 + 6m2n-m2 − 4mn	4mn2 + 4m2n − 2mn	2mn2 + 2n3 − n2
	Kk	4n2m + 2n3 + 2nm2 − n2 − 2nm + m3	2mn2 + 2m2n − 2mn + m3	4n2m + 4nm2 − 3nm + m3
In total		4n2 + 2n + 10n3 + 5m + 3m2 + 18nm2 + nm + 26n2m + m3	8n3 + 10n2m + 5n2 + 4mn + 2n + 4m + 4m2 + 8m2n + m3	6n3 + 6mn2 + 4nm2 − 2nm + m3 + m + n
n = 21, m= 6		177,300	109,731	74,457

In Table 1, we list the calculation amount of AIKF, AKF, and KF. The calculation amount is evaluated by the floating-point operations (flops), which is defined as the operation of adding, decreasing, multiplying, or dividing between two floating numbers. Supposing the dimension of the state vector is n and the measurement vector dimension is m, we can calculate the total flops of each filter algorithm. In SINS transfer alignment, the state vector n is 21 and the measurement vector m is 6. Then the total flops of AIKF, AKF, and KF are 177,300, 109,731, and 74,457, respectively. AIKF and AKF are more complex than KF because of the on-line noise estimation part. AIKF increases 61.6% calculation amount compared to AKF. However, the formulas of AIKF are still simple enough for practical application.

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.

4. 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.

Figure 3

Schematic diagram of the transfer alignment algorithm.

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.

Table 2

The parameters of the MEMS IMU.

	Gyro	Accelerometer
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 Figure 4 and Figure 5.

Figure 4

Comparison of estimation result using KF and AIKF. (a) Estimation of attitude error using KF; (b) Estimation of attitude error using AIKF; (c) Estimation of velocity error using KF; (d) Estimation of velocity error using AIKF; (e) Estimation of gyroscope bias using KF; (f) Estimation of gyroscope bias using AIKF; (g) Estimation of accelerometer bias using KF; (h) Estimation of accelerometer bias using AIKF.

Figure 5

Comparison of estimation error result using KF and AIKF. (a) Estimation error of attitude error using KF; (b) Estimation error of attitude error using AIKF; (c) Estimation error of velocity error using KF; (d) Estimation error of velocity error using AIKF; (e) Estimation error of gyroscope bias using KF; (f) Estimation error of gyroscope bias using AIKF; (g) Estimation error of accelerometer bias using KF; (h) Estimation error of accelerometer bias using AIKF.

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.

Figure 6

Part of the estimation error result using KF and AIKF. (a) Estimation error of gyroscope bias using KF; (b) Estimation error of gyroscope bias using AIK.

In Table 3 and Table 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.

Table 3

Bias estimation result of two algorithms.

Algorithm	Estimation Error of Gyroscope Bias/(°/h)			Estimation Error of Accelerometer Bias/(mg)
	x-Axis	y-Axis	z-Axis	x-Axis	y-Axis	z-Axis
KF	6.94	6.71	7.00	6.18	1.36	0.21
AIKF	0.81	0.81	0.73	0.81	0.14	0.02

Table 4

Main functional specification of MEMS gyroscope.

Algorithm	Estimation Error of Attitude Error/(′)			Estimation Error of Velocity Error /(m/s)
	x-Axis	Y-Axis	z-Axis	x-Axis	y-Axis	z-Axis
KF	10.42	18.81	4.77	0.0151	0.0151	0.0147
AIKF	1.33	2.46	0.48	0.0016	0.0015	0.0015

5. 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.