Aircraft attitude estimation of MEMS sensor based on modified particle filter

MA Wen-gang, WANG Xiao-peng, ZHANG Yong-fang, CHENG Dong-liang

(School of Electronic and Information Engineering, Lanzhou Jiaotong University, Lanzhou 730070, China)

0 Introduction

The aircraft attitude is an important part of aircraft navigation and attitude control system[1,2]. The precision of aircraft attitude directly affects the carrier attitude control precision and positioning accuracy. Attitude description parameters include mainly cosine matrix, Euler angle and attitude quaternion. The attitude quaternion[3-5]is widely used in flight control system based on its small computation, non-singularity and full pose. But the quaternion belongs to the rotation vector, and its four variables are not independent. It is constrained by the normalized parameters. In the combination of quaternion and nonlinear filtering algorithm, the method based on quaternion to extend Kaman filter algorithm[6,7]is commonly used. However, nonlinearity of the system is strong, and the attitude estimation is often inaccurate. For the actual strong non-linear and non-Gaussian problem, this simple method by using the mean and variance is not enough to represent the true state distribution. The method of particle filter (PF) algorithm[8,9]completely overcomes the problem of system nonlinearity and Gaussian hypothesis[10-12], and it becomes the most suitable filtering method for nonlinear system at present.

In this paper, an aircraft attitude estimation of MEMS sensor based on modified particle filter is proposed, which avoids the shortcomings of Newton method. Taking the STM32F103 as the control core, the attitude information module is composed of the MEMS sensor, the three-axis gyroscope and three-axis accelerometer. The latter two are used to collect the aircraft attitude information in real time, and the attitude estimation processing system is designed. The acquired attitude information data is sent to the STM32 microprocessor to preprocesses via the wireless transceiver module. The data fusion and attitude estimation are completed in the microprocessor.

1 Conjugate gradient method correction

The postures of accelerometer and magnetometer are better than the horizontal and magnetic north poles, which can show good performance under static state and non-magnetic interference mode. The magnetic field interference will bring a serious deviation. Only a sensor can not estimate the precise attitude information data. The corresponding components of magnetometer on the three axes of carrier coordinate system aremx,myandmz. The corresponding components on the three axes of the coordinate system areax,ayandaz.

1) The error function can be defined as

(1)

Quaternion optimization problem can be converted into error functionf(q)=E(q)TE(q). It is used to calculate the minimum value of the problem.

2) The conjugate gradient method is used to optimize the aircraft attitude. According to Eq.(2), vector posture quaternion can be updated as

Qmin,(n+1)=Qmin,n+γndn,

(2)

gn-1=fn-1,gn=fn,d1=-g,

(3)

(4)

whereγnis the optimal step size, anddnis a gradient and represents the iterative direction of the system.

3) According to the conjugate gradient method,f(q)=E(q)TE(q) can be calculated by

f=JT(q)E(q),

(5)

whereJT(q) is the Jacobin matrix ofE(q).

The attitude correction process shown in Fig.1 is designed by adopting the conjugate gradient method and MEMS sensor.

Fig.1 Iterative optimization of conjugate gradient method

In Fig.1,f(q)=E(q)TE(q) is the error objective equation. When the objective functionf(q) reaches the minimum value, the difference between the attitude angle and the true value of the quaternion is the smallest.

2 Modified particle filter fusion

2.1 Modified particle filter optimization

Particle scarcity is the main drawback of particle filtration. Doucet proved the inevitability of particle scarcity phenomenon in the particle filter algorithm. Therefore, the particle is assumed to initialize the state variablex0～p(x0), and the modified particle filter is defined as follows.

2) The PF algorithm is used to update the particles， and its general form can be expressed as

(10)

3) The corresponding weights of particles are calculated.

4) The re-sample particles and weights are calculated, and they are expressed as

(12)

In the experiment, the 8-dimensional trajectories of particles are analyzed to determine the optimal position of the particle motion in Fig.2. And the average fitness of particle swarm iteration can reach 96%-99%, and the attitude parameters optimized by particle swarm optimization are more credible.

Fig.2 Particle iteration processing

2.2 Weighted fusion

The error functionE(k) is established by using the conjugate gradient method to solve the quaternion update step sizeγnand the gradient directiondn. The measured attitude angle data is modeled by the particle, and the particle weight is updated by the observed value to obtain the estimation and variance of optimized state. The weight fusion is made to determine the optimal weighting factor based on the conjugate gradient method and the modified particle filter.

