Attitude Estimation With Accelerometers and Gyros Using Fuzzy Tuned Kalman Filter

AbstractThis paper introduces the attitude estimation method of humanoid robot using an extended Kalman filter with a fuzzy logic based tuning algorithm. A humanoid robot which uses inertial sensors such as gyros and accelerometers to calculate its attitude is considered. It is known that the attitude update using gyros are prone to diverge and hence the attitude error needs to be compensated using accelerometers.

In this paper, a Kalman filter model with a modified state is presented and an adaptive algorithm is used to make the filter more robust regarding acceleration disturbances. If the accelerometer measures any disturbances caused by movement of the vehicle, the characteristics of the filter must be changed to ensure confidence of the outputs of the gyros. The performance of the proposed algorithm is shown by the experiments.

I. INTRODUCTION here has been increasing academic and commercial interest in humanoid robots for various applications,

since the significant recent advances in robot control technology. An effective attitude control capability is one of the most important factors for a humanoid to make it enabled to accomplish its mission. Most land and air vehicles currently use GPS and inertial sensors to calculate the attitude of the vehicles while indoor robots use inertial sensors and vision, or other external signals and cues, for determining the position and attitude. However, in most cases, humanoid robots use only micro-electromechanical systems (MEMS) based inertial sensors to calculate its attitude due to cost and payload and indoor limitations. The inertial measurement unit (IMU), which consists of three gyros and three accelerometers, is used to calculate the attitude. That is a sensor system which is mostly used in the development of robots or micro air vehicle (MAV). In particular, low cost inertial sensors are becoming more interesting for robotic applications with the growth of MEMS sensor technologies.

The gyros measure the angular rate of the vehicle while the accelerometers measure the specific forces of the vehicle. These inertial sensors do not need external sources for the

Manuscript received October 15, 2008. This work was supported by

Korea DAPA and ADD under a fundamental research project (No. UD080015FD), and the Ministry of Education, Science and Technology of Republic of Korea under National Space Lab Program (No. S10801000163- 08A0100-16310).

Chul Woo Kang is with School of Mechanical and Aerospace Eng., Seoul National University, Seoul 151-744, Korea (e-mail: [email protected]).

Chan Gook Park is with School of Mechanical and Aerospace Eng. & Automatic System Research Institute (ASRI), Seoul National University, Seoul 151-744, Korea (e-mail: chanpark@ snu.ac.kr).

attitude measurement. In conventional inertial navigation systems, the computation of the attitude is accomplished by integrating the angular rate obtained from the gyro readings. However, this method is not appropriate for calculating the attitude of the humanoid robot because gyro bias error makes the attitude error to diverge [1]. In this case, the attitude is calculated by using the accelerometer outputs as well as the gyro outputs. Due to the integration process, the attitude updated by the gyros diverges with time, but the outputs of the gyros are stable during some disturbances, and they also have good short term performance. On the other hand, the attitude errors calculated by the accelerometers, do not diverge with time because they are not calculated via integration, but rely on the earths gravity measured on each axis. Although this method has long term attitude stability, the attitude calculation by the accelerometers is heavily influenced by acceleration disturbances caused by movement of the vehicle.

Accordingly, a number of filtering algorithms have been developed for integrating the output of the gyros and the accelerometers, to estimate the attitude. Previous studies of attitude calculation methods, using only inertial sensors, have been researched for use in robotics [2], air vehicles [3], underwater vehicles [4], and human motion tracking [5], [6], [7]. Filtering methods for estimating the gyro bias have been actively researched by Veltink and Luinge [8]. In addition, Bachmann [9] has developed the attitude filter using quaternion and the Kalman filter has been used in much research, particularly in modified states [10]. This method has the advantage of low cost IMU, because it estimates all the states rapidly and it has good observability.

In this paper, the extended Kalman Filter (EKF), using the modified state and fuzzy tuning methods are applied to determine the attitude of a humanoid robot.

II. EXTENDED KALMAN FILTER METHOD FOR ATTITUDE COMPUTATION

