1 Complementary Filter Design for Angle Estimation using MEMS Accelerometer and Gyroscope Hyung Gi Min and Eun Tae Jeung Robotics Lab., NTREX Ltd., co., Juan-Dong 5-38, Nam-Gu, Incheon, Korea Department of Control and Instrumentation, Changwon National University, Changwon, 641-773, Korea Abstract: MEMS gyroscopes and accelerometers are designed to measure angular rate of rotation and acceleration forces, respectively. In many applications, a measurement of the angle itself is needed but gyroscopes have a drift error occurred by integrating the angular rate output and accelerometers have errors due to translation forces. In this paper, we present a method of designing a complementary filter to estimate the angle using both an accelerometer and a gyroscope. First, we show that the estimation of the angle using single sensor of a gyroscope or an accelerometer has a lack of accuracy in some experiments. And we propose a design method of the complementary filter using linear least squares to estimate the angle. Experimental results of the proposed method are compared with the cut-off frequency method in a practical rotary inverted pendulum system. Keywords: gyroscope, accelerometer, filter, inverted pendulum, complementary filter, least squares. 1 Introduction MEMS sensors have received considerable attention because of their advantages such as miniature size, low power consumption, low cost, and mass-production. Particularly, they are very useful in applications to unmanned aerial vehicles, automobiles, and mobile robots [1-3]. Gyroscopes are typically designed to measure the angular rate of rotation. In order to estimate the absolute angle, the angular rate signal must be integrated with respect to time. The integrated over time drifts due to presence of bias errors even if small [4-5]. We show this drift in section 2.2.1. Accelerometers are designed to measure acceleration forces and can be used to measure seismic activity, machine vibration, and inclination. The absolute angle can be determined by the measurement of the direction of the gravitational acceleration. However, it is not easy to measure the absolute angle if translation forces are included in the outputs of accelerometers [6-7]. We show this error in section 2.2.2. In order to increase the accuracy of estimating the absolute angle, filter design methods have been introduced. The Kalman filter algorithms [8-10] are used to estimate the absolute angle, but they have some drawbacks to mathematical complexity and long computational time. There is a relatively simple filter called the complementary filter which uses the outputs of two or multi sensors [11-18]. The basic concept of this filter is to enhance advantages of each sensor. For example, the angular estimation using a gyroscope has a good accuracy in the sense of angular direction at high frequencies and the angular estimation using an accelerometer has a good accuracy at low frequencies. A problem of designing the complementary filter is to determine its coefficients such that it has the properties of low pass filter for the accelerometer and high pass filter for the gyroscope. In general, the coefficients of the complementary filter are determined by the cut-off frequency obtained from frequency characteristic of each sensor. However, it may be not easy to obtain a cut-off frequency because of frequency gap between a sensor with good performance at low frequencies and a sensor with good performance at high frequencies. That is shown in section 2.4. In this paper, we present a systematic design method to determine the coefficients of the complementary filter using well-known linear least squares[19-20] which minimizes the sum of the squared errors between estimating angle and 2 true angle (encoder measurement in experiment). A rotary pendulum system is experimented to illustrate the validity of the proposed method and the results are compared with the cut-off frequency method. And the proposed filter is also applied to a rotary inverted pendulum. 2 Some problems in experiment We show that the angular estimation using single sensor of an accelerometer or a gyroscope has a lack of accuracy in some experiments, and we introduce the complementary filter to detect the angle using both an accelerometer and a gyroscope. 2.1 Test equipment We use a rotary pendulum system as test equipment in Fig.1. The gyroscope(IDG300) of InvenSense company and accelerometer(MMA7260QT) of Freescale company are attached between the joint of pendulum and arm, and also the rotational encoder(E30S4-500-3-2F) of Autonics company is installed for comparison. In addition, the DCmotor(320385) of MAXON company with a built-in encoder is mounted on the arm’s center of rotation. DSP-2812 of TI company is used for data acquisition, filter realization, and control algorithm. Fig.1 Rotary pendulum system. 2.2 2.2.1 Angle detection using single inertial sensor Gyroscope Since most of gyroscopes have bias errors, the drift errors occur from interation of the gyroscope’s outputs. Fig.2 shows this phenomenon. The solid line is the angle obtained from the encoder, the dotted line is the integration of the angular rates obtained from gyroscope. 3 Fig.2 The result of a drift error. 2.2.2 Accelerometer By measuring the amount of static acceleration due to the gravity, it can be found out the tilted angle of device with respect to the direction of the gravity. When the z- component ( Z a ) and y- component ( Ya ) of accelerometer is found out, Fig.3 shows that the angle( q c ) using accelerometer can be caculated as qc = tan -1 ç æ Ya ö ÷. è Za ø (1) If there exists a translation motion, the estimated angle using accelerometer is not accurate. Fig.3 is the case of no translation and Fig.4 is the case with a translation motion. In Fig.3 and Fig.4, the solid lines are the angles from encoder, and the dotted lines are the angles obtained from accelerometer. We recognize that translation motion causes the estimation error. 25 20 15 10 5 Encoder Accelerometer degree 0 -5 -10 -15 -20 -25 0 5 10 time(s) 15 20 Fig. 3 The estimated angle from accelerometer in the case without translation motion. 4 Fig.4 The estimated angle from accelerometer in the case with translation motion. 2.3 Complementary Filter Since the angular estimation using single sensor of a gyroscope or an accelerometer has a lack of accuracy, the basic idea of the complementary filter is to combine the outputs of gyroscope and accelerometer. The estimate of the angle is obtained from the sum of their measurements as Fig.5. The integration of the output of a gyroscope feeds into a high pass filter and the output of an accelerometer feeds into a low pass filter. And the sum of the high pass filter and the low pass filter is 1, that is, G1 ( s ) + G2 ( s ) = 1 (2) Fig.5 The complementary filter. Fig.6 The second order complementary filter. 5 where G1 ( s ) and G2 ( s ) are transfer functions of the low pass filter and the high pass filter, respectively. We consider Fig.6 as a complementary filter and its equation is ù 1é K ö æ q f = êq&g - ç K p + i ÷(q f - q c )ú së s ø è û (3) & and q denote the outputs of the gyroscope and the accelerometer, respectively. The filter (3) can be where q g c rewrriten as qf = K p s + Ki æ1 & ö qc ç qg ÷ + 2 s + K p s + Ki è s ø s + K p s + Ki 2 s2 (4) and (4) satisfies the condition (2). The problem of designing a complementary filter is converted to the problem of determining the gains K p and K i . We review the cut-off frequency method in the next section and we introduce least squares method in the next chapter. 2.4 Determination of the filter coefficients using cut-off frequency In order to determine the complementary filter coefficients using cut-off frequency method, we consider the test result of Fig.7. The solid line is the angle of the pendulum and the dotted line is the angle of the arm. Fig.8 is the angle calculated by (1) from the accelerometer output (dotted line) and the estimates from 17 seconds to 22 seconds are different from the anlge of the encoder (solid line). This implies that the estimate using an accelerometer is not accurate at frequencies more than about 3.7[Hz]. So the cut-off frequency of (3) is chosen 1[Hz] and the filter coefficients K p and K i are K p = 1.414, Ki = 1. (5) Fig.7 The angles of the pendulum and the arm in the test. 6 Fig.8 The result of estimating the angle using accelerometer. Fig.9 The estimated angle from complementary filter with the gains (5). Fig.9 is the result of estimating the angle with the filter coefficients (5) and RMSE (root-mean-square error) expressed by 2 1 N éq f (k ) - q e (k ) ù å ë û N k =1 Er = (6) is 0.0565. 3 Complementary filter using least squares In this chapter, we present a systematic method to determine the coefficients of the complementary filter using wellknown linear least squares method[19-20] which minimizes the sum of the squared errors between the estimated angle and the angle obtained from encoder. 7 Since we want that the filter output is very similar to the encoder output, letting the filter output q f be the encoder output and time derivative of (3) give æ ç qe - qc è Let æKp ö & 1 & (qe - qc ) ö ÷ ÷ç ÷ = q g - qe . s K øç è iø (7) & (1) ù éq g é qe (1) ù é qc (1) ù ê ú êq (2) ú êq (2) ú & êq g (2) ú e c ê ú ê ú & Qe = , Qc = , Qg = ê ú, ê M ú ê M ú M ú ê ê ú ê ú & êq ú ëqe (k ) û ëqc (k ) û ë g (k ) û then (7) can be rewritten as (8) é ê Qe - Qc ë éK p ù 1 & -Q & ù. ( Qe - Qc ) ù ê ú = éQ g eû ú s û ë Ki û ë (9) Since the problem of finding the gains K p and K i of (9) is a linear least square problem, the gains can be obtained as follows: -1 T éK p ù T ê ú = A A A B, ë Ki û ( ) (10) where é A = ê Qe - Qc ë 1 & & ù ( Qe - Qc ) ù ú, B = é ë Q g - Qe û . s û (11) Using (10), we can get the following gains of the complementary filter from the experiment in Fig.7: é K p ù é0.1664 ù ê ú=ê ú. ë Ki û ë0.0089 û (12) Fig.10 is the estimate of the angle using the complementary filter with the gains (12) and its RMSE is 0.0389 which is smaller than the result (RMSE: 0.0565) of the cut-off frequency method. In the cut-off frequency method, the RMSE can be reduced according to choosing the cut-off frequency through many tests. But the proposed method is clearly simple and systematic. To verify the performance, apart from the test in Fig.7, the experiment of Fig.11 is carried out and its result is shown in Table 1. In Fig.11, the solid line is the angle from encoder and the dotted line is the angle obtained from acclerometer. In Table 1, (a) is the experiment of Fig.7, and (b) is of Fig.11. Er in Table 1 is the RMSE expressed by (6), and L.S. is the proposed method using least squares. Table 1 shows that the proposed method is better than the others. 8 Fig.10 The estimated angle of the complementary filter using least squares. Fig.11 The other experiment to verify performance of the proposed method. Table 1 The RMSEs comparison with the cut-off frequency method. Cut-off frequency 0.01 Kp 0.1 0.1414 0.0100 0.0449 0.0488 1 1.4140 1.0000 0.0565 0.0732 3 4.2420 9.0000 0.0744 0.0780 L.S. 1.664 0.0089 0.0389 0.0425 0.0141 0.0001 (a) (b) 0.0735 0.1392 Ki Er 4 An application to inverted pendulum control In this chapter, we apply the complenetary filter to the stabilizing problem of a rotary inverted pendulum. The rotary inverted pendulum[21-22] is inherently unstable, so it is important to esimate the angle of the pendulum for stabilization. The linear dynamics of the rotary inverted pendulum[21] is given by 9 && = q && = a where 1é & ( J + m r 2 ) - m l r K K (V - K K a & & ù m p l p gq - f pq a p p p g t b g ) Ra - f aa û Dë ( ) ( ) (13) (14) 1é & + K K (V - K K a & & - m p l p r m p l p gq - f pq g t b g ) Ra - f aa Dë ( ) ( )( J p 2 ù + mpl p )û , D = J a + ma r 2 J p + m p l 2 - (m p l p r ) ( )( ) 2 (15) and q is the angle of the pendulum, and a is the angle of the arm. The parameters are shown in Table 2 and we design a stabilizing controller using LQR[23] method. Table 2 Parameters of the rotary inverted pendulum. Parameter Gravity Mass of pendulum Length of arm Distance between pendulum’s center of mass and center of rotation axis Friction coefficient of arm Friction coefficient of pendulum Friction coefficient of motor Inertia of arm Inertia of pendulum Gear ratio Back EMF constant Torque constant Registance of motor Inductance of motor Value 2 9.8 é ëm s ù û Symbol g mp 0.126 [ kg ] 0.42 [ m ] l p = 0.24 [ m ] r lp fa fp 0.0362 0.003 0.001 2 0.045 é ë kgm ù û 2 0.003 é ë kgm ù û fm Ja Jp Kg 12 Kb Kt Ra La K b = 0.0031 0.0169 0.316 [ W ] La = 0.00008 [ H ] The angle of the arm is measured by built-in encoder in DC-motor. And the angle is detected by the proposed complementary filter using a gyroscope and an accelerometer. The angular velocity of the arm is obtained from differentiation its angle, and the angular velocity of the pendulum is obtained from the gyroscope. Fig.12 is a snap-shot of the actual stabilization of the rotary inverted pendulum. And Fig.13 is a part of results to estimate the angle using the proposed filter, in which the solid line is the angle of the pendulum obtained by the proposed complementary filter and the dotted line is the angle from the encoder for comparison. It can be verified that the proposed method has good performance enough to stabilize the inverted pendulum. 10 Fig.12 A snap-shot of the actual stabilization of the rotary inverted pendulum. Fig.13 The estimated angle and the encoder angle of the inverted pendulum. 5 Conclusions We have proposed a design method of the complementary filter with a gyroscope and an accelerometer using least squares. The complementary filter has both a low pass filter for the output of an accelerometer and a high pass filter for the output of a gyroscope. The least square method is used to estimate the angle and the proposed mothod is systematic, simple, and accurate in comparison with the cut-off frequency method. And we show that the proposed filter has good performance through the application to the stabilization of the rotary inverted pendulum. References [1] [2] [3] Sung Kyung Hong, “Fuzzy logic based closed-loop strapdown attitude system for unmanned aerial vehicle (UAV),” Sensors and Actuators, A. 107, pp. 109-118, 2003. Yorihisa Yamamoto, “NXTway-GS Model –Based Design – Control of self-balancing two-wheeled robot built with LEGO Mindstorms NXT,” http://www.mathworks.com/matlabcentral/fileexchange/19147. Pedro Castillo, Alejandro Dzul, and Rogelio Lozano, “Real-Time Stabilization and Tracking of a Four-Rotor Mini Rotorcraft,” IEEE Transactions on Control Systems Technology, vol. 12, no. 4, pp. 510-516, July 2004. 11 [4] [5] [6] [7] [8] [9] [10] [11] [12] [13] [14] [15] [16] [17] [18] [19] [20] [21] [22] [23] A. D. King, “Inertial Navigation – Forty Years of Evolution,” GEC REVIEW, vol. 13, no. 3, pp. 140-149, 1998. D. Piyabongkarn, R. Rajamani, and M. Greminger, “The development of a MEMS gyroscope for absolute angle measurement,” IEEE Transactions on Control Systems Technology, vol. 13, no. 2, pp. 185-195, March 2005. V. Krishnan, “Measurement of Angular Velocity and Linear Acceleration Using Linear Accelerometers,” Journal of the Franklin Institute, vol. 280, no. 4, pp. 307-315, 1965. A beginner’s guide to accelerometers. http://www.dimenstionengineering.com/accelerometers.htm M. S. Grewal, V. D. Henderson, and R. S. Miyasako, “Application of Kalman filtering to the calibration and alignment of inertial navigation systems,” Institute of Electrical and Electronics Engineer, New York, pp. 65-72, 1986. David E. Gaylor, and E. Glenn Lightsey, “GPS/INS Kalman Filter Design for Spacecraft Operating in the Proximity of the International Space Station,” American Institute of Aeronautics and Astronautics Guidance, Navigation and Control Conference and Exhibit, Austin, TX, August 2003. H. J. Luinge, and P. H. Veltink, “Measuring orientation of human body segments using miniature gyroscope and accelerometer,” Medical and Biological Engineering and Computing, vol. 43, no. 2, pp. 273-282, April 2005. Albert-Jan Baerveldt, and Robert Klang, “A Low-cost and Low-weight Attitude Estimation System for an Autonomous Helicopter,” IEEE international conference on intelligent engineering systems, proceedings, Budapest Hungary pp. 391-395, 1997. Lauro Ojeda, and Johann Borenstein, “Experomental Result with the KVH C-100 Fluxgate Compass in Mobile Robots,” Proc.of the IASTED International Conference Robotics and Applications, Honolulu, Hawaii, August, 2000. A. Pascoal, I. Kaminer, and P. Oliveira, “Navigation System Design Using Time-Varying Complementary Filters,” IEEE Transactions on Aerospace and Electronic Systems, vol. 36, no. 4, pp. 1099-1114, October, 2000. Anthony Gallaher, Yoky Matsuoka, and Wei-Tech Ang, “An Efficient Real-Time Human Posture Tracking Algorithm Using Low-Cost Inertial and Magnetic Sensors,” Proc.of IEEE/RSJ International Conference on Intelligent Robots and Systems, Sendai, Japan, September, 2004. Robert Mahony, Tarek Hamel, and Jean-Michel Pflimlin, “Complementary filter design on the special orthogonal group,” Proc.of 4th IEEE Conference on Decision and Control, and the European Control Conference, Seville, Spain, December, 2005. Woon-Tahk Sung, Sangkyung Sung, June-Young Lee, and Taesam Kang, “Development of a lateral velocitycontrolled MEMS vibratory gyroscope and its performance test,” Journal of the Micromechanics and Microengineering, vol. 18, 055028. Cao Dong, Qu Qiang, and Li Chuntao, “Research of Attitude Estimation of UAV Based on Informantion Fusion of Complementary Filter,” Fourth International Conference on Computer sciences and Convergence Information Technology, Seoul, 2009. Armando Alves Neto, Douglas Guimaraes Macharet, Victor Costa da Silva Campos, and Mario Fernando Campos, “Adaptive complementary filtering algorithm for mobile robot localization,” ” Journal of the Brazilian Computer Society, vol. 15, no. 3, Campinas, September, 2009. Dennis G. Zill, Warren S. Wright, Advanced Engineering Mathematics Fourth Edition, Jones and Bartlett Publishes, 2009. Wikipedia’s Article of Least Squares, http://en.wikipedia.org/wiki/Least_Squares. Myung-Gong Sohn, Cheon Don Son, Hyung Gi Min, Sung-Ha Kwon, and Eun Tae Jeung, “Design of a Stabilizing Controller using T-S Fuzzy model for a rotary inverted pendulum,” AISM2006, Hong Kong, 2006. Wikipedia’s Article of Inverted Pendulum, http://en.wikipedia.org/wiki/Inverted_pendulum. Richard C. Dorf, and Robert H. Bishop, Modern Control System 10th Edition, Person Education, 2005.