Dynamic Modelling for MEMS-IMU/Magnetometer Integrated Attitude and Heading Reference System

International Global Navigation Satellite Systems Society IGNSS Symposium 211 University of New South Wales, Sydney, NSW, Australia 15 17 November, 211 Dynamic Modelling for MEMS-IMU/Magnetometer Integrated Attitude and Heading Reference System Wei Li (1) 1. School of Electronics and Information/Northwestern Polytechnical University/China 2. School of Surveying and Spatial Information Systems/ University of New South wales/australia Phone: 61-4486982, wei.li@student.unsw.edu.au Jinling Wang (2) School of Surveying and Spatial Information Systems/ University of New South wales/australia Phone: 61-4463668, jinling.wang@unsw.edu.au Zhanrong Jing (3) School of Electronics and Information/Northwestern Polytechnical University/China Phone: +86 29 8849294, jingzr@nwpu.edu.cn ABSTRACT The MEMS based Inertial Measurement Unit (IMU) has a wide range of applications due to its low-cost, small size, and low power consumption. However, its performance is affected by sensor noise, bias, and scale factor error. So the traditional strap-down algorithm, only using low-cost MEMS, cannot satisfy the attitude and heading performance requirements. To achieve a long term stable solution, modern attitude and heading reference systems (AHRS) generally integrate IMU with other sensors, such as magnetometers. This paper proposed a new dynamic modelling method for low-cost IMU and magnetometer integrated AHRS. In this method, the system dynamics model is based on the Psi-angle error equation. The acceleration residuals in North and East detraction and heading residual are chosen as the observation vectors, and the corresponding system observation model has been developed. In order to yield optimal estimates, an adaptive Kalman filter is adopted, in which the measurement noise covariance matrix is tuned adaptively according to the system dynamics sensed by the accelerometers. This proposed method doesn t need to model the system angular motions, avoids the nonlinear problem of system dynamic model, and considers the influence of dynamic acceleration to the filter. The performance tests are conducted with a low-cost AHRS sensor, and the results show that the proposed method is a promising alternative for the AHRS. KEYWORDS: AHRS; IMU; MEMS; Adaptive Kalman Filter 1

1. INTRODUCTION MEMS based Inertial Measurement Unit (IMU), which includes MEMS accelerometers and gyroscopes, is a chip-based inertial technology for wide applications. With given initial angles and gyroscope measurements, an inertial navigation system (INS) can provide the Euler angles (heading, pitch and roll) results through the Direction Cosine Matrix (DCM) updating. However, such Euler angles drift quickly in time, due to the high sensor noise, bias, and scale factor error of the low-cost MEMS sensor (Geiger et al. 28). Magnetometers, which sense the geomagnetic field, can provide a heading output with the tilt angles (pitch and roll) given by accelerometers, which can be used to estimate the gyroscope drift and compensate the attitude errors of INS. As a result, MEMS based IMU integrated with magnetometers is a suitable system configuration that is widely applied in Attitude and Heading Reference System (AHRS) (Wang et al. 24). Koifman et al (199) presented an Euler angle based method for AHRS. In their model the body angular rates were modeled according to the aircraft s dynamics, which could provide a more accurate orientation solution. However, the precise modelling of the body angular motion is quite difficult for general applications. Also, this method uses the Euler angle kinematics model, which is a nonlinear derivative equation, so the Euler angle based method suffers from the nonlinear problem. Foxlin (1996) presented an Euler angle error based method to integrate IMU with magnetometer, and Setoodeh et al. (24) used a similar method to achieve the integration for the AHRS. In their papers, three Euler angle errors and three gyro biases were estimated as states in the Kalman filter. Being different from the methods above, their methods do not model the body angular motion. However, since the Euler angle errors in their method are expressed in the body frame (b frame), their system equations also suffer from the nonlinear problem. In this paper, the navigation frame is the local-level local-north frame, and the system dynamics model is based on the Psi-angle error equation (Weinred and Bar-Itzhack 1978). The angle error between the true navigation frame (n frame) and computed navigation frame ( frame) (Blankinship, 28), the gyro bias and the scale factor error are chosen as the state vectors to avoid the nonlinear problem of system dynamic model. The corresponding system observation model has been developed. An adaptive Kalman filter is adopted to yield optimal performance. The measurement noise covariance matrix of the filter is tuned adaptively according to a system dynamic acceleration scalar, which is defined to monitor the magnitude of system dynamic. Several experiments are conducted with an Xsens MTi sensor, which is a miniature AHRS composed by MEMS gyro, accelerometer and magnetometer. The experimental results show that the proposed method improves the orientation performance of the sensor, and can be a promising alternative for the AHRS. 2. Kalman Filter Design for the Integrated Attitude and Heading Reference System 2.1 System Dynamics Model Due to the errors of gyros, the computed navigation frame ( frame) is different from the true navigation frame (n frame) (Goshen-Meskin and Bar-Itzhack, 1992). Using the Psi-angle 2

