A+linear+fusion+algorithm+for+attitude+determination

1、Measurement 40 (2007) 322328A linear fusion algorithm for attitude determination using low cost MEMS-based sensorsRong Zhu a,*, Dong Sun b, Zhaoying Zhou a, Dingqu Wang aa State Key Laboratory of Precision Measurement Technology and Instruments, Department of Precision Instruments and Mechanology, T

2、singhua University, Beijing 100084, Chinab Department of Manufacturing Engineering and Engineering Management, City University of Hong Kong, Hong Kong, ChinaReceived 29 March 2006; accepted 25 May 2006Available online 16 June 2006AbstractThis paper presents a novel sensing methodology with an extend

3、ed Kalman-based fusion algorithm for attitude estima- tion, using inexpensive micromachined gyroscopes, accelerometers and magnetometers. Unlike conventional methodology using quaternions and Euler angles, in the proposed fusion algorithm the state vector is dened to be a 6 1 vector con- taining sen

4、sing components of earth gravity and magnetic eld in the body frame. By this way, the Kalman model can be represented by linear equations, which makes the iterative computations easy to be implemented at a faster rate using inex- pensive microprocessors. The computation of the lter is further simpli

5、ed by updating gravity and magnetic vectors respectively in smaller dimension. Experiments are performed to validate the eectiveness of the proposed approach. 2006 Elsevier Ltd. All rights reserved.Keywords: Attitude determination; Kalman-based fusion algorithm; Electro-mechanical system (MEMS)1. In

6、troductionMany applications such as micro air vehicles (MAVs) 1 and human pose tracking 2 etc., demand for small, light, ambulatory and inexpen- sive attitude measurement system. The fundamental requirements for such measurement system are lightweight, small-size, low power consumption, and easy imp

7、lementation. With the recent prolifera- tion of low-cost micro inertial sensors based on elec- tro-mechanical system (MEMS) technology, it has* Corresponding author. Tel.: +86 10 62793166; fax: +86 1062771478.E-mail address: zrwzwyj (R. Zhu).become viable to construct inexpensive and small- attitude

8、 measurement system. The attitudes of MEMS-based inertial sensors are mainly deter- mined by three methods at the time being. The rst method is to integrate the instantaneous angular velocity measured by micromachined rate gyro- scopes 3. Successful implementation of this method requires the use of

9、rate gyros with excep- tionally accurate and stable outputs. The perfor- mances of inexpensive micro inertial sensors, however, are generally not satisfactory 4. The second method is to utilize micromachined acceler- ometers and Hall eect magnetometers 5. Pitch and roll angles are estimated by using

10、 the gravity components deduced from the measurement of0263-2241/$ - see front matter 2006 Elsevier Ltd. All rights reserved. doi:10.1016/j.measurement.2006.05.020accelerometers, and the heading angle is estimated from the measurement of magnetometers. This solu- tion is simple but is not ideal for

11、the dynamic motion since non-gravitational acceleration aects the gravity measurement.Considerable amount of eorts have been directed towards the third method the attitude estimation with redundant and fusion technologies using low- cost sensor systems 611. It was discussed in 8 to use a triad of ra

12、te gyros fused with an ultra-short baseline dierential GPS attitude determination sys- tem. The system presented in 9 used magnetome- ters, accelerometers, and GPS as the primary sensors for a gyro-free attitude determination sys- tem. Deducing gravity components from two acceler- ometers, a gyrosco

13、pes-based methodology was used in 10 to fuse tilt using a nonlinear Kalman model. An extended Kalman lter with a nonlinear attitude quaternion error measurement model was also uti- lized in industry, with help of gyroscopes, accelerom- eters, and magnetometers. For addressing the nonlinear Kalman mo

14、del problem, a linearized mea- surement equation by using an additional Gausssensing components of the earth gravity and mag- netic eld in the body frame, which leads to a linear (and simple) Kalman model to fuse the attitudes. The iterative lter computation is further simplied by updating gravity a

15、nd magnetic vectors in 3 3 dimensions, respectively. As a result, the computation of the proposed lter is greatly simplied and can be executed at a faster rate using inexpensive micropro- cessors. Experiments are performed on a home-made prototype, and the results validate the eectiveness of the pro

16、posed algorithm and sensing system.2. Design of state variables for Kalman lterThe proposed attitude measurement system is comprised of 3-axis micromachined gyroscopes, accelerometers and magnetometers, which are mounted orthogonally as shown in Fig. 1.To determine the attitude by redundant data fus

17、ion, it is a requirement that several vectors mustbe known or measured in the respective coordinateframes. The vectors must not be zero and collinear. In this paper, three vectors are selected, denoted by*Newton iterated process was proposed in 11.In this paper, a sensor conguration that consists of

18、 3-axis MEMS-based gyroscope, accelerometer and magnetometer, is used to determine the attitude with a novel Kalman fusion algorithm. Unlike com- monly used quaternion parameters or Euler anglesx for angular velocity, m for earth magnetic eld,*and g for gravity respectively.Consider a body frame tha