A. Euler Angle Attitude Update Attitude calculation methods, similar to inertial navigation

systems, are classified as quaternion methods, direction cosine matrix methods, or Euler angle methods. Among these, the Euler angle method, that updates the Euler angles such as roll( ), pitch( ), and yaw( ) uses the relationship between the body angular rotation rate and the derivative of the Euler

Attitude Estimation with Accelerometers and Gyros

Using Fuzzy Tuned Kalman Filter Chul Woo Kang and Chan Gook Park

T

Proceedings of the European Control Conference 2009 Budapest, Hungary, August 2326, 2009 WeA7.4

ISBN 978-963-311-369-1 Copyright EUCA 2009

Attitude Estimation with Accelerometers and Gyros Using Fuzzy Tuned Kalman Filter

3713

angles.

The Euler angle has the merit of being a more meaningful attitude expression than either the quaternion method or the direction cosine matrix method; and the user can recognize the attitude of the vehicle directly. The definition of the Euler angle is represented in Fig. 1

The transformation from reference frame to body frame can be defined by three successive rotations about different axes taken in turn. Firstly, the reference frame rotates to

2 2 2X Y Z frame by the yaw angle; next it rotates to 3 3 3X Y Z frame by its pitch angle, and finally we obtain the body frame from the rotation of 3 3 3X Y Z frame, by the roll angles. Overall the coordinate transform matrix ( bnC ) is calculated from the direction cosine matrix of each rotation transformation stated above.

3 23 2( ) ( ) ( )cos cos sin sin cos cos sin cos sin cos sin sincos sin sin sin sin cos cos cos sin sin sin cos

sin sin cos cos cos

b bn nC C C C

=

+ = +

(1)

where,

3

1 0 0( ) 0 cos sin

0 sin cos

bC

=

(2)

32

cos 0 sin( ) 0 1 0

sin 0 cosC

=

(3)

2

cos sin 0( ) sin cos 0

0 0 1nC

=

(4)

A relation between Euler angles and gyro outputs of each axis can be expressed as

33 3 2

0 00 ( ) ( ) ( ) 00 0

xb b

y

z

C C C

= + +

(5)

Substituting Eqs. (2)-(4) into Eq. (5), we get 1 sin tan cos tan0 cos sin0 sin sec cos sec

x

y

z

=

(6)

where each x , y , z are the gyro outputs in the x, y, z axis. The primary concern of this study is to obtain the roll and pitch angles which are represented by the gyro outputs as following.

sin tan cos tanx y z = + + (7) cos siny z = (8)

These equations are used for the Kalman filter process model.

Yaw angle is updated by equation (9). sin sec cos secy z = + (9)

Yaw angle will diverge because there is no any other measurement to compensate gyro bias errors. Yaw angle is separated from roll and pitch angle, so that roll and pitch axis is stable from divergence of yaw angle.

B. Extended Kalman filter structure

The EKF attitude estimation algorithm is composed as shown in Fig. 2 where the gyro outputs are used to process the model, which is not significantly affected by the motion of the humanoid robot. Accelerometer outputs are used for the measurement model that is affected by the disturbances in he system. Thus, when humanoid motion is sensed, the filter needs an adjustment in order to trust the gyros more than the accelerometers. The fuzzy reasoning method is applied to the sequence of filter adaptation. When body motion is measured by the IMU, fuzzy reasoning decides the appropriate filter parameters.

C. Process model In this research, modified Euler angles were used as the

state variables [4], [10]. These states are defined as 1 sinC = (10) 2 sin cosC = (11) 3 cos cosC = (12) The system model is derived from differentiating these

states using (7) and (8).

1 2 3cos z yC C C = = (13) 2 1 3cos cos sin sin z xC C C = = + (14) 3 1 2sin cos cos sin y xC C C = = (15)

N E

2,D Z

2X

2 3,Y Y

3, bX X

3ZbZ

bY

Fig. 1. Definition of Euler Angle.

