A Novel Attitude Measurement Algorithm in Magnetic Interference Environment

Transcription

1 Sensors & Transducers, Vol. 75, Issue 7, Jul 4, pp. 6- Sensors & Transducers 4 IFSA Pulishing, S. L. A Novel Attitude Measurement Algorithm in Magnetic Interference Environment Lingxia Li, * Lili Shi, Yu Liu, Zengshan Tian, Mu Zhou Institute of Communication and Information Engineering, Chongqing Universit of Posts and Telecommunications, Chongqing 465, China Institute of Optical Communication Technolog, Chongqing Universit of Posts and Telecommunications, Chongqing 465, China * Tel.: , fax: * slili@sina.com Received: 3 April 4 /Accepted: 3 Ma 4 /Pulished: 3 Jul 4 Astract: The approach of using Magnetic Angular Rate Gravit (MARG) sensor for the current multi-sensor ased pedestrian navigation algorithm magnetometers is susceptile to the external magnetic interference. The result of attitude is affected man factors, lie the low-precision MEMS gro drift and large od linear acceleration measurements. In this paper, we propose anti-jamming algorithm which is ased on four elements of Extended Kalman Filtering (EKF). To reduce carrier linear acceleration and local magnetic field that impact on attitude measurement, the adaptive covariance matrix structure is considered. Moreover, the heading angle correction threshold method is used in magnetic field compensation and interference environment. Based on the experimental results, the effectiveness of the proposed algorithm suppresses the influence of the external magnetic interference on heading angle, as well as improving the accurac of sstem attitude measurement. Copright 4 IFSA Pulishing, S. L. Kewords: Multi-sensor, Complex environment, Fusion algorithm, Anti-magnetic interference.. Introduction The widespread dissemination of smartphones has prompted a great success of positioning sstems using moile terminals for location ased services, lie the emergenc relief map for in-door intelligent navigation construction of cit life [-3]. Magnetic Angular Rate Gravit (MARG) sensor which is ased on micro-electro mechanical sstem (MEMS) technolog has een widel applied to intelligent terminals. Hence, the design of MARG positioning sstem graduall ecomes a hot topic [4]. MARG positioning sstem is with the advantages of low cost, self-positioning, high positioning accurac, and good stailit in short time duration. Due to the sensor noise and cumulative errors, MARG positioning sstem cannot provide long-term stale and reliale location information. To solve this prolem, an optimal fusion of MARG sensor is justifiale. The MARG sensor ased on quaternions Extended Kalman Filtering (EKF) was adopted for attitude algorithm in [5]. The Kalman Filtering (KF) ased on quaternion is used for real-time human motion tracing in [6]. In [6], the QUEST algorithm was used instead of Gauss- Newton algorithm. In [7] and [8], to mae a progress in improving the accurac of MARG sensor attitude measurement, the EKF ased on four elements was considered. On this asis, the sensor mode which introduced sensor ias compensation and adaptive measurement noise covariance matrix structure 6

2 Sensors & Transducers, Vol. 75, Issue 7, Jul 4, pp. 6- to improve the accurac of measurement of attitude was estalished Liu in Tsinghua Universit in [9]. The precision of magnetometer is especiall important to the attitude measuring accurac since MEMS gro error spreads rapidl with respect to time duration. Although the aove mentioned algorithms can calculate carrier s attitude, the do not consider the situation that the actual measurements could e influenced interference. When there is interference magnetic field, the magnetometer data could e resulted into great error attitude solution. In this paper, the anti-jamming algorithms ased on quaternion EKF is proposed to reduce the influence of linear acceleration and carriers around the local magnetic field on attitude measurement constructing an adaptive covariance matrix. Moreover, ased on the magnetic field compensation and threshold in magnetic interference environment, the heading angle is corrected and the errors generated from the external interference fields are suppressed effectivel.. Attitude Measurement and Analsis The measurement data from inertial measurement unit, lie the groscope, magnetometer, and accelerometer, are defined as od coordinate frame. The origin of coordinates is the center of od s gravit and the three coordinate axes are corresponding to the longitudinal axis of the carrier, the horizontal axis, and the vertical axis respectivel. The asolute coordinate frame is named navigation frame, notated as n. The transformation is achieved quaternion and Euler angle method. The quaternion has een widel used since it can solve singular prolem []. The conversion relationship etween od coordinate frame and navigation frame is shown in (): x xn = Tn ( q) n, () z z n q = q + qi + qj+ q3 () Tn () q = q + q q q ( qq + qq ) ( qq qq ) ( ) ( ) ( ) ( ) qq qq3 q q+ q q3 qq3+ qq qq 3+ qq qq3+ qq q q q q3 (3) If the rotation quaternion q is otained, each element, T ( q ), can e determined (3). n When the three-axis gro angular velocit and initial attitude are otained, the differential equation of attitude quaternion is as follows: qt ( ) = q, d q = q ω dt (4) the argument t is the initial time of od movement. q is the initial alignment attitude quaternion. Notation is the quaternion multiplication sign. ω is the posture angular velocit vector which is defined in (5): ωx ω ωz ωx ωz ω ω = ω ωz ω x ωz ω ωx (5) Within the time sampling interval T, the angular rate can e assumed to e a fixed value, T is also recognized as the update interval of rotation quaternion. On this asis, the attitude quaternion model in discrete time condition can e expressed in (6): q = exp( ωt) q,( = +,, ) (6) Therefore, the attitude rotation matrix is updated (6) and () ased on the angular velocit data from three-axis groscope. 3. EKF Design EKF is proposed to do sensor data fusion. The process of EKF is defined in (7): noise. x+ Ax w = +, (7) = Cx+ + v w is the covariance matrix vector of process v is the measurement noise covariance matrix vector. According to the discrete-time model of gro quaternion method, the state transition vector equation is defined as q = + Aq + w, (8) A = exp( Ω ( ωt )) is state transition matrix. The process noise covariance matrix Q is 7