Error model, the system dynamics model of the integrated system could be defined as: (1) where is the process noise vector and the state vector is constructed as : (2) where is the angular difference between the n frame and the frame, is the gyro bias expressed in the b frame, and is the scale factor error expressed in the b frame. The system matrix F is defined as: [ ( ) ] (3) where is the rotation rate of the frame respect to the inertial frame (i frame) expressed in the frame, is the skew symmetric matrix of, is the DCM from the b frame to the frame, and represents the angular increment in the b frame measured by 3 axis gyros. 2.2 Measurement Model Based on the magnetic compass heading and tilt determination method (Caruso, 2), the measurement model could be defined as: [ ] [ ] (4) where and are the elements of the acceleration residuals between the frame and the n frame in N, E directions respectively, and is the heading residual between the compass and INS are chosen as the measurement vector. The measurement matrix is: [ ] (5) 2.3 Adaptive Kalman Filter for Dynamic An adaptive Kalman filter with nine states is adopted for the heading and attitude angles estimation, with the linearized system dynamics model given in equation (1) and the measurement model given in equation (4). The process noise covariance matrix is composed as: 3

[ ] (6) where is the matrix of the gyro angular random walk (ARW) process, is the matrix of the gyro bias driven by the rate random walk (RRW) process, and is the matrix of gyro scale factor stability. The measurement noise covariance matrix is related to the measurement noise of accelerometer and magnetic compass. Both and can be determined by Allan variance analysis (Lam, 23; Han and Wang, 211) or checked from the sensor datasheet. The system schematic diagram is shown in Figure 1. θ, γ Magnetometers Heading Computation δf N δψ Kalman Filter Accelerometers Acceleration residual δf E C b n c G b, G s Ψ Gyros Gyro Error Compensation INS Attitude Computation ψ, θ, γ Figure 1. Schematic Diagram of IMU/Magnetometer Integrated AHRS When the AHRS system is in the non-acceleration mode, the measurements of accelerometers could be used to estimate the acceleration residual for the Kalman Filter, and to estimate the tilt angles for the magnetic compass heading computation. In this mode, the Kalman filter can provide an optimal estimate output. When the system is in the dynamic mode, the measurements of accelerometers would include not only the gravity vector but also the dynamic acceleration, which could not directly be used in the magnetic compass heading computation and acceleration residual estimation. In order to yield the optimal performance during the dynamic mode, the tilt output of INS would be used instead in the magnetic compass heading computation, and of the filter would be tuned according to the system dynamic acceleration scalar. Define the system dynamic acceleration scalar (Wang et al. 24) as: (7) where,, are the means of the latest 3 epochs of accelerometer measurements in each axis of the b frame respectively, and the measurement noise covariance matrix is given as: 4

[ ] (8) where is the variance of the magnetic compass heading angle which could be determined by the compass calibration experiment, and, are the variances of the acceleration residuals between the frame and the frame as: [ ] [ ] (9) in which,, are the variances of the 3 axis accelerometer measurements in the b frame. According to the system dynamic acceleration scalar, the movement of the system could be divided into three modes for further discussions. 2.3.1 Non-acceleration mode When, with defined in equation (8), the filter is properly modelled as a stochastic process with an optimal estimate output. 2.3.2 Low-acceleration mode When, such small dynamic acceleration could be treated as the part of measurement noise. In order to yield the optimal outputs, the measurement noise covariance matrix should be adjusted on-line according to : where is a scalar parameter, both and could be determined by the experiments. In this mode, the gain of the Kalman filter could be tuned down automatically based on the matrix. 2.3.3 High-acceleration mode When, the measurements of accelerometers are far away from the value of the gravity, and then, the acceleration residual estimation and the tilt estimation based on the accelerometer measurement are far away from the truth. In order to avoid such measurements affecting the filter, a proper solution is to set the gain of the Kalman filter to during the high dynamic period, and then the system would become a stand-alone INS. In this paper, current INS tilt angles are used to continue the compass heading estimation, which means that the magnetometer measurements could continue to aid the system during the high dynamic. Therefore, could be written as: (1) 5

