Abstract—The complexity and computation burden of

conventional position and Orientation estimating filters lead to the development of simple and equally efficient algorithms. The extensive research in this area resulted in relatively simple and robust techniques. Explicit complementary filter and Gradient descent based algorithm are such examples of the computationally simple and still efficient algorithms which are applicable to low cost tri- gyroscopes and tri-accelerometers based inertial measurement units (IMUs) using quaternion representation. These orientation estimators fuse gyro output for high frequency estimation along with accelerometer output for low frequency attitude estimation. This paper evaluates the performance of aforementioned two algorithms using both simulated and experimental data acquired from IMU. Simulated data was generated for known orientation to validate the filters performance whereas for experimental data; MPU6050 IMU was used for different scenarios. The assessment is based on the root mean square error computation for both algorithms. Moreover, both algorithms are equipped with adjustable parameters (filter gains) which were exploited for a range of values in the quest for perfection.

Keywords— Complementary Filter, Estimation, IMU, ROV.

I. INTRODUCTION

HE problem of object attitude estimation has long been the subject of extensive research [1]-[15]. The object may

include aerial (UAV), land moving (robot) or underwater (UUV/ROV) [2]-[4]. For this purpose, two types of sensors are employed; namely internal or dead reckoning (like IMU) and external or sometimes called aided (like GPS). In theory, a single integration of the accelerometer data will provide velocity whereas double integration results in position information. In practice, however, working with standard IMU units, the trajectory prediction are applicable for very short periods of time (in seconds) due to the non-linearity, biases and noise present in the INS sensors [5]. The accumulation of errors with time for dead reckoning sensors and low frequency output for external sensors leads to the aided-INS system where the two sensor groups are fused to account for the problems stated [6]. This accumulation results in a very large

Fakhri Alam is with the College of Automation Engineering, Nanjing University of Aeronautics & Astronautics, 29 Yudao Street, Nanjing, Jiangsu, China (corresponding author’s phone: 008615720611925; e-mail: engr_falam@hotmail.com ).

Zhou ZhaiHe is with the College of Automation Engineering, Nanjing University of Aeronautics & Astronautics, 29 Yudao Street, Nanjing, Jiangsu, China (e-mail: zzhcom@nuaa.edu.cn ).

Hu JiaJia is with the College of Automation Engineering, Nanjing University of Aeronautics & Astronautics, 29 Yudao Street, Nanjing, Jiangsu, China (e-mail: 15088666336@163.com ).

position and attitude error without aid of complementary navigation sensors. The aided sensors include depth, heading, air speed, GPS, APS (SBL, LBL, USBL), Doppler velocity log (DVL), to compensate the position and attitude errors of INS. A standard way is to fuse the inertial sensors and one/some aided sensors using Kalman filter. Many interesting fusion schemes have been devised; GPS/INS, APS/INS, Vision based INS, LVS/IMU and DVL/IMU are some proposed algorithms for UAV, robots and underwater UUV/ROV using Kalman filter [6]-[13].

With the advent of MEMS based IMU, the size of the sensory set dramatically reduces to chip size along with reduction in cost and power consumption. Being in the developing phase still facing the associated noise issues, this technology has the potential to replace the expensive sensory sets. A tri-gyroscopes and tri-accelerometers (mutually orthogonal) based IMU seems to be a complete solution for orientation estimation problem [16]. In most of the cases, it is easier to combine and fuse all the installed sensors data using Kalman filter [10]. However, as noted [15], the efficient Kalman filter demands for the computational complexity which may not be feasible in some application. The more advance filtering technique include particle filtering which are even more demanding in computational complexity and hence computationally much expensive [15], [17]. The fuzzy logic based algorithm is another simple approach to deal with the attitude estimation problem with relatively less computational burden [22]. However, the applications are limited. Recently, the search for simple and still effective filtering algorithm lead to the development of constant gain based complimentary algorithms [15]-[21]. These schemes utilize inertial measurement unit (IMU) data where the accelerometer output estimates the gravitational direction. The explicit complementary filter (ECF) suggested by Mahony et al [15] and gradient descent based orientation filter (GDOF) authored by Madgwick et al [16] have been shown to offer efficient performance with little computational cost. Both techniques employ quaternion representation for orientation estimation applicable to IMU. This paper present the results achieved through the application of explicit and gradient descent based complimentary filters on IMU data and compares the two algorithms for different scenarios.

The body of paper consists of five sections followed by a conclusion. Section II provides an overview in the context of orientation problem. Section III details the two algorithms; the explicit and gradient descent based complementary filter.