3 Sensors & Transducers, Vol. 75, Issue 7, Jul 4, pp. 6- σ ω I Q = σ gi σ g I (9) The oservation equation is constructed stacing accelerometer and magnetometer measurement vectors, such that a + = = m + a Tn ( q +) g v, + m Tn ( q +) h v () T ( q ) is the attitude rotation matrix n + of quaternion updated; g is the gravitational acceleration vector; h is the magnetic field intensit vector. The covariance matrix of measurement model R is σ a I R = σ mi () Due to the nonlinear relationship etween state and measurement vectors, () can e otained as a linearized oservation matrix, as shown in (). C = q q3 q q q q q3 q 4q 4q 3 q 3 q z q + q z 3 q q z q + q z 4q + q z q z 4q + q z 3 q z q + 4q z q q 3 q + 4q z 3 () compensated previousl []. To solve this prolem, we construct an adaptive covariance σ as σ m m m m m var m-n m+ N = ( ) + ( : ), (4) m and m are the setting weighting factors. var( m-n : m + N ) is the variance modulo value that each point of the magnetometer measurements. m is the local magnetic field strength standard. When the variance is large, the external magnetic interference magnetometer modulus value and modulus values also ecome large, as well as the covariance. 4. Magnetic Interference Analsis 4.. Magnetic Field Compensation To eliminate elliptic differential effect of magnetic compass which is generated environmental magnetic interference, the initial caliration of magnetometer efore movement is considered in []. Without considering the situation that the vector magnetic field and measurement error exist, the od movement is with small change in magnetic field area. For the strap-down magnetic sensor, the horizontal component of geomagnetic field trajector measurement is a circle on the X and Y shaft, the circle is centered on the origin, and the radius is the horizontal intensit of geomagnetic field [3-4]. The trajector can e approximatel recognized as an ellipse while actual magnetic sensor measurement, for there are man inds of measurement error and the magnetic interference existing, which will cause the center of the circle shift, thus the shape distorted, as shown in Fig.. m In the stationar state, the od tilt angle of relative plane can e calculated accelerometer accuratel. B using the actual output value of accelerometer directl when the od gets linear acceleration, the od attitude angle results in a large error. Therefore, the estimator constructs a new data covariance an adaptive method, as shown in (3). σ = ( a g ) (3) a O O hard magnetic disturance The proposed sstem can e adjusted to the weight of measured value in a real-time manner. The covariance changes with the change of the od producing a larger linear acceleration. A large error of magnetometer measurements appears when the amient magnetic interference exists. Interference caused magnetometer is normall modified magnetic field compensation, while the external magnetic interference which is random and difficult to e eliminated cannot e no disturance soft magnetic disturance Fig.. D magnetic field trajector measured strap-down magnetometer. In this paper, the 8 characters caliration method is used to determine the maximum and minimum of three-axis magnetometer data efore od movement, and meanwhile the three-axis caliration 8