/ deg / deg [ ] (11) where is a large number which would set the corresponding Kalman filter gain of and to close to. is the standard deviation of the magnetic compass heading angle which is estimated with current INS tilt angles, it is determined by the accuracy of magnetic compass and the gyro bias, and close to in a short period of time. 3. Experimental Test The experimental data were logged by an Xsens MTi sensor, which can output both raw sensors data and Euler angles at the same time. A magnetic calibration procedure was executed according to the user manual of MTi sensor before the experiment, and then the expected accuracy of the magnetic compass is.5 degree, the data refresh frequency was set to 1Hz and other configurations of the MT software remained default. Experiments were implemented through both stationary test and dynamic test. 3.1 Stationary Test In the stationary test, the system was fixed on a static table, and the Euler angles outputs of MTi and adaptive Kalman filter were analysed. The Euler angle errors in stationary test are shown in Figure 2, and the error analysis is shown in Table 1. The results indicate that the accuracy of the adaptive Kalman filter in the stationary test is within.1 degree. / deg.5 MTi AKF -.5 2 4 6 8 1 12 14 16 18 2.5 -.5 2 4 6 8 1 12 14 16 18 2.5 -.5 2 4 6 8 1 12 14 16 18 2 6

Figure 2. Errors of Euler Angles in the Stationary Test Yaw/deg Pitch/deg Roll/deg MTi.78.33.37 AKF.44.16.15 Table 1. RMS Error in the Stationary Test 3.2 Dynamic tests The dynamic tests were conducted without a rotating table. Having considered the different dynamic accelerations, the tests were divided into two parts. 3.2.1 Low acceleration test The low acceleration test was designed to simulate the process of flight take-off and landing. The procedures of this experiment were: take-off process with a pitch changing, next hovering with a heading cycle changing, and then adjusting with roll, and finally landing. As shown in Figure 3, the outputs of magnetic compass, MTi and adaptive Kalman filter are close to each other. In order to evaluate the performance more reasonably, a ground truth is needed. As shown in Figure 4, besides the vibration between 3 and 5 seconds, the experimental procedures were taken with a low acceleration, so the outputs of magnetic compass were accurate besides the period from 3 to 5 seconds. Set the outputs of magnetic compass as the ground truth, and the Euler angle errors are plotted in Figure 5, and the error analysis is shown in Table 2. The results show that the accuracy of the adaptive Kalman filter in the low acceleration test is within.5 degree. 7

Acc Z / m/s/s Acc Y / m/s/s Acc X / m/s/s Roll / deg Pitch / deg Yaw / deg 2 Magnetic Compass MTi AKF -2 1 2 3 4 5 6 7 8 9 1 5-5 1 2 3 4 5 6 7 8 9 1 2-2 1 2 3 4 5 6 7 8 9 1 Figure 3. Time Series Output of Magnetic Compass, MTi and Adaptive Kalman Filter 5-5 1 2 3 4 5 6 7 8 9 1 5-5 1 2 3 4 5 6 7 8 9 1 12 1 8 1 2 3 4 5 6 7 8 9 1 Figure 4. Plots of Accelerometer Measurements in the Low Acceleration Test 8

/ deg / deg 5 MTi AKF -5 1 2 3 4 5 6 7 8 9 1 2-2 1 2 3 4 5 6 7 8 9 1 2 / deg -2 1 2 3 4 5 6 7 8 9 1 Figure 5. Errors of Euler Angles in the Low Acceleration Test Yaw/deg Pitch/deg Roll/deg MTi.492.563.247 AKF.41.113.129 Table 2. RMS Error in the Low Acceleration Test 3.2.2 High acceleration test Since the magnetic compass could not be set as the ground truth in a high acceleration test, this test was designed to evaluate the performance of the system when linear accelerations were added to the MTi sensor. The procedures of experiment were: fixing the sensor on a table, setting the initial Euler angles as the ground truth, and then linearly oscillating the sensor alone the X and Y axis respectively as shown in Figure 6. The outputs of the magnetic compass, MTi and adaptive Kalman filter are plotted in Figure 7, the Euler angle errors of the MTi and the adaptive Kalman filter are plotted in Figure 8, and the error analysis are shown in Table 3. From the results, the accuracy of heading and attitude angles in the high linear acceleration tests is within 1 degree. 9

Roll / deg Pitch / deg Yaw / deg Acc Z / m/s/s Acc Y / m/s/s Acc X / m/s/s 1-1 5 1 15 2 25 1-1 5 1 15 2 25 1 9.5 9 5 1 15 2 25 Figure 6. Plots of Accelerometer Measurements in the High Acceleration Test 15 1 5 Magnetic Compass -5 5 1 15 2 25 MTi AKF 2-2 5 1 15 2 25 2-2 5 1 15 2 25 Figure 7. Time Series Output of Magnetic Compass, MTi and Adaptive Kalman Filter 1