A Comparative Analysis of Orientation Estimation Filters using MEMS based IMU

Fakhri Alam, Zhou ZhaiHe, and Hu JiaJia

T

2nd International Conference on Research in Science, Engineering and Technology (ICRSET’2014), March 21-22, 2014 Dubai (UAE)

http://dx.doi.org/10.15242/IIE.E0314552 86

Section IV compares the two techniques based on simulation data. Section V includes the results obtained by applying the algorithms on the IMU data, the discussion and comparison part followed by the conclusion.

II. PRELIMINARIES

In the context of position and attitude estimation problem, usually the Euler angel (roll, pitch, and yaw) representations are used. However, a phenomenon called gimbal lock occurs as soon as the pitch reaches 90°. A quaternion based solution is preferred to avoid this problem. The two representations are easily interchangeable. A frame of reference is another important issue in this regard. Usually the term body frame or sensor reference is referred to the measurement taken by the sensor directly whereas earth reference or inertial frame is usually used for North-East-Down (NED). For mathematical operation, the quantities should be in the same frame of reference. Inertial Measurement Unit (IMU) is a part of Inertial Navigation System (INS) comprised by triplets of accelerometers and gyros.

A. Gyros Model: A gyro measures the angular velocity; orthogonally

installed three gyros measure angular velocity in x, y, z directions. However, like all the measurements, the gyro measurement contains noises and biases. Hence a gyro can be model as

b b nω ω= + + (A)

Where, b is the gyro bias and n is the associated noise.

B. Accelerometer Model: Ideally, accelerometer measure acceleration only. However,

practically, it is not that simple. An ideal accelerometer, measures the instantaneous linear acceleration as well as the gravitational acceleration field plus some added bias and noise which can be represented as,

a aa v g b n= − + + (B) Here, v , g, ab and an represent instantaneous linear

acceleration, gravitational acceleration , accelerometer bias and noise respectively.

III. COMPLEMENTARY FILTERS The two algorithms are outline as following.

A. Explicit Complementary filter A complementary filter fuses accelerometer and gyroscope

data for orientation estimation such that low-pass filtering is applied on accelerometer data and high-pass filtering on gyro output [15]. Nonlinear extensions of the complementary filters can be used in the case where a low pass estimate of the orientation can be reconstructed from the IMU measurements and for attitude, the full coupled rotation matrix may be work out as an algebraic function of the gravitational and magnetic fields measured in the body co-ordinates. The first step in ECF

is to measure the inertial direction, v and the measured angular velocities bω where

b bp IK e K eω ω= + + ∫ (1)

Where ˆe v v= ×

1ˆ ˆ ( )2

bq q p ω= ⊗

(2) Where

( ) (0, ) [0 ]b b b b bx y zp ω ω ω ω ω= =

2 4 1 3

3 4 1 22 2 2 21 2 3 4

ˆ ˆ ˆ ˆ2( )ˆ ˆ ˆ ˆ ˆ2( )

ˆ ˆ ˆ ˆ

q q q qv q q q q

q q q q

+

= + − − +

Here, bω is the angular velocity given by gyro (in body

coordinates), 𝐾𝑝, 𝐾𝑖 are proportional and integral gains respectively-the adjustable and tuning parameters, v is the measured inertial direction based on accelerometer data, v̂ is the estimated inertial direction, e is the error between the two whereas q̂ is the estimated orientation expressed in quaternion and ⊗ is a quaternion product operator. Further detail can be found in [20], [21].

Following steps were followed for implementing explicit complementary algorithm.

Initialization: initialize (assume initial as [1 0 0 0]) quaternion, sampling rate, proportional and integral gain. Normalization: Normalize accelerometer data Gravity direction: Estimated direction of gravity using

2 4 1 3

3 4 1 22 2 2 21 2 3 4

ˆ ˆ ˆ ˆ2( )ˆ ˆ ˆ ˆ ˆ2( )

ˆ ˆ ˆ ˆ

q q q qv q q q q

q q q q

+

= + − − +

Error calculation: Compute error by cross multiplying normalized accelerometer data and estimated direction of gravity (in step 3) as

ˆe v v= × Data fusion: Debias gyro data by applying feedback terms as

b b

p IK e K eω ω= + + ∫ For suitable values of adjustable𝐾𝑝, 𝐾𝑖 and then compute rate of change of quaternion as

1ˆ ˆ ( )2

bq q p ω= ⊗