4 Sensors & Transducers, Vol. 75, Issue 7, Jul 4, pp. 6- factor and ias value are otained, as shown in (5) and (6). X = ( Xmax Xmin )/ Y = ( Ymax Ymin )/, Z = ( Zmax Zmin )/ (5) Kx = max{, X / ( X Xmax )} K = max{, Y / ( Y Ymax ), } Kz = max{, Z / ( Z Zmax )} (6) X, Y, and Z stand for the three- axis magnetometer ias values. K, K, and x K z are the three-axis magnetometer caliration factor respectivel. The output of the three-axis magnetometer is as follows. X = Kx Xin + X Y = K Yin + Y Z = Kz Zin + Z 4.. Threshold Limit (7) The influence can e normall eliminated magnetic field compensation algorithm when the outside interference field is stale. The magnetometer data cannot e effectivel corrected magnetic field compensation algorithm when the external magnetic field change is large. Then, a threshold value method for heading correction is proposed in this paper. The existence of interference is determined the preprocessing of three axis magnetometer modulus values. With magnetometer molding and its change rate to judge whether there is a magnetic disturance. The equations are expressed as follows. var( mstep-n : mstep+ N ) < m mstep <.3 m var( m m + ) step-n step N, (8) : is the variance of magnetometer measurements modulo value for each step. The size of sliding window equals to N. m step is the modulus value of three-axis magnetometer for each step. If the relations in (8) are satisfied, the change of magnetometer field strength measurement is not dramatic, the external magnetic interference is not significant, and the attitude angle solved EKF is optimal. Otherwise, the attitude angle solved EKF cannot e reliale. The influence of the external magnetic interference on attitude angle is reduced the incremental attitude angle which is solved gro. In the unstale interference environment, the magnetic field compensation algorithm is ineffective, especiall for the local magnetometer with the data varing remaral, lie the shopping malls and garages. The attitude angle solved gro is not affected magnetic interference, ut it is credile onl in a short time due to cumulative error. Therefore, the heading angle is optimized the differences etween ever two neighoring step headings which are solved gro, as shown in (9). ave() i =Δ gro () i + EKF ( i ) gro() i gro() i ( i ) Δ = gro, (9) Δ () i is the heading angle solved gro gro for the current step. ( ) EKF i is the heading angle i is the solved EKF for the previous step. ave() optimal estimated heading angle. 5. Experimental Results In our experiments, a Huawei smartphone is selected as the test platform, as shown in Fig.. B using Android operating sstem, the smartphone is integrated with MARG sensors containing a threeaxis accelerometer sensor LIS3DH, a three-axis magnetometer sensor am8963, and a three-axis groscope ST L3G4D. The raw data from MARG sensor are otained from the Application Program Interface (API) provided the sstem with frequenc of 5 Hz. Z Y X Fig.. Sensor data acquisition platform. In a corridor environment in our universit campus, the pedestrian wals along a line in the corridor which is closed rectangle. We can use the magnetometer over extended periods of time in magneticall stale environments. The stailit of an environment is characterized low ferrous ojects, or power-line near the 9

5 Sensors & Transducers, Vol. 75, Issue 7, Jul 4, pp. 6- magnetometer. In complicated indoor areas, the prolem of navigating a pedestrian ecomes even more challenging, due to the proximit to metallic ojects and walls, supported ferrous pillars. The magnetometer modulus value can clearl reflect the change of the magnetic field. In the magnetic interference area, the magnetometer modulus value is rather changeale. Mag n orm(ut) magnetic interference area pointing due south or north direction. The results in Fig. 4 show that the heading angle EKF results in large errors, while the errors can e suppressed effectivel our proposed anti-jamming algorithm, especiall in the environment with strong magnetic field interference. Fig. 5 shows that in magnetic interference environment, the value of attitude determination algorithm is much close to the real value inside the circle. The maximum deviation of heading angles etween the anti-jamming algorithm prediction and reference values is within ±5 deg, and average deviation is aout.67 deg. The heading angles from EKF have large enough error. 5 EKF and Reference anti-jamming optimization and Reference Time(s) Fig. 3. Curves of three axis magnetometer modulus values. Difference(Deg) magnetic interference area Fig. 3 shows that there is strong magnetic interference in the environment. Inside the two circles there is a large change of the magnetometer modulus value. In this case, the sensor data are optimized interference algorithm and EKF. We compare the results of heading angles among EKF, the anti-jamming, and reference in Fig. 4. Head(Deg) hopping from to 36 EKF anti-jamming optimization Reference Time(s) Fig. 4. Comparison of heading angles. In Fig. 4, the dotted line, thin solid line, and thic solid line indicate the heading angle EKF, heading angle EKF with interference, and the real heading angle respectivel. The heading angle produces a hopping from to 36 when the Time(s) Fig. 5. Difference etween EKF and anti-jamming. 6. Conclusions In this paper, the proposed anti-jamming algorithm ased on EKF reduces the influence of the local magnetic field on attitude measurement. A new structure of adaptive covariance matrix using modulus value and variance method is also addressed to solve the prolem of the heading errors in magnetic interference environment. The proposed technique has man advantages over an other conventional method such as the use of the threshold limit values, which does not need an error modeling or awareness of the nonlinearit. The algorithm of anti-jamming techniques decreased the required time for the fixing up interference process, extending the opportunit to appl the proposed algorithm in real-time applications. This technique would help decrease the heading errors of the users in pedestrian navigation. Furthermore, in magnetic interference environment, the methods of magnetic field compensation and heading angle correction effectivel improve the accurac of attitude measurement sstem.

