172

Attitude Estimation Adaptively Compensating External

Acceleration∗

Young-Soo SUH∗∗ , Sang-Kyeong PARK∗∗ , Hee-Jun KANG∗∗ and Young-Shick RO∗∗

This paper is concerned with attitude estimation using low cost, small-sized accelerom-
eters and gyroscopes. A two step extended Kalman filter is proposed, which adaptively com-
pensates external acceleration. External acceleration is the main source of estimation error.
In the proposed filter, direction of external acceleration is estimated. According to the esti-
mated direction, the accelerometer measurement covariance matrix of the two step extended
Kalman filter is adjusted. The proposed algorithm is verified through experiments.

Key Words: Attitude Estimation, Inertial Sensors, Kalman Filter, Adaptive Filter

celeration. External acceleration, which affects attitude

1. Introduction
estimation based on accelerometers, is the major source of
Attitude estimation is necessary in many different ap- attitude estimation error. Similar approaches, which also
plications. Probably the most extensively studied area is adaptively compensate external acceleration, are used in
attitude estimation in inertial navigation systems (INS)(1) . Refs. (7) and (8). A brief comparison is made in section 4.
In INS, attitude is very accurately estimated using expen- We propose more sophisticated adaptive algorithm.
sive accelerometers and gyroscopes. Though the princi-
2. Inertial Sensors for Attitude Estimation
ple of attitude estimation in INS is relatively simple, until
recently it has not been applied to more mundane appli- Attitude in the paper means pitch angle (θ) and roll
cations such as robots. The reason is accelerometers and angle (φ) of the Euler angles. The heading (yaw angle) is
gyroscopes are too expensive and too large for most appli- not considered in the paper. The Euler angles are the an-
cations. gular rotation between the body axis (xb , yb , zb ) and the
However, due to recent electro-mechanical techni- inertial axis (x f , y f , z f ): we follow the standard aeronau-
cal advance, in particular due to microelectromechanical tics convention in Ref. (9).
systems, low cost, small-sized accelerometers and gyro- To estimate attitude, we use 6 measurement variables:
scopes have been developed(2) . Subsequently these sen- • (a x , ay , az ): accelerometer outputs in the body axis
sors are used in attitude estimation of many applications. (xb , yb , zb )
Basically attitude can be estimated using accelerom- • (g x , gy , gz ): gyroscope outputs, which measure an-
eters only by measuring the gravitational field. However, gular rates (ω x , ωy , ωz ) around the body axis (xb , yb , zb )
due to disturbances (most notably external acceleration), As an accelerometer, we used ADXL202. The
gyroscopes are also used to reduce effects of disturbances. ADXL202 is a low cost (about US$30), small-sized
Thus the key issue is how to combine accelerometers and (10.6 × 9.9 × 5.4 mm3 ) 2 axis accelerometer with a mea-
gyroscopes to obtain good attitude estimation. Almost all surement range of ±2g. To measure 3 axis acceleration,
papers use the Kalman filter to do this: attitude estimation two ADXL202 are used.
for mobile robots(3) – (5) , for a walking robot(6) , and for a As a gyroscope, we used Murata Gyrostar (ENV-
head-tracker(7) . 05F-03). The Gyrostar is a low cost (about US$70),
In our paper, we propose the two step extended small-sized (11.5×19.6×27.2 mm3 ) piezo-type gyroscope
Kalman filter, which adaptively compensates external ac- with a measurement range of ±60 deg/sec. The bias drift
∗ (9 deg/sec) in the data sheet is very large: it means that the
Received 19th March, 2004 (No. 04-5033)
∗∗
Department of Electrical Engineering, University of Ul- gyroscope integration error could be as large as 90 degrees
san, Mugeo, Nam-gu, Ulsan 680–749, Korea. after 10 seconds. However, our experiment shows that the
E-mail: [email protected]; [email protected]; actual error is far smaller: less than 1 degree after 10 sec-
[email protected]; [email protected] onds. Thus for short term attitude estimation, gyroscope

Series C, Vol. 49, No. 1, 2006 JSME International Journal

 173

 
integration can be used. To measure 3 axis rotation, three  sinθ 
 
Gyrostars are used.  sinφcosθ 
f (x(t))
 ωx  .
Attitude can be estimated using accelerometers only
 
by measuring the gravitational acceleration. From simple  ωy 

geometry, we have ωz