19、t is denoted by super- script b and xed to the vehicles body, and the earth frame that is denoted by superscript n and attached to the locally level plane. It is obvious that*for gyro-based systems, in our method, the Kalmanstate vector is dened to be a 6 1 vector containingthe vectors m and g can b

20、e transformed between thebody frame b and the earth frame n, i.e.ZMagnetometerAccelerometerGyroscopeGyroscopeGyroscopeAccelerometerAccelerometerMagnetometerMagnetometerXYFig. 1. Conguration of the attitude measurement system.R. Zhu et al. / Measurement 40 (2007) 322328329*bb *nb#m Cn m ;1*_S b xnb0*

21、b S6*bb *n0xbg Cn g ;2nbnwhere Cbdenotes the Orientation Cosine Matrix3. Implementation of linear Kalman lterrepresenting the rotation from the earth frame tothe body frame, expressed as a function of the attitudeThe output vector of the 3-axis accelerometer a is normally expressed as follows:2cos w

22、 cos hsin w cos hsin h 3Cbn 4 cos w sin h sin c sin w cos c sin w sin h sin c cos w cos ccos h sin c 5;3cos w sin h cos c sin w sin csin w sin cos c cos w sin ccos h cos cwhere c, h and w denote roll, pitch and heading angle, respectively. Combining (1) and (2) yields*abbb g al*b e ;7*b*bC* bS b no

23、# * S n;4where gb, al and edenote the gravity acceleration,noCb*. * .where S *m is a 6 1 vector containing sensingg*compon*ents of the local magnetic eld and gravity. S b and S n represent the sensing components in the body-frame and earth-frame, respectively.nDierentiate Cb with respect to time yie

24、lds 12the linear acceleration and the measurement noise inthe body frame. The linear acceleration can be esti- mated by such sensors as air speed sensors, or odometry, which is then subtracted from the output of accelerometers. The sensor vectors (i.e. the mea- surements of earth magnetic eld and gr

25、avity by using magnetometers*and accelerometers) are re-garded as the output Z of the Kalman lter, so thatC_ b2 0xzxy 3bbbthe measurement equation becomes a linear andsimple identity function.n 4xz0xx 5 Cn xnb Cn;5xyxx0The state and measurement equations in discrete form are formulated as2 xx 3*. *b

26、 .b*where 4 xy 5 x . Dene S bm *bas the state*Skb Uk=k1* S b;8xzgk1variables. Dierentiati*ng (4) and utilizing (5), the*dynamics of the state S b is obtained asZk S bkk R ;9Fig. 2. Kalman lter update algorithm.*where Rk is the measurement noise, Uk/k1 is thetransfer matrix of the state dynamics and

27、is approx- imated byx0b#nbUk=k1 I66 Tnb0xbx0T 2b#2nb4. Attitude calculation*After estimating the state vector S b by theproposed Kalman iterative algorithm, the corre- sponding attitude can be computed by the following arc-tangent solution 13:c arctgg =g 110xb 2!yz nbx T2b 2 2bnbh arctggx cos c=gz12

28、3.mz sin c my cos c.I33 xnb T 26=407bxb 2 T 2 5w arctgmx cos h my sin c mz cos c sin hnb0I33 xnb T 2132 U0k=k100U 40k=k135;10whereb*m .Tgggxyz*b g , andT mxmymz where T is the sampling period.gFig. 2 illustrates the Kalman iterative algorithm for updating the state vector. It can be seen that the Ka

29、lman iterative process can be divided into two loops: lter update loop and gain update loop. For reducing the computational load, the state vec-5. Experiments and resultsExperiments were performed to verify the pro- posed approach. The system block diagram of the experimental setup is shown in Fig.

30、3. Fig. 4mtor *band the state vector *bare updated separatelyillustrates a homemade prototype. The prototypeusing the same small-dimension transition matrix*is comprised of two ADXL202 accelerometersU0bk=k1, since the updating computations of m*band(two-axis accelerometer with 2 g measurementpg are

31、non-coupled as shown in Eqs. (8) and (10).Note that in the gain update loop, the probabilityrange and about 200 lg Hz noise density), a pairof Honeywells HMC1001/1002 magnetometerskkmatrix P jand the Kalman gain Kjare calculatedk=k 1using U0in 3 3 dimensions rather than Uk/k1in 6 6 dimensions, where

32、 the superscript j is m*bwhen updating the magnetic vector m*band is g whenupdating the gravity vectors g . In the lter updatekloop, P jand Kjk*are utilized for estimation of thestate vector S b.To extend the damping time lag, a positive for- getting factor k (1) is induced in the gain updating proc

