AbstractThe attitude estimation of the rigid bodies using MEMS inertial sensors is presented. The bias of gyros and accelerometers are tracked by a state estimation algorithm in real-time. The algorithm uses characteristics of the sensor noise to automatically recognize motionless periods and update the sensors bias level without any dependency on application specific parameters, frequency separation between the signal of interest and the sensor noise, or a high-level system model. Then the attitude estimation algorithm that fuses data from rate gyros and accelerometers is proposed. Based on the kinematics of the body and the Newtons force law, the modified Rodrigues parameter is represented in place of quaternion. We describe rotation without encountering singularity between the modified Rodrigues parameters and their shadow parameters. And the attitude is estimated by Extended Kalman filter under low acceleration, meanwhile the situation of high acceleration is considered. Finally, the proposed estimation algorithm is tested, the simulation results are provided to show the effectiveness of the proposed algorithm.

Keywords- attitude estimation; MEMS inertial sensors; bias;

Extended Kalman filter

I. INTRODUCTION Attitude estimation is often a prerequisite for controlling

aerospace and underwater vehicles, mobile robots, and other mechanical systems moving in space. Hence, attitude estimation of a rigid body has applications in spacecraft and aircraft dynamics, unmanned vehicle dynamics, and robot dynamics, including walking robots [1-3]. In this paper we will provide a solution to fusing data from a 3-axis rate gyro and a 3-axis accelerometer that will provide stable estimates of the robots attitude. The paper first presents a method to estimate bias level from inertial measurements. The technique is similar in sprit to tracking bias used in [4]. Then the attitude problem is formulated mathematically and the proposed algorithm is described and analyzed in Section II. The modified Rodrigues parameter is adopted to represent the rotation matrix, and the Extended Kalman filter is employed to attitude estimation. We also consider the situation of high acceleration about attitude determination. In section III we describe some illustrative simulations. The paper is concluded in Section V with a discussion and summary.

II. BIAS ESTIMATION

A. Equipment and sensor noise characterization The measuring head contains four sensors: one triple-axis

MMA7260QT accelerometer (full range-scale 1.5 g) and

three single-axis ADXRS300 gyroscopes (full range-scale 300/s). All of them are single chip sensors. Electronic circuits of MMA7260QT and ADXRS300 sensors contain capacitors at every signal output fulfilling the role of low pass filter.

First, static measurements were taken in order to accurately characterize the overall noise of the measurement system. In the experiments, the sensors were powered up to a steady-state operating temperature and avoid bias drift due to transient thermal effects. Representative data from static measurements of the accelerometers and gyroscopes horizontal axis are shown in Fig.1 ~ Fig.2. From the static measurements, the following overall noise characteristics were identified for the measurement system employed: peak-to-peak amplitude of accelerometer and gyroscope is respectively 31.9 mV and 15.6 mV, and output signals are containing white Gaussian noise as their distribution is normal distribution. The values of peak were found to be reasonably consistent throughout the experiments. They are also in very good agreement with the values observed for the vertical axis for which the noise was found to have the same dominant frequency.

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.51.73

1.74

1.75

1.76

1.77

1.78

1.79

1.8

t(s)

Outp

ut volta

ge(V

)

RawMaxMin

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.52.51

2.515

2.52

2.525

2.53

2.535

2.54

t(s)

Outp

ut volta

ge(V

)

RawMaxMin

Fig.1 Measurement noise during a static test of the accelerometer and

gyroscope

1.745 1.75 1.755 1.76 1.765 1.77 1.775 1.780

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

Signal Level(V)

Pro

babi

lity

Density

2.515 2.52 2.525 2.53 2.5350

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

0.18

Signal Level(V)

Pro

babi

lity

Density

Fig.2 Experimentally accelerometer (left) and gyroscope (right) measured

noise distribution compared to the normal distribution derived from its mean and standard deviation

And the assumption was made that the noise characteristics peak observed in the static tests would remain constant throughout the entire measurement range of the sensor under

Attitude estimation of rigid bodies using MEMS inertial sensors

Bin Fang, Wusheng Chou, Li Ding Robotics Institute of Beihang University, Beijing, 100191China