Fig. 2. Structure of extended Kalman filter attitude estimator

C. W. Kang and C. G. Park : Attitude Estimation with Accelerometers and Gyros Using Fuzzy Tuned Kalman Filter WeA7.4

3714

Above equations can be simply rewritten in the matrix-vector form as

1 1

2 2

3 3

00 ( )

0

z y

z x

y x

C CC C W tC C

= +

(16)

( )W t means the process noise with zero mean Gaussian noise. This process model uses only the gyro outputs and simple linear equations so the model is affected by only the gyro errors, but it is not affected by external disturbances because nothing is assumed.

D. Measurement model Accelerometer outputs are used in the measurement

equation and the outputs expressed in the body frame are represented as shown in equation (17).

sinsin coscos cos

bb b b bnb

u ru qw gf v g v ru pw g

w qu pv gv

+ = + = + +

+

(17)

Here , ,u v w are velocities of each axis and , ,u v w are the accelerations of each axis, which cannot be directly measured without external signals. Thus we ignore those terms and assume that the IMU is in a steady state.

1

2

3

sinsin coscos cos

x

y

z

f Cf g g Cf C

=

(18)

If those accelerations and velocities exist, they will act as disturbances.

In addition to this linear measurement model, we use the measurement the following constraint as

2 2 2 2 2 2 2 21 2 3 sin sin cos cos cos 1C C C + + = + + = (19) Hence the measurement equation from Eq (18), (19) can be

written as; 1

21 2 3

32 2 2

1 2 3

( , , )

1

x

y

z

gCfgCf

h C C CgCf

C C C

= = + +

(20)

And the measurement matrix is H, as shown in the following equation (21).

1 2 3

0 00 00 0

2 2 2

gghH

gxC C C

= =

(21)

This measurement equation is nonlinear and has a critical assumption to ignore the other accelerations except gravity. Hence when the movement of the humanoid is faster, this measurement equation will be more uncertain.

III. FUZZY ADAPTATION Typical adaptive filtering, such as MMAE(multiple model

adaptive estimation)or IAE(innovation adaptive estimation), uses the algorithms associated with the innovations or the residuals [12], [13]. The residual comparing the measured value and the estimated values, provide the criterion of accuracy to estimated states. If the residual do not have appropriate values, then the adaptive algorithm makes changes to the filter parameters to improve the filter performance.

Recently, there have been several researches which have adapted the parameters of the Kalman filter [14] in effective ways. The Takagi-Sugeno fuzzy model (TSK fuzzy model) is a famous fuzzy model which is generated from a given input-output data set [15]. A typical rule in a Sugeno model has the form

- If x is A and y is B then ( , )z f x y= where A and B are fuzzy sets in the antecedent, while

( , )z f x y= is a function in the consequent. In this paper, fuzzy rules are used to modify the measurement covariant matrix R. The Kalman gain is calculated from the following equation [16].

1Tk k k kK P H R

+ = (22)

With we have a large R then a small Kalman gain emerges and the measurement is nearly ignored, but the process model is trusted with a small Kalman gain. The proposed system has a higher confidence to the gyros, which are used in the process model, in a dynamic situation. Thus the R must be enlarged in a moving body. The criterion of a dynamic property of body is the angular rates and the accelerations. When normalized acceleration and angular rate are large, R must be changed to be large because the movement of the body is supposed to be measured. The normalized acceleration is as following and the magnitude of the gyro rates is described as shown in equation (23).

( )22 /f g g = (23)

0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08 0.09 0.1

0

0.2

0.4

0.6

0.8

1

acceleration.normalized

Deg

ree

of m

embe

rshi

p small large

0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 0.04 0.045 0.05

0

0.2

0.4

0.6

0.8

1

angular.rate

Deg

ree

of m

embe

rshi

p small large

Fig. 3. Membership function of acceleration and angular rate


3715

2 = (24)