Where q̂ =Estimated normalized quaternion at time, t-1 and

2nd International Conference on Research in Science, Engineering and Technology (ICRSET’2014), March 21-22, 2014 Dubai (UAE)

http://dx.doi.org/10.15242/IIE.E0314552 87

( )bp ϖ = (0, )bϖ Estimate quaternion: Integrate to yield estimated quaternion and normalize. Repeat: Repeat from step 2 for next iteration

B. Gradient Descent based Orientation Filter The angular velocity rate measured by tri-gyro of IMU can

be represented by a vector representation as follow.

0b b b bx y zω ω ω ω = (3)

The rate of change of angular velocity in quaternion

representation can be then given by

(4)

If it was not the case of error accumulation of gyro with

time, a simple integration of gyro data would suffice the solution of orientation problem. To debias gyro data, further processing is needed using accelerometer data. The whole algorithm is summarized below; further detail can be found in [16].

(5)

(6)

(7)

(8)

(9)

The term β is the adjustable parameter, such that β<1. However, its initial value may be greater than 1 to accelerate convergence.

The following steps were followed for implementing explicit complementary algorithm.

Initialization: Assume initial quaternion as [1 0 0 0], define sampling time and β value. Normalization: Normalize accelerometer data G.D. Algorithm: Apply gradient descent algorithm to compute

ff

∇∇

Data fusion: Compute rate of change of quaternion by

( , ) ( , )b best t t

fq qfω β ∇

= −∇

;

Where

( , ) ( , 1)1 ˆ2

b b bt tq qω ω ω−= ⊗ (Depends on gyro data)

And ff

∇∇

Depends on accelerometer data

Integration: Integrate to estimate quaternion by

( , ) ( , 1) , )ˆest t

b b best t est tq q q t−= + ∆

Where ( , 1)ˆbest tq −

is the normalize quaternion estimated at time

1t − , ( , )est t

bq from step (4) and t∆ is sampling time.

Normalization: Normalize the estimated quaternion Repeat: Repeat from step2 for next iteration

IV. SIMULATION RESULTS

Both algorithms were evaluated against MATLAB simulated data for different values of tunable parameters; 𝐾𝑝, 𝐾𝑖 for ECT and β for GDOF. Following equations were used for known orientation at different times to simulate different scenarios [23].

tan yxyz

z

AA

φ

=

(10)

2 2tan x

xyz

y z

AA A

θ −=

+ (11)

Here, φ , θ represent roll and pitch angle respectively in radians, A represents accelerometer data.

Fig. 1 shows the mean of roll and pitch RMSE (Root Mean Square Error) as a function of 𝐾𝑝, for constant 𝐾𝑖 =0.02; using ECF. For 𝐾𝑝=0.28, 𝐾𝑖 = 0.02, mean of RMSE for roll and pitch is a minimum of 0.54 degree. Fig. 2 shows the mean of roll RMSE and pitch RMSE as a function of β using GDOF. A value of β= 0.044 seems appropriate value and result in minimum mean root square error of 0.54 degree. In comparison of Fig.1 and Fig.2, it is evident that ECF is less sensitive to adjustable parameter as compared to GDOF while keeping the other adjustable term constant.

1 ˆ2

b b bq q ω= ⊗

3 4 1 2

2 1 4 3

2 3

2 2 2 2ˆ( ) 2 2 2 2

0 4 4 0

bg

q q q qJ q q q q q

q q

− − = − −

2nd International Conference on Research in Science, Engineering and Technology (ICRSET’2014), March 21-22, 2014 Dubai (UAE)

http://dx.doi.org/10.15242/IIE.E0314552 88

Fig. 1 Performance of ECF as function of the adjustable parameter

Kpfor constant Ki = 0.02

Fig. 2 Performance of GDOF as function of filter gain, β.

Fig. 3 Roll angle estimated for 𝐾𝑝 = 0.28, 𝐾𝑖 = 0.02, β =0.038.

Fig. 4 Comparison of estimated pitch by ECF and GDOF

Also, for some particular values of adjustable filter gains, both filters perform identically in the perspective of Root Mean Square Error. Fig. 3 shows the typical result of roll estimation where Fig. 4 for pitch estimation Kp = 0.28, Ki = 0.02 (for ECF) and β=0.038 (for GDOF). All these results clearly show that the estimated error range for both algorithms seem identical. Fig. 5 shows the typical result of roll estimation for adjustable filter gain of Kp = 0.9, Ki = 0.02 (for ECF) and β=0.9 (for GDOF) where Fig. 6 is the associated error in roll estimation. Pitch estimation and the associated errors are depicted in Fig. 7 and Fig. 8 respectively, for the same higher values of filters adjustable gains (Kp = 0.9, Ki = 0.02 for ECF and β=0.9 for GDOF). It is evident that with higher values of GDOF gain, the resultant estimations by GDOF are much noisier with large errors in comparison with ECF.