/ deg 2 MTi AKF -2 5 1 15 2 25 5 / deg -5 5 1 15 2 25 5 / deg -5 5 1 15 2 25 Figure 8. Errors of Euler Angles in the High Acceleration Test Yaw/deg Pitch/deg Roll/deg MTi 4.212.726.447 AKF.643.375.338 Table 3. RMS Error in the Linear Acceleration Test 4. CONCLUDING REMARKS This paper has presented a dynamic modelling method for MEMS based IMU and magnetometer Integrated AHRS. A linear system error model based on the Psi-angle Error model has been developed and the corresponding system measurement model has been derived. Moreover, an adaptive Kalman filter is adopted in this method. According to the system dynamic acceleration scalar which is defined to determine the movement mode of system, the matrix can be tuned adaptively to yield optimal performance during the dynamic period. Experiments were conducted with an Xsens MTi sensor which is a low-cost AHRS, and the results have shown that the proposed method improves the orientation performance of the sensor. The stationary accuracy is within.1 degree, and the low acceleration dynamic accuracy is within.5 degree, during the high linear acceleration dynamic, the accuracy can still reach 1 degree. In conclusion, the proposed algorithm can yield the optimal performance with low cost sensors. It can provide an alternative integration approach for low cost AHRS to provide a real-time, reliable and relatively high accuracy heading and attitude solution. The further research is under way to evaluate the system performance with a high accuracy AHRS, and to 11

integrate this system with other navigation systems, such as a vision-based system. ACKNOWLEDGEMENTS The first author would like to appreciate the China Scholarship Council (CSC) for supporting his studies at the University of New South Wales, Australia. REFERENCES Blankinship K. G. (28) A General Theory for Inertial Navigation Error Modeling, Position, Location and Navigation Symposium, Monterey, 1152-1166 Caruso M.J. (2) Applications of Magnetic Sensors for Low Cost Compass Systems, IEEE Position Location and Navigation Symposium (2), San Diego Foxlin E. (1996) Inertial Head-Tracker Sensor Fusion by a Complementary Separate-Bias Kalman Filter, Proceedings of the 1996 Virtual Reality Annual International Symposium (VRAIS 96), Washington, DC, 185-195 Geiger W., Bartholomeyczik J., Breng U., Gutmann W., Hafen M., Handrich E., Huber M., Jäckle A., Kempfer U., Kopmann H., Kunz J., Leinfelder P., Ohmberger R., Probst U., Ruf M., Spahlinger, Rasch A., Straub-Kalthoff J., Stroda M., Stumpf M., Weber C,, Zimmermann M., Zimmermann S. (28) MEMS IMU for AHRS Applications, Position, Location and Navigation Symposium, Monterey, 225-231 Goshrn-Medkin D., Bar-Itzhack I.Y. (1992) Unified approach to inertial navigation system error modeling, Journal of Guidance, Control, and Dynamics, 15(3): 648-653 Han S., Wang J. (211) Quantization and Colored Noises Error Modelling for Inertial Sensors for GPS/INS Integration. IEEE Sensors Journal, 11(6):1493-153 Koifman M., Merhav S.J. (199) Autonomously Aided Strapdown Attitude Reference System, Journal of Guidance and Control, 14(6): 1164-1172 Lam Q.M., Stamatakos N., Woodruff C., Ashtom S. (23) Gyro modeling and estimation of its random noise sources, AIAA Guidance, Navigation, and Control Conference and Exhibit,Austin, Texas, 5562, 1-11 Setoodeh P., Khayatian A., Farjah E. (24) Attitude Estimation by Separate Bias Kalman Filter Based Data Fusion, The Journal of Navigation, 57(2):261-273. Wang M., Yang Y., Hatch R., Zhang Y. (24) Adaptive Filter for a Miniature MEMS Based Attitude and Heading Reference System, Position, Location and Navigation Symposium, 193-2 Weinred A., Bar-Itzhack I.Y. (1978) The Psi-Angle Error Equation in Strapdown Inertial Navigation Systems, IEEE Transactions on Aerospace and Electronic Systems AES-14(3):539-546 Wei Q., Wei Z.Y., Hong L.C., Liang X., Guang M. Y. (29) Real-Time and High-Performance Attitude and Heading Reference System Based on MIMU/Magnetometers, Advanced Materials Research, 6-61: (219-223) 12