Output is following these four fuzzy rules. 1) If is small and is small then 0z = 2) If is small and is large then 1 2 3z a a a = + + 3) If is large and is small then 1 2 3z a a a = + + 4) If is large and is small then 1z = Figure 3 shows the fuzzy rules.

Using fuzzy outputs above the fuzzy rules, R is changed with following rules.

( )21 2 3 3 00 1

k z k IR +

= (25)

The input-output behavior is obtained as seen in fig. 4. with the 1 2 33.5, 8, 0.5a a a= = = where the coefficients are determined heuristically. 1k and 2k are tuning parameters which are determined from experiments. In this experiment,

51 10k = and 2 400k = are used.

IV. EXPERIMENTAL RESULT Experiments have been carried out with MTi, which is a

commercial IMU from Xsens. The attitude output of MTi has an attitude performance specification of 0.5 deg in static situations and 2 degress in dynamic situations. MTi also offers calibrated sensor outputs that can be used as the IMU. To test the fuzzy EKF attitude calculation method, the attitude result is compared with the attitude output of MTi. The attitude output from MTi, and the proposed method with a sensor output of MTi, use the same data, thus the attitude result comparison means only a comparison of the differences in the attitude calculation method.

The first experiment is a roll rotation test where the IMU starts from a zero roll and pitch state and then moves to a 45 deg roll, with 50 deg/sec, using a 2 axis motion table. This motion mimics the walking motion of the humanoid.

We can see the both attitude results do not diverge but

remain stable near zero with an initial angle. However, there exist some nonzero values by mis-orientation errors that is caused by the differences between attitude of motion table and IMU. A mis-orientation error is assumed to be 2.6 deg, which is an initial attitude, and a 45 deg rotation is changed to a 47.6 deg rotation. The following figure shows the same result which is enlarged in 20~56 seconds.

Both outputs have the attitude error under 1deg compared

20 25 30 35 40 45 50 5546.5

47

47.5

48

48.5

time(s)

roll(

deg)

20 25 30 35 40 45 50 55-2

-1.8

-1.6

-1.4

time(s)

pitc

h(de

g)

MTiKalman Filter

MTiKalman Filter

Fig. 6. Roll rotation test result (close up)

00.02

0.040.06

0.080.1

0

0.02

0.04

0

0.2

0.4

0.6

0.8

1

acceleration.normalizedangular.rate

Rva

lue

Fig. 4. Overall input-output surface

0 10 20 30 40 50 60 70 80-50

0

50

time(s)

roll(

deg)

MTiKalman Filter

0 10 20 30 40 50 60 70 80-1

-0.5

0

0.5

1

time(s)

pitc

h(de

g)

MTiKalman Filter

Fig. 7. Roll rotation test upon dynamic situation

0 10 20 30 40 50 60 70-50

0

50

time(s)

roll(

deg)

0 10 20 30 40 50 60 70-2

-1

0

1

2

3

time(s)

pitc

h(de

g)

MTiKalman Filter

MTiKalman Filter

Fig. 5. Roll rotation test result


3716

with the true attitude. While the attitude outputs of MTi show significantly different attitude results on the first and the second turns, the fuzzy EKF attitude calculation method shows nearly same attitude result on first turn and second turn. This result shows that the new attitude calculation method has a better performance. Thus the fuzzy EKF attitude calculation method is more stable in dynamic situations.

To apply more extreme accelerations, the IMU is mounted on 30cm distance from the rotation axis of the motion table, and tested to the same roll rotation and the following graph shows similar results.

In this situation, the acceleration disturbance is bigger than the upper case. The fuzzy EKF method shows better performance than MTi, relatively.

To test the robustness about the accelerometer disturbance, the centrifugal force was used and the IMU, which is put on 30cm from the rotation axis with a roll of approximately 90 deg, is turned along the yaw direction. In this test, the roll and pitch angles must not be changed, but the accelerometer disturbance affects the roll angle. Figure 9 shows the result of this experiment where the dashed line describes the result of the Kalman filter without using the fuzzy adaptive method.