a 
y Process and measurement noise w(t) and v(t) are assumed
θ = sin−1 (a x ) and φ = sin−1 (1)
cosθ to be uncorrelated zero-mean white Gaussian processes
where all accelerometer outputs are normalized with the satisfying
gravitational acceleration constant g. For example, a x = 1  
 0 0 0 0 0 
means that 1 g = 9.8 m/sec2 is sensed by the xb axis ac-  0 0 0 0 0 
 
celerometer. Note that only a x and ay are needed for atti- Q =  0 0 q1 0 0  = E{w(t)w(t) }
 
 0 0 0 q1 0 
tude estimation. Although az is not directly used for atti-
tude estimation, az plays an important role in the proposed 0 0 0 0 q1
algorithm.  
 r1 0 0 0 0 
Attitude estimation error in Eq. (1) could be large  
when there are external acceleration: accelerometers can-  0 r2 0 0 0 

R =  0 0 r3 0 0  = E{v(t)v(t) }.
not tell difference between the gravitational acceleration  
and external acceleration. To avoid this problem, gyro-  0 0 0 r3 0 
0 0 0 0 r3
scopes are additionally used for attitude estimation. At-
titude can be estimated by integrating gyroscope outputs. Once the system model Eq. (3) is chosen, the remain-
However, the integration error inevitably accumulates as ing thing is to select the covariance matrix Q and R. Since
time goes by; thus gyroscope-based attitude estimation is the estimation error depends on how Q and R are selected,
reliable only for the short time. it is important to choose correct Q and R values.
The key issue when we combine accelerometers and In Eq. (3), the first two rows represent the standard
gyroscopes for attitude estimation is how to weight each relationship between (θ̇, φ̇) and (ω x , ωy , ωz ). Since this
sensor output. When the object is under external acceler- relationship is exact, the first 2×2 block of Q is a zero ma-
ation, we should rely more heavily on gyroscope outputs trix. The last three rows imply that we assume the deriva-
because accelerometer outputs contain unwanted external tives of (ω x , ωy , ωz ) are uncorrelated white noises and its
acceleration information. These issues will be considered covariances are all q1 . This assumption is made because
in section 4. of simplicity although it is possible that the assumption is
not true for many real situations. The covariance value q1
3. Standard Extended Kalman Filter reflects our knowledge about (ω̇ x , ω̇y , ω̇z ). For example,
In this section, we introduce the standard Kalman small q1 value means that we assume that (ω x , ωy , ωz ) of
filter for attitude estimation. In section 4, the standard the object is slowly changing.
Kalman filter will be modified so that external disturbance The measurement noise covariance R is a diagonal
is adaptively compensated. The state x(t) and the mea- matrix, which means that all sensors are assumed to be
surement z(t) are defined as follows: uncorrelated. Usually, R is chosen from sensor character-
    istics. In this case, ri indicates how good or bad the given
 θ   a x  sensor is: for example, large r3 value means that the gyro-
 φ   ay 
    scope output noise is large.
x(t)
 ω x  , z(t)
 g x  . (2) Since the measurement output is sampled, a dis-
   
ω
 y  g
 y  cretized system of Eq. (3) is used. Suppose the sampling
ωz gz period is T . An exact discretized system is a highly non-
The system equation is given by linear system; thus to obtain a simplified discretized sys-
tem, we assume that A(t) is constant during the sampling
ẋ(t) = A(t)x(t) + w(t) period. Then the discretized system is given by Ref. (10).
z(t) = f (x(t)) + v(t) (3)
xk+1 = Φk xk + wk
where zk = f (xk ) + vk (4)
 
 0 0 0 cosφ(t) −sinφ(t) 
  where
 0 0 1 sinφ(t)tanθ(t) cosφ(t)tanθ(t) 

A(t)
 0 0 0 0 0  , xk
x(kT ) and zk
z(kT )
 

 0 0 0 0 0 
Φk
exp(A(kT )T ) =
I W(kT )T
0 0 0 0 0 0 I

JSME International Journal Series C, Vol. 49, No. 1, 2006

174

0 cosφ(kT ) −sinφ(kT ) can be interpreted as selecting very large r1 and r2 when

W(kT )
.
1 sinφ(kT )tanθ(kT ) cosφ(kT )tanθ(kT ) Eq. (6) is not satisfied.
Process noise covariance matrix Qk of the discretized In this paper, we use the same method to check exis-
system is given by tence of external acceleration: it is assumed that there is
external acceleration if
Qk = E{wk wk }  
 T f (a ,a ,a )