Fig. 5 Roll angle estimated by ECF & GDOF (for 𝐾𝑝 = 0.9, 𝐾𝑖 = 0.02, β =0.9)

2nd International Conference on Research in Science, Engineering and Technology (ICRSET’2014), March 21-22, 2014 Dubai (UAE)

http://dx.doi.org/10.15242/IIE.E0314552 89

Fig. 6 Estimated error-Roll ( 𝐾𝑝 = 0.9, 𝐾𝑖 = 0.02, β =0.9)

Fig. 7 Pitch angle estimation (for 𝐾𝑝 = 0.9, 𝐾𝑖 = 0.02, β =0.9)

Fig. 8 Associated error in pitch estimation

V. 4BEXPERIMENTAL RESULT

MPU-6050 IMU (Inertial Measurement Unit) was used to generate data for different scenarios (at 100 Hz) and the aforementioned two algorithms were applied for roll and pitch estimation. Different scenarios were simulated and the orientation in quaternion were computed which were converted to Euler angles representation for comparison purpose. As only IMU was used with no aided or external sensor, so only the roll and pitch were considered for comparison.

First the IMU was tilted for a roll angle in the proximity of +60 degrees, kept for 25 seconds and then back to original position for next 20 seconds. Same was repeated in the opposite direction. The data thus generated by IMU was processed through both algorithms and again identical results were achieved as shown in Fig. 9. For ECF, filter gain values Kp=0.028 (proportional gain) and Ki =0.02 (integral gain) were used whereas β =0.038 was chosen for GDOF. A similar approach was repeated for pitch and the result is shown in Fig. 10. The two lines are hardly distinguishable, again verifying that both algorithms closely follow each other by tuning the adjustable parameters.

Fig. 9 Roll angle estimation

Fig. 10 Pitch angle estimation

2nd International Conference on Research in Science, Engineering and Technology (ICRSET’2014), March 21-22, 2014 Dubai (UAE)

http://dx.doi.org/10.15242/IIE.E0314552 90

VI. CONCLUSION A comparative analysis was presented in this paper for the

orientation problem using different approaches. Kalman filter and particle filter are considered the benchmark for position and attitude estimation, however, the associated computation burden in some circumstances are undesirable. Complementary filters are ideal in such situation. Both ECF and GDOF are effective and novel approaches in this regard. With the power of adjustable gain, these techniques find places in most of the real world applications. The evaluation of these filter results in identical outcome, however, ECF has a bit edge over GDOF partly because of higher accuracy and partly because of the two adjustable gains resulting in extra choices. Moreover, ECF is less sensitive to variation in filter gain in comparison with GDOF. Both techniques can be efficiently used in aided-INS system where less computation burden is of prime importance.

REFERENCES [1] J. L. Crassidis, F. L. Markley, and Y. Cheng, “Nonlinear attitude

filtering methods,” J. Guidance, Control, Dynam., vol. 30, no. 1, pp. 12–28, Jan. 2007. http://dx.doi.org/10.2514/1.22452

[2] Chang-sun, Y. and Ahn, I-K, Low Cost GPS/INS Sensor Fusion System for UAV navigation, Proc. IEEE Intelligent Transportation Systems, 2003, pp. 8.A.1-(1-9).

[3] E.H. Shin, “Accuracy improvement of low cost INS/GPS for land applications”, PhD Thesis, Dept of Geomatics Engineering University of Calgary, 2001

[4] X. Yun, Bachmann, E.R and McGhee, Testing and Evaluation of an Integrated GPS/INS System for Small AUV navigation, IEEE Journal of Oceanic Engineering , Vol24, No.3,1999,pp.396-40M. http://dx.doi.org/10.1109/48.775301

[5] Titterton, D.H., Weston, J.L., Strap down Inertial Navigation Technology. Peter Pegerinus, London, 1997.

[6] Francois Caron, Emmanuel Duflos, Denis Pomorski, Philippe Vanheeghe, “GPS/IMU data fusion using multisensor Kalman filtering: introduction of contextual aspects”, Information Fusion 7, p.221 230, 2006.