33、ess. A smaller k results in more history record abandoned, and subsequently a lower lag. However, there is a tradeo in tuning k, i.e. a smaller k corre- sponds to lower lag but increases the sensitivity to the dynamic acceleration.Fig. 4. Homemade prototype for determining attitude.Rate GyroMagnetom

34、eterInitializationmComputerKalman FilterAttitude CalculationPitch RollHeading angleAccelerometerFig. 3. System block diagram of the experimental setup.Fig. 5. Attitudes by three algorithms.Fig. 6. Pitch estimation and time lag with dierent forgetting factors.(single-axis/two-axis magnetic sensors wi

35、th 2 Gauss measurement range and about 4 l Gauss noise level), and three Muratas ENV-05 F-03 rate gyroscopes (single-axis gyroscope with 60 deg/s measurement range and about 0.004 deg/s noise level).In experiments, the hardware prototype was manipulated to move back and forth in a level plane, with

36、up-and-down moving acceleration. Both the pitch and roll kept constant (zero) during the motion. The heading angle suers from a bit uctu- ation around a constant value due to the manipula- tion error. The proposed Kalman algorithm was used to estimate the attitude with 20 Hz updating rate. For compa

37、rison, the quaternion integral algo- rithm 3 using rate gyros and the direct arc-tangent attitude solution 13 using measurements of magne- tometer and accelerometer, was also employed respectively. In the quaternion algorithm, the atti- tude was solved by the fourth-order RungeKutta integration with

38、 20 Hz updating rate.Fig. 5 illustrated the comparison results of atti- tude measurements using the three algorithms. Notethat the reference values of the pitch and the roll (dashed line) during the measurement were zero. It can be seen that the arc-tangent solution based on the gravity and the eart

39、h magnetic eld (dotted line) was very sensitive to the shaking acceleration and obvious errors occurred in estimations of the atti- tudes. Attitudes obtained by the quaternion algo- rithm (dashdotted line) had increasing drift as time increased. In contrast, the result of the pro- posed linear Kalma

40、n algorithm with forgetting fac- tor of 1.0 (solid line) exhibited better performance. The standard deviations from the reference values (i.e. zero) were approximately 0.5 for pitch and roll. The standard deviation of heading angle was about 2. The uctuation of the heading angle was mostly due to th

41、e manipulation error.We further examined the weighting eect of the forgetting factor k to the measurement, taking pitch estimation as the example. In the experiment, the prototype was xed on a single-axis rotating table, and rotated back and forth about the level axis. The reference angle and the ca

42、lculated angle using the proposed Kalman algorithm were compared inFig. 7. Pitch estimation and standard deviation with dierent forgetting factors.Fig. 6. The dynamic error was approximately 1 for pitch measurement. It can be seen that the time lag of the proposed Kalman solution falls as k decrease

43、s. On the other hand, as k decreases, the proposed Kalman solution becomes more sensitive to the moving acceleration (follow the same proce- dure as the rst experiment using dierent k), which implies growth of the standard deviation as seen in Fig. 7. Therefore, the forgetting factor k must be caref

44、ully chosen with consideration of both the time lag and the moving acceleration.6. ConclusionsA novel Kalman-based fusion algorithm is pro- posed to estimate attitudes for a small attitude mea- surement system using inexpensive micromachined gyroscopes, accelerometers and magnetometers. A novel stat

45、e vector that contains three orthogonal components of the magnetic eld and the gravity is used in the proposed algorithm, instead of tradi- tionally using quaternions and Euler angles, so that a linear and simple Kalman model is developed. Compared to the existing Kalman lters, the com- putation of

46、the proposed lter is simplied signi- cantly due to the use of a linear model and performing the update computation in a smaller dimension. The algorithm can be executed at fast rate using a small inexpensive commercial micro- processor. The eect of the forgetting factor to the time lag is also inves

47、tigated under the proposed algorithm. Experiments were conducted on a home- made prototype to demonstrate the eectiveness of the proposed approach.References1 Y. Oshman, M. Isakow, Mini-UAV attitude estimation using an inertially stabilized payload, IEEE Transactions on Aerospace and Electronic Syst

48、ems 35 (1999) 11911203.2 E.R. Bachmann, I. Duman, U. Usta, R. McGhee, X. Yun,M. Zyda, Orientation tracking for Humans and RobotsUsing Inertial Sensors, in: International Symposium on Computational Intelligence in Robotics and Automation (CIRA 99), Monterey, 1999, pp. 187194.3 P.G. Savage, Strapdown

49、inertial navigation integrationalgorithm design. Part 2: Velocity and position algorithms, Journal of Guidance, Control, and Dynamics 21 (2) (1998)208221.4 V.K. Varadan, et al., High sensitive and wide dynamic range navigation microsystem on a single chip, in: Proceedings of SPIE The International Society for Optical Engineering, Melbourne, Australia, 2000, pp. 134140.5 M.J. Caruso, Applications of magnetic sensors for low cost compass systems, in: IEEE Position Location and Naviga- tion Sy