(13)

whereδ2is the variance of theisensor. According to the mean square error criterion, the weighting factor can be written as

(14)

whereδihas the minimum value, and its general form can be expressed as

(15)

The sensor weights are determined only by their variances. As long as the various sensor weights meet Eq.(15), the total mean square error is the minimum as

(16)

3 Hardware implementation

The three-axis magnetometer (TAM) and the gyro are used as attitude sensors. Sampling period of the TAM is 10 s. The 10-order international geomagnetic reference field (IGRF) model is used to approximate the magnetic field distribution, and the measured noise of magnetometer is assumed to be zero. The attitude calculation module takes STM32F103 as the control core. And MEMS sensor, three-axis gyroscope and three-axis accelerometer are used as the posture information collection module to calculate the attitude information data as shown in Fig.3. They communicate with STM32 via I2C protocol with data bus and clock bus. The acquired attitude information data is sent to the STM32 microprocessor via the wireless transceiver module. The microprocessor preprocesses the current gesture information. The data fusion and attitude estimation are completed in the microprocessor.

The system samples at 100 Hz and stores 500 points of data. Because the magnetometer did not be used, so the pitch angle and roll angle were static test, horizontal sliding and dynamic testing. After the processor completes the posture calculation, the data is sent to the host computer through the 2.4 GHz wireless data transmission module.

Fig.3 Hardware implementation

4 Experimental data analysis

4.1 MEMS performance parameters and performance specifications

6-axis MEMS can effectively reduce the measurement noise and improve the measurement accuracy by using advanced digital filtering technology. In the module, the attitude calculator is integrated with the proposed particle filter algorithm, which can output the current attitude of the module accurately in the dynamic environment. The attitude measurement accuracy is 0.01°, and the stability is extremely high. The performance is even better than some professional inclinometers. The MEMS sensor does not contain the magnetic field meter. There is no observation of the magnetic field on the yaw angle, so the yaw angle is calculated by the pure integral. It can only achieve a short time of the rotation angle measurement. AndX,Yaxis angles can be filtered through the gravity field correction. It will be no drift phenomenon.

Table 1 shows the physical quantities output by the MEMS sensor.

Table 1 MEMS sensor output data

The six-axis module uses a high-precision gyro accelerometer MPU6050, and the processor is used to read MPU6050 measurement data. This module allows the user to avoid developing MPU6050 complex I2C protocol. Sensor parameters are as follows: acceleration: 6.1e-5g; angular velocity: 7.6e-3(°)/s; acceleration: 0.01g; angular velocity: 0.05 (°)/s; stance measurement stability: 0.01 °.

4.2 Accuracy comparison of attitude angle measurement data

In order to test the effectiveness of fusion strategy, the quaternion method, the conjugate gradient method and the proposed method are used to estimate the attitude angle data. According to Tables 2-4, the attitude is measured by the quaternion method, and the error of attitude angle data is relatively large.

Table 2 Quaternion attitude estimation data

Table 3 Conjugate gradient method attitude estimation data

Table 4 Particle filter fusion optimization pose estimation data

After the estimation by the conjugate gradient method, the measurement error is reduced. But with the increase of measurement time, the error of measurement increases. The measurement error is kept near 2° after the weighted fusion algorithm is adopted. The convergence of measured value is good, which means that the weighted fusion method can reduce small gyro drift error. The measured value is relatively stable, and the attitude estimation data can maintain good convergence performance.

4.3 Relative error comparison of attitude estimation method

In order to test the validity of the attitude angle of proposed method, Figs.4 and 5 show the comparisons of three attitude angle measurement errors by using the proposed method. When the attitude change is large, the three kinds of attitude angle measurement errors reach 3° in Fig.4. The pitch angle vibration trend is relatively violent.

Fig.4 Attitude estimation when attitude change is large

Fig.5 Attitude estimation when attitude change is small

Meanwhile, the roll angle and heading angle vibration trends are relatively slow, and the maximum measurement error is about 3.5°. Three kinds of attitude angle measurements will tend to be stable after a certain time.

