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: julio7@snu.ac.kr).
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 2
X Y Z frame by the yaw angle; next it rotates to
3 3 3
X Y Z
frame by its pitch angle, and finally we obtain the body frame
from the rotation of
3 3 3
X Y Z frame, by the roll angles. Overall
the coordinate transform matrix (
b
n
C ) is calculated from the
direction cosine matrix of each rotation transformation stated
above.
3 2
3 2
( ) ( ) ( )
cos cos sin sin cos cos sin cos sin cos sin sin
cos sin sin sin sin cos cos cos sin sin sin cos
sin sin cos cos cos
b b
n n
C C C C



=
+ (
(
= +
(
(

(1)
where,
3
1 0 0
( ) 0 cos sin
0 sin cos
b
C

(
(
=
(
(

(2)
3
2
cos 0 sin
( ) 0 1 0
sin 0 cos
C

(
(
=
(
(

(3)
2
cos sin 0
( ) sin cos 0
0 0 1
n
C


(
(
=
(
(

(4)
A relation between Euler angles and gyro outputs of each
axis can be expressed as
3
3 3 2
0 0
0 ( ) ( ) ( ) 0
0 0
x
b b
y
z
C C C



(
( ( (
(
( ( (
= + +
(
( ( (
(
( ( (

(5)
Substituting Eqs. (2)-(4) into Eq. (5), we get
1 sin tan cos tan
0 cos sin
0 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 tan
x y z
= + +

(7)
cos sin
y z
=

(8)
These equations are used for the Kalman filter process
model.
Yaw angle is updated by equation (9).
sin sec cos sec
y 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
sin C = (10)
2
sin cos C = (11)
3
cos cos C = (12)
The system model is derived from differentiating these
states using (7) and (8).
1 2 3
cos
z y
C C C = =

(13)
2 1 3
cos cos sin sin
z x
C C C = = +

(14)
3 1 2
sin cos cos sin
y x
C C C = =

(15)
N E
2
, D Z
2
X
2 3
, Y Y
3
,
b
X X
3
Z b
Z
b
Y

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
0
0 ( )
0
z y
z x
y x
C C
C C W t
C 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).
sin
sin cos
cos cos
b
b b b b
nb
u ru qw g
f v g v ru pw g
w qu pv g
v



+ ( (
( (
= + = + +
( (
( ( +

(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
sin
sin cos
cos cos
x
y
z
f C
f g g C
f 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 2
1 2 3
sin sin cos cos cos 1 C C C + + = + + = (19)
Hence the measurement equation from Eq (18), (19) can be
written as;
1
2
1 2 3
3
2 2 2
1 2 3
( , , )
1
x
y
z
gC f
gC f
h C C C
gC f
C C C
( (
( (

( (
= =
( (
( (
+ +

(20)
And the measurement matrix is H, as shown in the
following equation (21).
1 2 3
0 0
0 0
0 0
2 2 2
g
g
h
H
g x
C 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].
1 T
k k k k
K 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).
( )
2
2
/ 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
D
e
g
r
e
e

o
f

m
e
m
b
e
r
s
h
i
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
D
e
g
r
e
e

o
f

m
e
m
b
e
r
s
h
i
p
small large

Fig. 3. Membership function of acceleration and angular rate
Proceedings of the European Control Conference 2009 Budapest, Hungary, August 2326, 2009 WeA7.4
3715



2
= (24)
Output is following these four fuzzy rules.
1) If is small and is small then 0 z =
2) If is small and is large then
1 2 3
z a a a = + +
3) If is large and is small then
1 2 3
z a a a = + +
4) If is large and is small then 1 z =
Figure 3 shows the fuzzy rules.
Using fuzzy outputs above the fuzzy rules, R is changed
with following rules.
( )
2
1 2 3 3
0
0 1
k z k I
R

| |
+
= |
|
\ .
(25)
The input-output behavior is obtained as seen in fig. 4. with
the
1 2 3
3.5, 8, 0.5 a a a = = = where the coefficients are
determined heuristically.
1
k and
2
k are tuning parameters
which are determined from experiments. In this experiment,
5
1
10 k = and
2
400 k = 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 55
46.5
47
47.5
48
48.5
time(s)
r
o
l
l
(
d
e
g
)


20 25 30 35 40 45 50 55
-2
-1.8
-1.6
-1.4
time(s)
p
i
t
c
h
(
d
e
g
)


MTi
Kalman Filter
MTi
Kalman Filter

Fig. 6. Roll rotation test result (close up)
0
0.02
0.04
0.06
0.08
0.1
0
0.02
0.04
0
0.2
0.4
0.6
0.8
1
acceleration.normalized
angular.rate
R
v
a
l
u
e

Fig. 4. Overall input-output surface
0 10 20 30 40 50 60 70 80
-50
0
50
time(s)
r
o
l
l
(
d
e
g
)


MTi
Kalman Filter
0 10 20 30 40 50 60 70 80
-1
-0.5
0
0.5
1
time(s)
p
i
t
c
h
(
d
e
g
)


MTi
Kalman Filter

Fig. 7. Roll rotation test upon dynamic situation
0 10 20 30 40 50 60 70
-50
0
50
time(s)
r
o
l
l
(
d
e
g
)


0 10 20 30 40 50 60 70
-2
-1
0
1
2
3
time(s)
p
i
t
c
h
(
d
e
g
)


MTi
Kalman Filter
MTi
Kalman Filter

Fig. 5. Roll rotation test result
C. W. Kang and C. G. Park : Attitude Estimation with Accelerometers and Gyros Using Fuzzy Tuned Kalman Filter WeA7.4
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)
r
o
l
l
(
d
e
g
)


0 10 20 30 40 50 60
-15
-10
-5
0
5
10
time(s)
p
i
t
c
h
(
d
e
g
)


MTi
Kalman Filter without changing R
Kalman Filter
Acc. disturbance

Fig. 9. Yaw rotation test result
22 23 24 25 26 27 28 29 30 31 32 33
88
90
92
94
96
time(s)
r
o
l
l
(
d
e
g
)


22 23 24 25 26 27 28 29 30 31 32 33
-10
-5
0
5
10
time(s)
p
i
t
c
h
(
d
e
g
)


MTi
Kalman Filter without changing R
Kalman 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)
r
o
l
l
(
d
e
g
)


MTi
Kalman Filter
0 10 20 30 40 50 60 70
-1
-0.5
0
0.5
1
time(s)
p
i
t
c
h
(
d
e
g
)



Fig. 8. Roll rotation test upon dynamic situation (close up)
Proceedings of the European Control Conference 2009 Budapest, Hungary, August 2326, 2009 WeA7.4
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.
C. W. Kang and C. G. Park : Attitude Estimation with Accelerometers and Gyros Using Fuzzy Tuned Kalman Filter WeA7.4
3718