[7] S. Sukkarieh, ”Low cost, high integrity, aided inertial navigation systems for autonomous land vehicles”, PhD Thesis, Australian Center for Fields Robotics, University of Sydney, 2000.

[8] Chong-moo Lee, Seok-Won Hong, Woo-Jae Seong, ”An Integrated DVL/IMU System for Precise Navigation of an Autonomous Underwater Vehicle”, OCEANS 2003 Conference Proceedings.

[9] Chong-Moo Lee, Pan-Mook Lee, Seok-Won Hong, Sea-Moon Kim, ”Underwater Navigation System Based on Inertial Sensor and Doppler Velocity Log Using Indirect Feedback Kalman Filter”, International Journal of Offshore and Polar Engineering, Vol. 15, No. 2, June 2005, p. 8895

[10] E. Lefferts, F. Markley, and M. Shuster, “Kalman filtering for spacecraft attitude estimation,” AIAA J. Guidance, Control, Navig., vol. 5, no. 5, pp. 417–429, Sep. 1982. http://dx.doi.org/10.2514/3.56190

[11] Hanai A., Choi S.K., Yuh J., ”A new approach to a laser ranger for underwater robots,” Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2003, Vol. 1, pp. 824-829, October 2003.

[12] P.M Lee, B.H. Jun, “Pseudo long base line navigation algorithm for underwater vehicles with inertial sensors and two acoustic range measurements, Ocean Engineering 34 (2007) 416–425 http://dx.doi.org/10.1016/j.oceaneng.2006.03.011

[13] George C. K, Savvas G. L and Kostas J. K, Visual-Servoing Scheme for Semi-Autonomous Operation of an Underwater Robotic Vehicle Using an IMU and a Laser Vision System, 2010 IEEE International Conference on Robotics and Automation Anchorage Convention District May 3-8, 2010, Anchorage, Alaska, USA

[14] R. Garcia, T. Nicosevici, Towards a Real-Time Vision-Based Navigation System for a Small-Class UUV, Proceedings of the 2003 IEEURSJ Intl. Conference on Intelligent Robots and Systems Las Vegas. Nevada ’ October 2003 http://dx.doi.org/10.1109/IROS.2003.1250730

[15] T. Hamel and R. Mahony, “Attitude estimation on SO(3) based on direct inertial measurements,” in Proc. Int. Conf. Robotics Automation (ICRA), Orlando, FL, 2006, pp. 2170–2175.

[16] S. O.H. Madgwick, A.J.L. Harrison, R. Vaidyanathan, Estimation of IMU and MARG orientation using a gradient descent algorithm, 2011 IEEE International Conference on Rehabilitation Robotics Rehab Week Zurich, ETH Zurich Science City, Switzerland, June 29 - July 1, 2011

[17] M. Zimmerman and W. Sulzer, “High bandwidth orientation measurement and control based on complementary filtering,” presented at the Symp. Robotics Control (SYROCO), Vienna, Austria, 1991

[18] A.-J. Baerveldt and R. Klang, “A low-cost and low-weight attitude estimation system for an autonomous helicopter,” in Proc. IEEE Int. Conf. Intell. Eng. Syst., 1997, pp. 391–395

[19] B. Vik and T. Fossen, “A nonlinear observer for GPS and ins integration,” in Proc. IEEE Conf. Decision and Control, Orlando, FL, Dec. 2001, pp. 2956–2961

[20] Robert Mahony, Tarek Hamel, and Jean-Michel Pflimlin. Non-linear complementary filters on the special orthogonal group. IEEE Transactions on Automatic Control, to appear. Accepted for publication October 2007

[21] M. Euston, P. Coote, R. Mahony, A Complementary Filter for Attitude Estimation of a Fixed-Wing UAV, IEEE/RSJ International Conference on Intelligent Robots and Systems, 2008. IROS 2008

[22] S. K. Hong, “Fuzzy logic based closed-loop strapdown attitude system for unmanned aerial vehicle (uav),” Sensors and Actuators A: Physical, vol. 107, no. 2, pp. 109 – 118, 2003. http://dx.doi.org/10.1016/S0924-4247(03)00353-4

[23] Mark Pedley, “Tilt Sensing Using a Three-Axis Accelerometer”, document number: AN3461, Rev. 6, 03/2013

2nd International Conference on Research in Science, Engineering and Technology (ICRSET’2014), March 21-22, 2014 Dubai (UAE)

http://dx.doi.org/10.15242/IIE.E0314552 91