a2 + a2 + a2 − 1 > δ
x y z x y z (7)
≈ exp(A(kT )s)Qexp(A(kT ) s) ds where δ is a scalar parameter depending on accelerome-
0
1  ter measurement noise characteristics. When existence of
 q T 3 W(kT )W(kT ) 1 q T 2 W(kT ) 
 3 1 2
1  external acceleration is detected by Eq. (7), we use more
=   . sophisticated method to adjust r1 and r2 . The direction of
 1
q1 T 2 W(kT ) q1 T I

2 external acceleration is estimated and according to the di-
The measurement noise covariance matrix of the dis- rection, r1 and r2 are adjusted. To do this, we propose the
cretized system is the same as that of Eq. (3). Now the two step extended Kalman filter:
standard extended Kalman filter for Eq. (4) can be used, • Initialization
where Ck is given by – x̂0 : Initial attitude
 – P0 : set 0
∂ f (x) 
Ck =  • Time Update
∂x  x= x̂−k
  x̂−k+1 = Φk x̂k
 cos θ̂(kT ) 0 0 0 0 

 P−k+1 = Φk Pk Φk + Qk
 0 cos φ̂(kT )cos θ̂(kT ) 0 0 0 

=  0 0 1 0 0  (5) • Measurement Update Step 1: Gyroscope measure-
 
 0 0 0 1 0  ments only are updated

0 0 0 0 1 Kk,g = P−k C2 (C2 P−k C2 + R2 )−1
x̂k,g = x̂−k + Kk,g (zk,2 −C2 x̂−k ) (8)
4. Two Step Extended Kalman Filter
Pk,g = (I − Kk,gC2 )P−k (I − Kk,gC2 ) + Kk,g R2 Kk,g


The main drawback of the standard extended Kalman where

filter is that the estimation error becomes large if the object  
  r3 0 0 
2

is experiencing external acceleration. The Kalman filter   zk,1 R

C2
0 I3 , R2
 0 r3 0  , zk
∈ 3
contains the model Eq. (1), which is not valid when there   zk,2 R
0 0 r3
is external acceleration. This problem cannot be avoided
even if very accurate accelerometers are used. Thus when • Accelerometer noise covariance adjustment
there is external acceleration, gyroscope outputs should – if Eq. (7) is satisfied, then




be trusted more and this can be done by making r1 and r1,k r1,k−1 r1,nom
= max α1 + α2 (zk,1 −C1,k x̂k,g ),
r2 large. Similar ideas are employed in Refs. (7) and (8), r2,k r2,k−1 r2,nom
though different system models are used. (9)
In Ref. (7), if gyroscope outputs are zero and in- where α1 (|α1 | < 1) and α2 are scalar parameters and C1,k
clinometer outputs are not changing, then it is assumed can be obtained from the following partition of Ck :
that external acceleration does not exist and accelerometer

C1,k
measurement noise covariance (corresponding to r1 and Ck = .
C2
r2 ) is adjusted to small value.
In Ref. (8), the following observation is used to detect – if Eq. (7) is not satisfied, then

existence of external acceleration. r1,k r1,k−1 r1,nom

= α1 + . (10)
Observation: A necessary condition for acceleration free r2,k r2,k−1 r2,nom
movements is • Measurement Update 2: Acceleration measure-
a2x + a2y + a2z = 1. (6) ments are now updated
 
The above observation states that if external accelera- Kk,a = Pk,gC1,k (C1,k Pk,gC1,k + R1,k )−1
tion does not exists, then acceleration sensed by 3 axis ac- x̂k = x̂k,g + Kk,a (zk,1 −C1,k x̂k,g ) (11)
celerometer should be the gravitational acceleration only. Pk = (I − Kk,aC1,k )Pk,g (I − Kk,aC1,k )  
+ Kk,a R1,k Kk,a
Recall that the accelerometer outputs are normalized with
where
the gravitational acceleration so that Eq. (6) is satisfied.

In Ref. (8), if Eq. (6) is not satisfied, only gyroscope out- r1,k 0
R1,k = .
puts are used to estimated attitude. In our framework, this 0 r2,k

Series C, Vol. 49, No. 1, 2006 JSME International Journal

 175

Fig. 1 Experiment 1: accelerometer and gyroscope outputs when there is neither attitude
change nor external acceleration

In Eq. (8), only gyroscope outputs are used to esti-