Without adaptation, the roll angle is affected to approximately 12 deg by the acceleration disturbances.

Robustness characteristics can be seen in the disturbances

from the magnified figure. The result of the Kalman filter shows better robustness because its result sustains a more stable attitude during the 24~31 second time period.

V. CONCLUSION In this paper, a new attitude estimation algorithm has been

developed using the fuzzy adaptive extended Kalman filter. In order to design the EKF, the state variables have been defined using the relation of Euler angles and angular rates. From these states the process model could be expressed as a linear equation, and in the measurement model the accelerometer outputs are directly used without other complex transformations. However, this measurement model has acceleration disturbances whenever the robot maneuvers. To compensate it, a parameter adaptation algorithm has been implemented based on fuzzy logic which makes a decision about the dynamic state of the robot. If the motion is severe, by setting the large magnitude of R the accelerometer measurement will be ignored.

This proposed attitude calculation method was tested with the attitude output of MTi and the fuzzy EKF method showed more robust results than MTi in the experiments mimicking humanoid robotic motions. Moreover, when unwanted disturbances strongly affected the IMU, the proposed attitude calculation algorithm worked well. Future studies using the fuzzy adaptation method will be applied to estimate the gyro bias.

REFERENCES [1] D. H. Titterton and J. L. Weston, Strapdown Inertial Navigation

Technology. Stevenage, U.K.: Peregrinus, 1997. [2] N. Miller, O.C. Jenkins, M. Kallmann, M. J. Mataric, Motion capture

from inertial sensing for untethered humanoid teleoperation, Humanoid Robots, 2004 4th IEEE/RAS International Conference on, vol. 2, pp. 547- 565, Nov. 2004.

[3] Jong Nam Lim, Design of Attitude Estimation System for Micro Aerial Vehicle, Thesis, of Mechanical and Aerospace Engineering, Seoul National University., Korea, Seoul, 1993.

[4] H. Hong, J. G Lee, C. G. Park, H. S. Han, A leveling algorithm for an underwater vehicle using extended Kalman filter, Proceedings of the

0 10 20 30 40 50 60-50

0

50

100

150

time(s)

roll(

deg)

0 10 20 30 40 50 60-15

-10

-5

0

5

10

time(s)

pitc

h(de

g)

MTiKalman Filter without changing RKalman Filter

Acc. disturbance

Fig. 9. Yaw rotation test result

22 23 24 25 26 27 28 29 30 31 32 3388

90

92

94

96

time(s)

roll(

deg)

22 23 24 25 26 27 28 29 30 31 32 33-10

-5

0

5

10

time(s)

pitc

h(de

g)

MTiKalman Filter without changing RKalman Filter

Fig. 10. Yaw rotation test result

0 10 20 30 40 50 60 70-43

-42.8

-42.6

-42.4

-42.2

-42

time(s)

roll(

deg)

MTiKalman Filter

0 10 20 30 40 50 60 70-1

-0.5

0

0.5

1

time(s)

pitc

h(de

g)

Fig. 8. Roll rotation test upon dynamic situation (close up)


3717

IEEE 1998 Position Location and Navigation Symposium, Palm Springs, California, U. S. A., April 20-23, pp. 280-285, 1998.

[5] E. R. Bachmann, I. Duman, U. Usta, R. B. McGhee, X. Yun, and M. J. Zyda. Orientation tracking for humans and robots using inertial sensors, International Symposium on Computational Intelligence in Robotics and Automation, pp. 187194, 1999.

[6] D. Rotenberg , H. Luinge , C. Baten and P. Veltink Compensation of magnetic disturbances improves inertial and magnetic sensing of human body segment orientation, IEEE Trans. Neural Syst. Rehabil. Eng., vol. 13, pp. 395, Sep. 2005.