When the attitude change is small in Fig.5, the measurement error is up to 0.4°. The heading angle vibration trends are relatively intense, and the pitch angle and roll angle vibration trends are relatively slow. The maximum measurement error is about 0.3°. It shows the proposed particle filter weighted fusion method can reduce the measurement error. The aircraft has good static performance and dynamic performance.

The quaternion method, the conjugate gradient method and method in Ref.[4] are used to track the attitude of the vehicle for 120 s, as shown in Fig.6, respectively.

Fig.6 Comparisons of estimation errors in pitch angle attitude

The measured error is smaller than other methods by particle filter weighted fusion method. It indicates that the aircraft attitude estimation could remain relatively stable, and weighted fusion reduces the attitude angle estimation error.

4.4 Step tracking comparison

In order to detect tracking characteristics of the aircraft of proposed method, the feedback control system with the optimized method is added to the UAV model by using the Matlab software test environment in Fig.7.

Fig.7 Aircraft control platform

The step size of the simulation is set to 0.001 s, and the 4 order (Runge Kutta) is used to track the step. The yaw angle, pitch angle and roll angle are subjected to step tracking with amplitude of 30° in Figs.8-11.

Fig.8 z step curve

Fig.9 x,y step curves

It can be seen that the proposed method can better track the position and attitude of the aircraft from the step tracking curves of Figs.8-11. In the cases of external interference and uncertainties of UAV own parameters, the proposed method is more robust.

Fig.10 ψ step tracking contrast curve

Fig.11 θ,φ step tracking contrast curves

5 Conclusion

The weighted density of particle is obtained by combining the modified particle filter and the conjugate gradient method. The conjugate gradient method is weighted with the particle filter. The attitude analysis system based on STM32 is designed as the core, which can make the attitude of aircraft have good static and dynamic characteristics. The attitude angle based on modified particle filter weighted fusion is stable and accurate. Moreover, it has better robustness and stability.

[1] Duan Z S, Han C Z, Tao T F. Multi-sensor parameter estimation data fusion based on least squares criterion. Computer Engineering and Applications, 2004, 40 (15): 1-3.

[2] Ye Z F, Feng E X. Study on stabilization of two-wheel vehicle based on quaternion and kalman filter. Journal of Sensors and Telecommunications, 2012, 25(4): 524-528.

[3] Guo X H, Yang Z, Chen Z, et al. Application of EFF and complementary filters in flight attitude determination. Sensors and Microsystems, 2011, 30(11): 149-152.

[4] Pan Y. Attitude estimation of miniature unmanned helicopter using unscented kalman filter. In: Proceedings of International Conference on Transportation, Mechanical and Electrical Engineering, 2011: 1548-1551.

[5] Wang F, Zhu S h, Lei H J. Design of digital strapdown attitude and attitude system algorithm based on kalman filter. Journal of Chinese Inertial Technology, 2008, 16(2): 208-211.

[6] Qiao X W, Zhou W D, Ji Y R. Study on aircraft attitude estimation algorithm based on quaternion particle filter. Journal of Ordnance, 2012, 33(9): 1070-1075.

[7] Zhang L, Qin H C, Wang W B, et al. Intelligent wheelchair location method based on ultrasonic and trajectory estimation. Journal of Electronics Measurement and Instrumentation, 2014, 28(2): 62-68.

[8] Feng Y B, Li X S, Zhang X J. An adaptive compensation method of electronic magnetic compass direction error error. Journal of Instrumentation, 2014, 35(11): 2607-2614.

[9] Hong D, Liu G B, Chen H M, et al. Application of EKF for missile attitude estimation based on SINS/CNS integrated guidance system. In: Proceedings of Systems and Control in Aeronautics and Astronautics, 2010: 1101-1104.

[10] Fernando H C T E, De Silva A T A, De Zoysa M D C, et al. Modelling, simulation and implementation of a quadrotor UAV. In: Proceedings of IEEE International Conference on Industrial and Information Systems, 2013: 207-212.

[11] Ruan X G, Song K K. An adaptive extended kalman filterfor attitude estimation of self-balancing two-wheeled robot. In: Proceedings of International Conference on Electric Information and Control Enginee, 2011: 4760-4763.

[12] Jung K L, Edward J P, Stephen N R. Estimation of attitude and external acceleration using inertial sensor measurement during various dynamic conditions. IEEE Transactions on Instrumentation and Measurement, 2012, 61(8): 2262-2273.