fangbin1120@163.com

mVV

70.476.1

mVV

20.252.2

2011 Fourth International Conference on Intelligent Computation Technology and Automation

978-0-7695-4353-6/11 $26.00 2011 IEEEDOI 10.1109/ICICTA.2011.157

618

2011 Fourth International Conference on Intelligent Computation Technology and Automation

978-0-7695-4353-6/11 $26.00 2011 IEEEDOI 10.1109/ICICTA.2011.157

592

both static and dynamic conditions. Any signal that falls entirely inside the peak-to-peak noise amplitude and whose mean changes at a rate equal to or below the maximum drift rate is indistinguishable from the inherent noise in the sensor output. For convenience, this range of signals will be referred to as the sensors noise band.

B. Bias level estimated and correction in the output Low-pass filter is used in the [4] to estimate the bias.

According to the sensors character above, Kalman filter (KF) is more suitable, which eliminates random noises and errors using the knowledge about the state-space representation of system and uncertainties in the process: the measurement noise and the process noise [5]. When the signal in the band, the rigid bodys acceleration equals zero and angular rate is constant, so their state-space representations are given by as follows [6]:

)(1001

)(0010

)( twtXtX aaa

)()(10)( tvtXtZ aaa T)( aVtX a (1)

)(1001

)(0010

)( twtXtX ggg

)()(10)( tvtXtZ ggg T)( tX g (2)

Where V the linear rate, a the acceleration, the angular rate, the angular acceleration.

To get a high convergent speed, the initial value of voltage is the first number in the band. The algorithm developed to realize the state estimation objectives is shown in the form of a flowchart in Fig.3. The beginning of the interval is during a motionless period where the algorithm has identified the signal as being entirely within the noise band and applied the Kalman filter to recover the nominal signal level. This level is within the range to which the bias could have drifted since the last update, so it is updated during this period. Meanwhile the noise band is updated.

Is the signal entirelywithin the noise band?

The signal estimate isobtained by applying a

Kalman filter to recover thenominal level

YES

Could the bias havedrifted this far since the

last update?

Updata the bias by setting itequal to the nominal signal level

YES

Leave the bias unchanged

NO

NO

Check the change in signallevel over the last N noise

periods

Updatethe

noiseband

Fig.3 Flowchart of bias estimation algorithm. The processing steps shown are

repeated for each incoming data sample.

The state estimators function of recognizing periods where the gyroscopes signal is entirely within the noise band is

illustrated in Fig.4. When the signal is recognized as having entered the noise band, it is marked as being settled. After remaining settled for the designated number of noise periods, bias level adjustment begins if the signal is within the possible range of drift since the prior update. And the noise band is updated for the next judgment. The angular velocity is attained effectively after the filtered, showed in the Fig.5. The results of applying the state estimator to the other raw accelerometer and gyroscope voltage output are also available.

5 6 7 8 9 10 112.1

2.2

2.3

2.4

2.5

2.6

2.7

2.8

2.9

3

t(s)

Out

put V

olta

ge(v)

RawBiasSettledBias Adjust

Fig.4 Noise band recognition and bias adjustment for the raw gyroscope

output

5 6 7 8 9 10 11

-80

-60

-40

-20

0

20

40

60

80

100

t(s)

Angu

lar

vel

ocity

(/s

)

FilteredSettledBias Adjust

Fig.5 Angular velocity output after filtered

III. ATTITUDE DETERMINATION PROBLEM The data from MEMS inertial sensors are tracked bias by

the state estimator above, and it is the basic for the attitude estimation in this part. The rotation matrix may be represented by various sets of coordinates, like the Euler angles, Quaternions, or modified Rodrigues parameters [7]. In this study we will work with modified Rodrigues parameters for its simple and efficient in the problem of attitude estimation.

A. Mathematical modeling The attitude described by quaternion has the shortage of

redundancy, the modified Rodrigues parameter (MRP), which is three-dimensional independent vector. The definition of the modified Rodrigues parameter is as follows

4tanep (3)

Where e is a unit vector,

is the rotation angle. But the modified Rodrigues parameters could not represent

all the attitudes because of their singularity as 360 . To

619593

avoid it, it is reasonable to switch the modified Rodrigues parameters to the shadow parameters. The shadow parameters are

4cotep s (4)

The shadow parameters will occur singular as 0 . We

can using p to represent to the attitude when 1p , using sp to represent the attitude when 1p . Thus, the

singularities can be avoided by switching between p sp . The direction cosine matrix in terms of the modified

Rodrigues parameters can be described by

22222

2

1

8

1

14

pp

pp

pIC p (5)

Where

00

0

12

13

23

pppp

ppp

Then we assume the linear acceleration 0v , so the kinematic equation in terms of MRP is

pppIpp T2 22141

(6)

ACa TpB (7) Where is the angular velocity of the body frame B with

respect to the inertial frame I , expressed in the body frame B ; Ba is the so-called specific acceleration representing the sum of all non- gravitational forces applied to the body divided by its mass, expressed in the body frame B ;

3eA g is the gravitational acceleration expressed in the inertial frame I , with 1;0;03 e .

B. Attitude estimation using EKF We will estimate attitude by multiplicative attitude error [8]

as follows. The attitude error p is defined as the rotation from the

estimated attitude p to the true attitude p , that is

pppp

ppppppppp

21

211

T22

22

(8)

The high orders are neglected, and the following equation can be derived:

141

41 w ppp (9)

Now the state error equation can be written in a matrix-vector form:

wpp F (10) Where F , 141 ww .

The statistic property of the process noise w is given by

0wE , QwwE T . By using Eq. (8) and Eq. (7) and neglecting the higher order,

the following equation can be derived: vp HZ (11)

Where ApC T)(4H , ApCaZ T)( B . The statistic property of the measurement noise v is given

by 0vE , RvvE T . By using EKF to estimate the attitude, the program of

attitude estimation at the step kt ~ 1kt can be summarized as follows:

(1) Propagation Starting with the initial conditions kp and kP . kk /1 p can

be obtained by integrated at the internal 1kk tt .

kkk

kkk pp

4tan /1

!

"

"

"

#

#

(12)

Where tik "" # . And the prediction equation of the covariance matrix is

given by

kkkkkk QPP T

11/1 (13) Where tkk " 11 FI . (2) Update The gain matrix is

1T1/11

T1/11

kkkkkkkkk RHPHHPK (14) The state error estimation is

111 kkk ZKp (15) The state estimation is

1/11 kkkk ppp (16) The covariance matrix is

T111

T111/111 kkkkkkkkkk KRKHKIPHKIP

(17) This algorithm is termed multiplicative error attitude

estimator (MEAE).The equation to transform the shadow parameters of modified Rodrigues parameters sp to the direction cosine matrix and the kinematic equations in terms of sp are exactly the same as Eq. (6) ~ (7).Therefore, p in

MEAE can be replaced with sp .

C. Attitude estimation under high acceleration We have considered the idealized case that the

accelerations are so low above, but when the acceleration is so high that the accelerometer must be considered completely unreliable as a gravity sensor, we are forced to rely on the rate gyros. Now the pitch and roll estimation under high accelerations is as follow

620594

1/1 kkk pp (18) Meanwhile, we should set the rule, which must be able to

detect acceleration or rather. So the rule can be expressed as follow:

1AaB (19) Where, was chosen as three standard deviations of the

accelerometer noise.

IV. SIMULATIONS AND EXPERIMENTS We will demonstrate the filter algorithms using numerical

simulations. Continuous time motion and sensor data were generated and the sensor data were sampled with a sampling frequency of 100 Hz after which white noise was added. The parameter values are summarized in Table 1.

Gyro noise Accelerometer noise

Ts

0.01(std) 0.1(std) 0.01 Table.1 Parameter values used in the simulations

The angular velocity is set according to the (20), so the pitch and roll can be attained by integrated. They are shown in Fig.6.

$

%

&

100sin2.050sin2.0

'

'

ik

AngVec kiik () ,1000,0, (20)

Meanwhile, we calculate the running time for attitude computing by quaternion and modified Rodrigues parameters. The time, cost by modified Rodrigues parameters, is 0.0938s. The quaternions is 0.1094s. It is apparent from the comparing that the MRP is more efficiency, which is important to the embedded system. Then the performance of EKF base on MRP is tested. The results of a 10 s simulations are shown in Figs. 7. The solid line the true attitude, the dash line is the filtered results. The errors are showed after two seconds because the estimated attitude converges to its true values eventually from the initial values.

It can be seen that the estimated attitude could well track its true values. And the attitude estimator described in the paper achieves the success in the embedded system.

0 1 2 3 4 5 6 7 8 9 10-0.2

-0.1

0

0.1

0.2

t(s)

Angu

lar

vel

ocity

(rad/

s)

Angular xAngular y

0 1 2 3 4 5 6 7 8 9 10-0.05

0

0.05

0.1

0.15

t(s)

Angl

e(rad

)

pitchroll

Fig.6 Angular velocity (top) x (dashed line) and y (solid line).Pitch (dashed

line) and roll (solid line) angles (bottom).

0 1 2 3 4 5 6 7 8 9 10-0.5

0

0.5

1

1.5

t(s)

pitc

h(ra

d)

EstimatedTrue

0 1 2 3 4 5 6 7 8 9 10-1

0

1

2

3

t(s)

roll(r

ad)

EstimatedTrue

2 3 4 5 6 7 8 9 10-1

-0.5

0

0.5

t(s)

pitc

h(rad

)

2 3 4 5 6 7 8 9 10-1.5

-1

-0.5

0

0.5

t(s)

roll(r

ad)

Fig.7 Pitch and roll angles (left). Pitch and roll errors (right).Solid lines

denote true values and dashed estimated.

V. CONCULUSION In this paper we have presented a state estimator to track the

inertial sensors bias, which increases the accuracy and usefulness of the estimate obtained through MEMS inertial sensors, and the attitude estimation problem for a rigid body is solved by an integrated MRP-based Extended Kalman filter. We reduce the attitude determining computational efforts in embedded system, whose characteristics are small size, low cost, and good reliability. The algorithms are feasible to determine the rigid bodys attitude by MEMS inertial sensors. And they can be applied to the robots such as walking robot or micro quad-rotor UAV, which need balance control.

REFERENCES [1] Wang Lidai, Xiong Shenshu, Zhou Zhaoyinget al. Constrained

filtering method for MAV attitude determination[C]. Proceedings of the IEEE Instrumentation and Measurement Technology Conference. OttawaCanada2005:1480-1483.

[2] Su K, Ren D H , You Z , Zhou Q. Application of MIMU/ Magnetometer Integrated System on the Attitude Determination of Micro Satellite [C]. Proceedings of the 2004 international Conference on intelligent Mechatronics and Automation. Chengdu ,China 2004. 8.

[3] David Wooden, Matthew Malchano, Kevin Blankespoor etc. Autonomous navigation for BigDog [C]. 2010 IEEE International Conference on Robotics and Automation Anchorage Convention District May 3-8, 2010, Anchorage, Alaska, USA, 4736-4741.

[4] Eric Allen Johnson, Stacy J. Morris Bamberg, and Mark A. Minor. A State Estimator for Rejecting Noise and Tracking Bias in Inertial Sensors[C]. 2008 IEEE International Conference on Robotics and Automation Pasadena, CA, USA, May 19-23, 2008, 3256-3263.

[5] P. Zarchan, H. Musoff, Fundamentals of Kalman Filtering: A Practical Approach[C]. Progress in Astronautics and Aeronautics, AIAA, Reston, 2005.

[6] Cezary Kownacki .Optimization approach to adapt Kalman filters for the real-time application of accelerometer and gyroscope signals filtering [J]. Digital Signal Processing.2010 .09.001, 1-11.

[7] Markley F L. Attitude representations for Kalman filtering[C]. Paper No. AAS 01 - 309 of the AAS&AIAA Astrodynamics Conference, Quebec, CA, 2001: 133- 151.

[8] Chen Jizheng, Yuan Jianping, Fang Qun. Flight vehicle attitude determination using the modified rodrigues parameters[J]. Chinese journal of aeronautics, 21, 2008, 433-440.

621595