[7] E. Foxlin, Inertial Head-Tracker Sensor Fusion by a Complementary Separate-Bias Kalman Filter, IEEE Proceedings of VRAIS `96, pp. 185-194, 1996.

[8] Daniel Roetenberg, Per J. Slycke, and Peter H. Veltink, Ambulatory Position and Orientation Tracking Fusing Magnetic and Inertial Sensing, IEEE TRANSACTIONS ON BIOMEDICAL ENGINEERING, VOL. 54, NO. 5, MAY 2007

[9] Xiaoping Yun: Mariano Lizarraga, Eric R. Bachmann! and Robert B. McGhee, An Improved Quaternion-Based Kalman Filter for Real-Time Tracking of Rigid Body Orientation, Proceedings of the 2003 IEEE/RSJ Intl. Conference on Intelligent Robots and Systems, Las Vegas. Nevada, October 2003

[10] Chul Woo Kang, Young Min Yoo, Chan Gook Park, Performance Improve of Attitude Estimation Using Modified Euler Angle Based Kalman Filter, Journal of Institute of Control, Robotics and Systems, Vol. 14, No. 9, September 2008.

[11] A. Gelb, Applied Optimal Estimation, ed. MIT Press, 1992. [12] Magill, D.T., 1965. Optimal adaptive estimation of sampled stochastic

processes. IEEE Transaction on Automatic Control AC-10 4, pp. 434439

[13] Mehra, R.K., 1970. On the identification of variances and adaptive Kalman filtering, IEEE Transactions on Automatic Control AC-15 2, pp. 175184.

[14] Loebis D., Sutton R., Chudley J., and Naeem W, Adaptive tuning of a Kalman filter via fuzzy logic for an intelligent AUV navigation system, Control Engineering Practice, vol. 12, no. 12 SPEC. ISS., pp. 1531-1539, December 2004.

[15] J. S. Jang, C. T. Sun, E. Mizutani, Neuro-fuzzy and Soft Computing, Pretice Hall, 1997.

[16] D. Simon, Optimal State Estimation: Kalman, H-Innity, and Nonlinear Approaches, Wiley & Sons, 2006.


3718

/ColorImageDict > /JPEG2000ColorACSImageDict > /JPEG2000ColorImageDict > /AntiAliasGrayImages false /CropGrayImages true /GrayImageMinResolution 36 /GrayImageMinResolutionPolicy /Warning /DownsampleGrayImages true /GrayImageDownsampleType /Bicubic /GrayImageResolution 300 /GrayImageDepth -1 /GrayImageMinDownsampleDepth 2 /GrayImageDownsampleThreshold 2.00333 /EncodeGrayImages true /GrayImageFilter /DCTEncode /AutoFilterGrayImages false /GrayImageAutoFilterStrategy /JPEG /GrayACSImageDict > /GrayImageDict > /JPEG2000GrayACSImageDict > /JPEG2000GrayImageDict > /AntiAliasMonoImages false /CropMonoImages true /MonoImageMinResolution 36 /MonoImageMinResolutionPolicy /Warning /DownsampleMonoImages true /MonoImageDownsampleType /Bicubic /MonoImageResolution 600 /MonoImageDepth -1 /MonoImageDownsampleThreshold 1.00167 /EncodeMonoImages true /MonoImageFilter /CCITTFaxEncode /MonoImageDict > /AllowPSXObjects false /CheckCompliance [ /None ] /PDFX1aCheck false /PDFX3Check false /PDFXCompliantPDFOnly false /PDFXNoTrimBoxError true /PDFXTrimBoxToMediaBoxOffset [ 0.00000 0.00000 0.00000 0.00000 ] /PDFXSetBleedBoxToMediaBox true /PDFXBleedBoxToTrimBoxOffset [ 0.00000 0.00000 0.00000 0.00000 ] /PDFXOutputIntentProfile (None) /PDFXOutputConditionIdentifier () /PDFXOutputCondition () /PDFXRegistryName (http://www.color.org) /PDFXTrapped /False

/CreateJDFFile false /Description >>> setdistillerparams> setpagedevice

Documents

Attitude Estimation With Accelerometers and Gyros Using Fuzzy Tuned Kalman Filter