6 Sensors & Transducers, Vol. 75, Issue 7, Jul 4, pp. 6- Acnowledgements This wor was supported in part the Program for Changjiang Scholars and Innovative Research Team in Universit (IRT99), National Natural Science Foundation of China (636), Special Fund of Chongqing Ke Laorator (CSTC), Fundamental and Frontier Research Project of Chongqing (cstc3jcja43, cstc3jcja434, cstc3jcja44), Scientific and Technological Research Program of Chongqing Municipal Education Commission (KJ358), Startup Foundation for Doctors of CQUPT (A-33), Science Foundation for Young Scientists of CQUPT (A-77), and Student Research Training Program of CQUPT (A3-64). References []. Hightower J., Borriello G., A surve and taxonom of location sstems for uiquitous computing, IEEE Computer, 34, 8,, pp []. Lee S., Kim B., Kim H., et al., Inertial sensor-ased indoor pedestrian localization with minimum a configuration, IEEE Transactions on Industrial Informatics, 7, 3,, pp [3]. Chon J., Cha H., Lifemap: A smartphone-ased context provider for location-ased services, IEEE Pervasive Computing,,,, pp [4]. Woodman O., Harle R., Pedestrian localisation for indoor environments, in Proceedings of the th International Conference on Uiquitous Computing, 8, pp [5]. Marins J. L., Yun X., Bachmann E. R., et al., An extended Kalman filter for quaternion-ased orientation estimation using MARG sensors, in Proceedings of the IEEE/RSJ International Conference on Intelligent Roots and Sstems, 4,, pp. 3-. [6]. Yun X., Bachmann E. R., McGhee R. B., A simplified quaternion-ased algorithm for orientation estimation from earth gravit and magnetic field measurements, IEEE Transactions on Instrumentation and Measurement, 57, 3, 8, pp [7]. Strohrmann C., Harms H., Tröster G., et al., Out of the la and into the woods: Kinematic analsis in running using wearale sensors, in Proceedings of the 3 th International Conference on Uiquitous Computing,, pp. 9-. [8]. Munguia R., Grau A., An attitude and heading reference sstem (AHRS) ased in a dual filter, in Proceedings of the IEEE 6 th Conference on Emerging Technologies & Factor Automation (ETFA),, pp. -8. [9]. Liu Xingchuan, Zhang Sheng, Li Lizhe, et al., Measurement algorithm ased on quaternion attitude MARG sensor, Tsinghua Universit: Natural Science, 5, 5,, pp []. Li F., Zhao C., Ding G., et al., A reliale and accurate indoor localization method using phone inertial sensors, in Proceedings of the ACM Conference on Uiquitous Computing,, pp []. Ali A., Siddharth S., Sed Z., et al., An Improved Personal Dead-Reconing Algorithm for Dnamicall Changing Smartphone User Modes, in Proceedings of the 5 th International Technical Meeting of the Satellite Division of the Institute of Navigation (ION GNSS ), Nashville,, pp []. Madgwic S. O. H., Harrison A. J. L., Vaidanathan R. Estimation of IMU and MARG orientation using a gradient descent algorithm, in Proceedings of the IEEE International Conference on Rehailitation Rootics (ICORR),, pp. -7. [3]. Shin B., Lee J. H., Lee H., et al., Indoor 3D pedestrian tracing algorithm ased on PDR using smartphone, in Proceedings of the th International Conference on Control, Automation and Sstems (ICCAS),, pp [4]. Nilsson J. O., Zachariah D., Sog I., et al., Cooperative localization dual foot-mounted inertial sensors and inter-agent ranging, arxiv Preprint arxiv: , 3. 4 Copright, International Frequenc Sensor Association (IFSA) Pulishing, S. L. All rights reserved. (