An Adaptive Filter for a Small Attitude and eading Reference System Using Low Cost Sensors Tongyue Gao *, Chuntao Shen, Zhenbang Gong, Jinjun Rao, and Jun Luo Department of Precision Mechanical Engineering of Shanghai University P.. Bo 8, Yanchang Rd 49, Shanghai 7, P.R. China gty@shu.edu.cn Abstract. Small Attitude And eading Reference System (ARS) based on MEMS are small size, light weight, low power consumption, and low cost by the inclusion of micro sensors. Small ARS have many potential uses beyond air- and spacecraft applications. owever, the MEMS sensors have large noise, bias and scale factor errors due to drift..an etended Kalman filter with adaptive gain was used to build a small ARS system based on a stochastic model. The adaptive filter tunes its gain automatically based on the system dynamitic sensed by the movement state to yield optimal performance. Keywords: adaptive Kalman filter, Attitude And eading Reference System, low cost sensors. Introduction Small Attitude And eading Reference System (ARS) based on Micro-Electro- Mechanical Systems (MEMS) are small size, light weight, low power consumption, and low cost by the inclusion of micro sensors. Small ARS have many potential uses beyond air- and spacecraft applications. But the MEMS sensors have large noise, bias and scale factor errors due to drift. The traditional algorithm using a low-cost MEMS sensor is difficultly satisfying the ARS performance reuirements. Recently, a lot of efforts have been directed at developing low-cost systems for ARS. Although, they effort to achieve the best performance of ARS. They all do not think about the state of carrier movement. In this paper, An etended Kalman filter with adaptive gain was used to build a small ARS system. The adaptive filter tunes its gain automatically based on the system dynamitic sensed by the movement state to yield optimal performance. System Design Small ARS system block diagram is figure. ARS consists of three parts. One part is sensors which are made up of triple ais gyroscopeâtriple ais accelerometerâtriple ais magnetometer and temperature sensor. The seconded part * Corresponding author. Y. Wu (Ed.): Advances in Computer, Communication, Control & Automation, LNEE, pp. 9. springerlink.com Springer-Verlag Berlin eidelberg
T. Gao et al. is MCU which realizes data AcuisitionÂSensor Calibration ÂKalman Algorithm and Communication. The third part is user interface. Fig.. The ARS system block diagram The Model of Sensors. The Mode of Gyroscope Gyroscope measures the rotational velocity. The angle of rotation could be obtained through integration of this sensor signal. owever, as integration is reuired, even the smallest constant bias can make the error grow to infinity. It is known as drift that is biggest error of rate gyro. In the following analysis we denote by ω : the biased compensated gyro measurements: ω ω ω () where, ω :the output of gyroscope,ω :the estimated bias drift by Kalman filter. Further, ω ω ω T ω ω ω T ω ω ω T (). The Mode of Accelerometer Accelerometer measures the sum of all inertial forces and gravity. But the high translational accelerations will happen only in transient modes and by small time intervals. So the accelerometer can be used as inclinometers:
An Adaptive Filter for a Small Attitude and eading Reference System A arcsina /g () A arcsina,a (4) where, A : the pitch angle by the data of accelerometer A : the roll angle by the data of accelerometer A, A,A : the output of accelerometer g:the gravity. The Mode of Magnetometer Magnetometer measures the sum of the earth magnetic and other stuff magnetic. When the values of other stuff magnetic surrounding the ARS is very large, the corresponding arithmetic will compensate the output of magnetometer. M arctany,x (5) where, Y : the level component of the earth magnetic X : the level component of the earth magnetic M : the yaw angle by the data of magnetometer When the ARS is inclining, M must be compensated by the pitch and roll angle: X Xcos Y sin cos Zcos cos (6) where, X,Y,Z: the output of the magnetometer. Y Y cos Zsin (7) 4 The Adaptive Kalman Filter 4. The Adaptive Kalman Filter Thought In this paper, by Kalman filter, the attitudeâheading and gyro bias is estimated. Furth more, the attitude is calculated by integrating angular rates measurements given by gyroscope. Accelerometers and magnetometer will work as observation data to correct the predicted attitudeâheading and gyro bias. A diagram of the Kalman filter is shown in Figure. Fig.. The diagram of Kalman Filter In order to obtain better accuracy, the adaptive gain was gotten by adjust observer noise matri R based on the movement state such as non-acceleration mode, acceleration mode and high dynamic mode. In one word, the adaptive filter tunes its gain automatically based on the system dynamitic sensed by the movement state to yield optimal filter performance.
4 T. Gao et al. 4. The Adaptive Kalman Filter Design In order to achieve the etended kalman filter on computer, the following parameter must be gotten: A. State transition Matri / The uaternion differential euations computation is relatively small, and there are no singularities. So we set up the Kalman state euation by uaternion. uaternion differential epression: 4 ω ω ω ω ω ω ω (8) ω ω ω ω ω ω 4 where, 4 T is uaternion. Based on the formulation () and (8), the state euation can be gotten: ω m ω my ω mz ω X AXWt m ω mz ω my 4 ω my ω mz ω m 4 ω mz ω my ω 4 m Wt (9) ω e ω ey ω ez Assume the state vector () The seven states are four uaternion elements and three bias errors for the gyroscopes. So the system state euation is: ω m ω my ω mz χ χ ω X AXWt m ω mz ω my χ χ χ χ ω my ω mz ω m χ χ χ ω mz ω my ω m χ χ χ Wt () χ χ χ The above euation is Kalman filter state euation. Above nonlinear state euation can be linearized and discrete along the currently estimated trajectory χ. So the state transition matri: ω m ω my ω mz χ χ ω m ω mz ω my χ χ 4 / ω my ω mz ω m χ χ ω mz ω my ω t () m χ 4 χ
An Adaptive Filter for a Small Attitude and eading Reference System 5 B. Measurement matri Such as above description, the observer vector is: Y A A M () Then the observer euation is Y XVt (4) Above nonlinear state euation can be linearized and discrete along the currently estimated trajectory χ. So the observer euation is A A A A A A A χ χ χ χ χ χ χ χ χ A A A A A A χ A Y Vt χ χ χ χ χ χ χ χ Vt χ M M M M M M M χ χ χ χ χ χ χ χ χ (5) According the relationship between the Euler angle and uaternion, the measurement matri can be gotten. Based on the information, the Direction Cosine Matri (DCM) is: cc sc s DCM sc css cc sss cs (6) sc csc cs ssc cc where, c and s are the abbreviation of cos and sin. uaternion also can be converted to DCM: 4 4 4 DCM 4 4 4 (7) 4 4 Based on the formulation (6) and(7 ),then 4 atan asin 4 4 atan 4 According to above relationship, the partial derivatives between Euler angle and uaternion in the measurement matri can be gotten. If we define the three variables: (9) 4 (8) () 4
6 T. Gao et al. 4 () Then measurement matri () C. Process Noise Matri Process noise matri () Each element on the diagonal corresponding to each state vector component, said the state vector components of the noise situation. Each element of process noise matri can be determined eperimentally using measurement data from the ARS sensors. D. Observer Noise Matri Adaptive Regulation Traditional Kalman filtering algorithm uses fied noise covariance matri, and can t obtain the optimal attitude angle in various conditions. This paper provides a kind of adaptive noise matri euation, namely adaptive unscented Kalman filter algorithm. In static state, observation noise covariance matri R k can be obtained through the analysis of observational data, and then determine the Kalman filter gain for optimal attitude estimation. In dynamic state, the measurements of the accelerometer not only include the acceleration of gravity but also the linear acceleration. Therefore, this paper proposed regulating the value of R according to the motion condition, and the gain of Kalman filter is adjusted to get the optimal attitude estimation. The state parameter is defined as follows: η A A y A z g (4) = = k 4 5 6 7 k =
An Adaptive Filter for a Small Attitude and eading Reference System 7 When ησ σ σ, the system is considered in static state. Where σ, σ and σ are the noise variance of the accelerometer in the stationary state. In stationary state the noise variance matri R is as follows: σ R σ (4) σ When σ σ σ ηt, the system is considered in low-dynamic state. Where Th is the threshold parameter of the state. In low-dynamic state the noise variance matri R is as follows: αη σ R αη σ (5) αη σ Where α is an adjustable scale factor. When ηt that the system is in high dynamic condition. In high dynamic state the noise variance matri R is as follows: T R T (6) T Where T ÂT and T are the threshold of the maimum observation noise. 5 Eperiment In order to verify the effect of adaptive Kalman filter algorithm proposed in this paper, the system is tested in three different conditions with turntable systems. The accelerometer attitude angle (AAA), estimated attitude angle (EAA) and real attitude angle (RAA) are compared respectively. The error is represented by the mean error (ME) and mean suare error (MSE). The test details are as follows: (a) static state (b) accelerated motion (c) high freuency motion Fig.. Filtering effect in stationary state
8 T. Gao et al. Figure (a) shows the effect of the filter that the sensor module is in static state. The optimal attitude angle has no drift, and the accuracy is greatly improved compared to only use the accelerometer. Table. Stationary state error error ME MSE Acceleration-based angle error.88.44 Filter-based angle error.98.8 Figure (b) shows the effect of the filter that sensor module doing accelerated motion in the horizontal plane. The accelerometer attitude angle is impact by the linear acceleration, while using the adaptive Kalman filter the error of the pitch angle is significantly improved. Table. Linear acceleration state error error ME MSE Acceleration-based angle error 4.67.678 Filter-based angle error. 54.85 Figure (c) shows the effect of the adaptive Kalman filter that the sensor module doing high freuency movement. The accuracy of the EAA angle is greatly improved compared to only use the accelerometer. Table 4. igh freuency motion state error error ME MSE Acceleration-based angle error 5.7.47 Filter-based angle error.57. 6 Conclusion ARS based on MEMS have many potential uses beyond air- and spacecraft applications. owever, the MEMS sensors have large noise, bias and scale factor errors due to drift. The adaptive Kalman filter is put forward. The adaptive filter tunes its gain automatically based on the system dynamitic sensed by the movement state to yield optimal performance. Finally the eperiment tests the good performance of the ARS based on the adaptive Kalman filter in three situations. In other word, he result proves the effectiveness of the adaptive Kalman filter using the low cost sensors. Acknowledgment. This project is supported by National Natural Science Foundation of China (No.595 and 558).
An Adaptive Filter for a Small Attitude and eading Reference System 9 References. Euston, M., Coote, P., Mahony, R., Kim, J., amel, T.: A Complementary Filter for Attitude Estimation of a Fied-Wing UAV with a Low-Cost IMU. In: 6th International Conference on Field and Service Robotics, FSR 7 (July 7). Kim., S., Park., M., Anumas., S., Yoo, J.: ead Mouse System Based on Gyro- and Opto- Sensors. In: IEEE International Conference on Biomedical Engineering and Informatics (). Bo, A., Borges, G.: Low Cost D Localization System for Applications on Aerial Robots. In: ABCM Symposium Series in Mechatronics, vol., pp. 55 6 (8) 4. Gao, T., Gong, Z., Luo, J., Ding, W., Feng, W.: An Attitude Determination System For A Small Unmanned elicopter Using Low-Cost Sensors. In: IEEE International Conference on Robotics and Biomimetics, Kunming, China, December 7- (6) 5. Mahony, R., amel, T., Pflimlin, J.: Complementary filter design on the special orthogonal group SO(). In: IEEE Conference on Decisoin and Control, pp. 477 484 (December 5) 6. Simon, D.: Optimal state estimation, pp. 95 457. John Wiley & Sons, Inc., oboken (6) 7. Woodman:, An introduction to inertial navigation University of Cambridge Technical Report, UCAM-CL-TR-696, ISSN 476-986 (August 7) 8. Gareth Evans, D., Drew, R., Blenkhorn, P.: Controlling Mouse Pointer Position Using an Infrared ead-operated Joystick. IEEE Transactions on Rehabilitation Engineering 8() (March )