mate the state. Note that Eq. (8) is nothing but the stan-
dard Kalman filter equation when only gyroscope out-
puts (zk,2 ) are available. In Eq. (9), r1,k and r2,k are ad-
justed if there is external acceleration. Note that zk,1 is
the accelerometer output and C1,k x̂k,g is the estimated ac-
celerometer output using only gyroscope outputs. The dif-
ference between these values should be small when there
is no external acceleration. When there is external acceler-
ation, zk,1 −C1,k x̂k,g is proportional to external acceleration.
Thus the role of Eq. (9) is that accelerometer output in the
direction of external acceleration is not trusted. The level
of trust is reflected in r1,k and r2,k . Note that in Eqs. (9)
and (10), a low pass filter is used so that r1,k and r2,k are
not changed abruptly. Also note that r1,k and r2,k are al- Fig. 2 Experiment 1: f (a x ,ay ,az ) plot when there is neither
ways greater than r1,nom and r2,nom since too small r1 and attitude change nor external acceleration
r2 values may cause the Kalman filter divergence problem.
In the measurement update 2, acceleration measurements sampling period T = 0.05 sec).
only are used to estimate state. To determine r1 , r2 , and r3 , sample variance of a x ,
We note that if the same Ck is used and Rk is con- ay , w x , wy , wz are computed: 0.000 016 8, 0.000 016 7,
stant (i.e., the adaptive algorithm is not used), the standard 0.003 2, 0.004 0, 0.003 1. Note that not the absolute value
Kalman filter and the two step Kalman filter Eqs. (8) and but the ratio between (r1 , r2 ) and r3 is important. The ratio
(11) are identical. In the two step extended Kalman filter, of accelerometer sample variances to gyroscope sample
the standard extended Kalman filter is divided into two variances is about 200. Since the accelerometers are more
step to estimate and compensate external acceleration. sensitive to disturbances, we decrease ratio to 20 and (r1 ,
5. Experiments r2 ) and r3 are chosen as follows:

To verify the proposed filter, the sensor system con- r1,nom = r2,nom = 0.05, r3 = 1
sisting of 3 accelerometers and 3 gyroscopes is made and where r1,nom and r2,nom are r1 and r2 values for the standard
experiments are performed. In the first experiment, sensor Kalman filter and also used in Eqs. (9) and (10).
outputs are measured while the sensor system is still: i.e., The value f (a x ,ay ,az ) in Eq. (7) is given in Fig. 2.
there is neither attitude change nor external acceleration. Note that δ in Eq. (7) is a parameter, which determines
This experiment is to test how good the sensors are. Ide- existence of external acceleration. If δ is too small, exter-
ally, all sensor outputs should be zero. The accelerometer nal acceleration is falsely detected even though there is no
outputs (a x , ay , az ) and gyroscope outputs (g x , gy , gz ) are external acceleration. If δ is too large, external accelera-
given in Fig. 1. All outputs are sampled at 20 Hz (i.e., the tion is not detected even though there is external accelera-

JSME International Journal Series C, Vol. 49, No. 1, 2006

176

Fig. 3 Experiment 1: attitude estimation by the proposed filter

Fig. 4 Experiment 2: a cart and rails for external acceleration generation

tion. From Fig. 2, δ is set to be 0.005. The parameters for conversion errors.
the two step extended Kalman filter are as follows: In the second experiment, the sensor system is at-
q1 = 1, α1 = 0.4, α2 = 10. tached on the cart, which can move along the rails (Fig. 4).
By moving the cart with hands, external acceleration is ap-
These parameters are rather heuristic choices based on plied to the sensor system. The sensor system is located
trial-and-error. As performance indices of estimation, we so that there is only yb axis direction external acceleration.
use average absolute errors and maximum errors as fol- Attitude is estimated using 3 different methods: the
lows: standard extended Kalman filter in section 3, the simple
1 N
Sθ
(θ̂(kT ) − θ(kT ))2 , adaptive extended Kalman filter, and the proposed two
N k=1 step extended Kalman filter in section 4. In the simple
1 N adaptive extended Kalman filter, r1 and r2 are set to large
Sφ
(φ̂(kT ) − φ(kT ))2 values (5 in this experiment) when Eq. (7) is satisfied.
N k=1
  The simple adaptive filter have a similar method to that
Mθ
max θ̂(kT ) − θ(kT ) ,
k in Ref. (8), where gyroscope outputs only are integrated
 
Mφ
max φ̂(kT ) − φ(kT ) . (12) when there is external acceleration. Note that attitude is
k not changed in the second experiment; that is, true attitude
Attitude estimation by the proposed filter is given in values are θ = 0 and φ = 0.
Fig. 3. Since there is no external acceleration, the result is Attitude estimation by 3 different filters is given in
almost the same as the standard Kalman filter. Fig. 5. The three filters show similar performance in pitch
Since the sensor system is still, the true attitude values estimation. Recall that there is no external acceleration in
are θ = 0 and φ = 0. We can see the maximum estimation the direction of xb ; thus pitch estimation is not affected
errors for θ̂ and φ̂ are 0.733 and 0.909 degree, respectively. by external acceleration in the second experiment. On the
These errors may be attributed to sensor noises, misalign- other hand, the three filters show different performance in
ment (sensors are not located exactly orthogonal) and A/D

Series C, Vol. 49, No. 1, 2006 JSME International Journal

 177

Fig. 5 Experiment 2: attitude estimation by 3 different filters

roll estimation. In the standard Kalman filter, external ac- simple adaptive filter (compare S φ and Mφ ).
celeration is not effectively rejected, which results in large The r1 and r2 variations in the simple adaptive ex-
estimation error. In the simple adaptive and the two step tended Kalman filter and the proposed two step extended
filter, effects of external acceleration are reduced and the Kalman filter are given in Fig. 6. In the two step filter, r2,k
performance of the two step filter is better than that of the becomes significantly larger that r1,k , which reflects that

JSME International Journal Series C, Vol. 49, No. 1, 2006

178

Fig. 6 Experiment 2: (r1,k , r2,k ) variations

there is external acceleration in the direction of yb but not merce, Industry and Energy and Ulsan Metropolitan City
xb . In the ideal situation, r1,k should stay at its nominal which partly supported this research through the Network-
value; it can be conjectured that vibration (from the cart based Automation Research Center (NARC) at University
and the rails) and misalignments increase noise level of a x of Ulsan
output.
References
6. Conclusion
(1) Lawrence, A., Modern Inertial Technology, (1998),
In this paper, we have proposed the two step extended Springer Verlag.
Kalman filter for general purpose attitude estimation. The (2) Barbour, N. and Schmidt, G., Inertial Sensor Technol-
main contribution is external acceleration, which is the ogy Trends, IEEE Sensors Journal, Vol.1, No.4 (2001),
pp.332–339.
main source of estimation error, is estimated and compen-
(3) Vaganay, J., Aldon, M.J. and Fournier, A., Mo-
sated. To verify the proposed algorithm, the sensor system bile Robot Attitude Estimation by Fusion of Inertial
consisting of 3 axis accelerometers and 3 axis gyroscopes Data, Proceedings of IEEE International Conference on
is constructed and tested while intentional external accel- Robotics and Automation, (1993), pp.277–282.
eration is generated. The experiment results show that the (4) Barshan, B. and Durrant-Whyte, H.F., Evaluation of a
direction of external acceleration is detected and the esti- Solid-State Gyroscope for Robotics Application, IEEE
mation performance is less disturbed by external acceler- Trans. on Instrumentation and Measurement, Vol.44,
ation. No.1 (1994), pp.61–67.
(5) Barshan, B. and Durrant-Whyte, H.F., Inertial Navi-
Acknowledgement gation System for Mobile Robots, IEEE Trans. Robot.
Automat., Vol.11, No.3 (1995), pp.328–342.
The authors would like to thank Ministry of Com-

Series C, Vol. 49, No. 1, 2006 JSME International Journal

 179

(6) Rehbinder, H. and Hu, X., Nonlinear Pitch and Roll mation for Accelerated Rigid Bodies, Proceedings of
Estimation for Walking Robots, Proceedings of the the 2001 IEEE International Conference on Robotics
2000 IEEE International Conference on Robotics & & Automation, (2001), pp.4244–4249.
Automation, (2000), pp.2617–2622. (9) Nelson, R.C., Flight Stability and Automatic Control,
(7) Foxlin, E., Inertial Head-Tracker Sensor Fusion by a (1998), McGraw-Hill.
Complementary Separate-Bias Kalman Filter, Proceed- (10) Brown, R.G. and Hwang, P.Y.C., Introduction to Ran-
ings of the IEEE 1996 Virtual Reality Annual Interna- dom Signals and Applied Kalman Filtering, (1997),
tional Symposium, (1996), pp.185–194. John Wiley & Sons.
(8) Rehbinder, H. and Hu, X., Drift-Free Attitude Esti-

JSME International Journal Series C, Vol. 49, No. 1, 2006