A low-cost attitude heading reference system by combination of GPS and magnetometers and MEMS inertial sensors for mobile

applications
Yong Li, Andrew Dempster, Binghao Li, Jinling Wang, Chris Rizos School of Surveying and Spatial Information Systems, University of New South Wales, Sydney, NSW 2052, Australia Yong.li@unsw.edu.au Abstract. This paper describes a prototype system for attitude and heading determination. A L1-only GPS receiver is integrated with microelectromechanical gyroscopes, accelerometers and magnetometers. In contrast to a multiantenna/multi-receiver GPS attitude determination system, this system uses a single antenna/single receiver configuration to derive standalone velocity and acceleration solutions from the GPS L1 carrier phase measurements. No reference station is needed to form differences of carrier phase measurements for the velocity and acceleration calculation. The GPS-derived acceleration is further used in the attitude determination by combination with the three-dimension acceleration sensed by the accelerometers. The magnetometers sense the Earths magnetic field intensity, and can give the heading estimation regardless of the status of the host platform. To satisfy real-time applications, infinite impulse response differentiators instead of finite impulse response differentiators are used to derive the acceleration from GPS. The algorithms have been implemented and their efficiency demonstrated by experiments. Keywords. Attitude determination, micro-electromechanical system, inertial units, GPS DMU-HDX-AHRS (Crossbow, 2000). However, todays MEMS sensors are still much less accurate than expensive precise inertial sensors, intended for use in inertial navigation systems (INS). Smart filtering and extra sensors can help to address these problems. However, complex algorithms require powerful computational platforms to keep the solutions tracking in highly dynamic mobile applications. Handheld devices can be used as the computational platform to perform these algorithms; however, such a design may require a heavy data communication load between the sensors and the computational platforms. Taking these factors into account, one has to make a tradeoff between the computational speed and complexity of the data processing algorithms. This paper introduces the development of an AHRS/GPS integrated system. The MEMS sensorbased AHRS consists of microelectromechanical gyroscopes, accelerometers and magnetometers; which sense the tri-axial acceleration of the host platform, the Earths magnetic field vector, and the tri-axial angular rate. These measurements are combined with the known acceleration and magnetic vectors in the reference coordinate system to give the orientation of the host platform. This scenario is valid when the host platform is static, as in a territorial application. For a dynamic platform, gyroscope measurements must be processed in order to update the attitude solution. GPS can help to monitor the dynamics of the platform and drive the procedure in a suitable mode in order to respond properly to the dynamics. To achieve the highest accuracy that a L1 GPS receiver can reach to, the L1 carrier phase measurements are used for computing the acceleration of the receiver. This implementation is more advanced than deriving the acceleration through differencing the position. The GPS-derived acceleration can be used to estimate the attitude, as well as correcting the gyroscope bias. To satisfy the speed requirement for mobile applications, infinite impulse response

1. Introduction
Microelectromechanical sensors (MEMS) are experiencing rapid growth in demand in many applications, including general aviation, unmanned automotive vehicle (Brown and Lu, 2004), locating personnel (Collin et al 2002; Cho and Park, 2005), mobile mapping systems (Ellum and El-Sheimy, 2002), and athletic training monitoring. MEMS inertial sensors are devices well suited to many applications because of their tiny size and low cost, and are ideal as components of a compact and affordable attitude and heading reference system (AHRS), e.g. in the 3DG-M (Microstrain, 2003) and

differentiators are used instead of finite impulse response differentiators to derive the acceleration from GPS.

2. System design
The prototype system consists of a SuperStar II GPS OEM board, a 3DM-G, a USB-RS232 adapter, and a laptop PC. The SuperStar II provides the GPS raw measurements, including pseudorange and L1 carrier phase (CPH) measurements, as well as the broadcast ephemeris. The 3DM-G consists of 3axial magnetometers, 3-axial accelerometers, and 3axial gyroscopes. It provides the raw measurements for computing orientation. The USB-RS232 adapter provides additional RS232 ports for the GPS receiver and the 3DM-G to be connected with the laptop PC where the data are recorded and the commands are sent to the devices. The electronic board of the 3DM-G is shown in Fig. 1, and it specifications are listed in Table 1. The output of the magnetometers is a vector quantifying the direction and magnitude of the magnetic field. It is a relative quantity and scaled with respect to another, e.g. the geomagnetic field strength present in the factory during calibration (MicroStrain, 2003). Fig. 1 Electronic board of 3DM-G

Table 1. Main specifications of the 3DM-G (MicroStrain, 2003) parameter Orientation range Orientation angle resolution Temperature drift Repeatability Accuracy Sensor range Value 360 deg, all axises <0.1 deg +/-0.025% per Celsius degree 0.01 deg +/- 5 deg Gyros: +/- 300 deg/sec Accelerometers: +/- 2 g Magnetometers: +/- 1 Gauss 100Hz 64 mm by 90mm by 25mm 40 grams with enclosure, 18 grams without enclosure 5.2 12 VDC 90mA

Output data rate Enclosure Weight Supply voltage Supply current

The software is designed to be suitable for operation in a real-time environment. The memory

storage requirement and the computational load are the crucial issues addressed in the design. The software uses multithreads and assigns them different priority levels. The communication thread is designed to run in the background and monitor the communication events and receive/send data as well. The whole system function is divided into tasks/modules to response the requests from the main thread. The corresponding Windows messages are defined to notify the event occurrences. For example, the instantaneous vector packet of the 3DM-G arrives, a corresponding Windows message will be triggered and the processing function of this message will become active to perform the attitude determination. Thanks for the similarity between the Windows message mechanism and the interrupt mechanism of an embedded operating system, this design should allow easy migration of the source code to an embedded system. The 3DM-G has an internal clock and updates its raw data at a rate of a hundred times per second. But it is a relative time and does not align with, for example, Universal Time Coordinated (UTC). Visual C++ commands allow the users software to access the UTC time. The software labels the first arrival of the raw data with the UTC time, and uses the 3DM-G embedded clock to account for the increase of the time span. This method marks the raw data of the 3DM-G on UTC time. In the meantime, the GPS messages are also marked on arrival on UTC time so that the delay of the actual arrival UTC time relative to the GPS time can be derived. The method provides the common time for both the GPS data and the 3DM-G data, and makes it possible to integrate the GPS with the 3DM-G. When the 3DM-G is static, the accelerometers and magnetometer data are sufficient for computing the tri-axial angular solutions. The gyroscopes can provide short-term accurate updates after their biases have been compensated. But the accuracy would degrade over the long-term. Therefore the SuperStar II GPS receiver is used as an external aid. Through differencing the carrier phase twice the range acceleration can be obtained. The linear acceleration can be further derived from the range accelerations of at least four visible satellites. The GPS-derived acceleration provides a reference for the output of the accelerometers. Further combining the accelerations with the magnetometer data the tri-axial attitude can be obtained in a dynamic environment. Such solutions can aid the estimation of the on-line gyroscope bias. A flow chart of the algorithm for GPS position, velocity and acceleration determination, as well as attitude determination, is shown in Fig. 2. Two parts, the attitude determination from the MEMS sensor data and the GPS acceleration determination, have been implemented and the remainder of this paper will focus on these two aspects.

Fig. 2 Structure of the AHRS/GPS system Bias estimate Gyro Magn Accl GPS
fb

problem (Wertz, 1984):

2 J (A ) = w a a B Aa 2 R 2 + wm m B Am 2 R

(1)

b
mb

output

Att updating Attitude Determination

Hatch filter s

SV pos

Pos cal
SV p.v.

2 2 and wm are weighted coefficients where wa associated with the accelerometers and the magnetometers, respectively. ( a B , a R ) are the gravity vectors coordinates in the B-frame and Rframe, respectively. ( m B , m R ) are the magnetic field vectors coordinates in the B-frame and Rframe, respectively. The optimal quaternion estimate of the Wahba problem is the eigenvector of K, with the largest eigenvalue:

Vel cal s

= max q Kq
C + CT (trC)I D K= DT trC

(2) (3)

3. Attitude determination
The coarse attitude solution can be obtained from the output of the accelerometers and the magnetometers when they are stationary or in a low-dynamic environment. The tilt angles can be calculated from the accelerometer output and the heading can be calculated by combining the magnetometer output. In a high-dynamic environment, the gyroscope output can be used to update the attitude solution. But the MEMS gyroscope loses accuracy very rapidly because of its large drift. The acceleration derived from GPS L1 carrier phase measurements can be used for dynamic attitude determination. In general, for the acceleration vector and the magnetic field vector their coordinates both in the reference coordinate system and the body-fixed frame are known, the attitude solution can be obtained through a quaternion method or a direction cosine matrix method. An attitude solution can be mathematically represented by either of three kinds of parameters, a quaternion, orientation matrix, or Euler angles. All the representations are mathematically equivalent and can be converted from one to another. If there are limitations on data storage and the communication load, the four-parameter quaternion would be a good choice for the attitude solution output. 3.1 Optimal attitude solution For the gravity vector and the magnetic field vector, their coordinates in both the reference coordinate system (R-frame) and the body-fixed coordinate system (B-frame) are known. The optimal attitude solution can be obtained by resolving the Wahba

Ephemeris

Acc cal where

PVA cal

SV p.v.a.

T 2 2 C = wa a BaT R + wm m B m R

(4) (5)

D = w (a B a R ) + w (m B m R )
2 a 2 m

Householder's method can be used to compute

but it is computationally burdensome. The q

QUEST (QUaternion ESTimator) algorithm reduces the computational load significantly. 3.2 TRIAD algorithm The gravity vector and magnetic field vector can define a Cartesian coordinate system with unit vectors along three axes i, j, and k. Two Cartesian systems can be determined by two pairs of ( a B , m B ) and ( a R , m R ) (Wertz, 1984):
i B = aB aB , i R = aR aR k B = i B jB , k R = i R jR

(6) (8)

j B = i B m B i B m B , j R = i R m R i R m R (7)

The coordinate transformation matrix A can be then obtained:

T T A = i B iT R + jB jR + k B k R

(9)

The solution of Eq. (9) is orthogonal. No additional orthogonalisation is needed.

4. Digital differentiator
A differentiator approximates the ideal digital differentiator (Antoniou, 1993):
H (e jT ) = j

(10)

The proposed differentiators fall into two categories, the finite impulse response (FIR) and the infinite impulse response (IIR). Various ways to design the FIR differentiators have been discussed in the

literature (e.g. Brutton et al, 1999), including loworder Taylor approximations, Fourier series design, optimal designs, and curve fitting methods. A FIR differentiator which has less attenuation in the high frequency band usually has a relative high order, which leads to difficulties in applying it in a realtime scenario. The simplest first-order backward differentiator is widely used to perform the derivative. However, it is able to approximate the ideal differentiator to 0.3 of the Nyquist frequency (refer to Fig. 3). The IIR differentiator can be derived from analogue filter designs. Al-Alaoui (1993) proposed an approach for the design of differentiators by inverting the stabilised weighted combination of the trapezoidal and rectangular integrators. The AlAlaoui operator is an excellent approximation to the ideal differentiator and its digital transfer function is:
H ( z) = 8 / 7 z 1 T z + 1/ 7

frequency). Thus, the Al-Alaoui differentiator is employed in this system.

5. Precise acceleration from GPS

The GPS acceleration can be derived either in the position domain or the range domain using a differentiator. Implementation in the position domain is simpler than that in the range domain. As the GPS position is usually derived from the pseudorange the position error is of the order of several to tens of metres. The derived acceleration is therefore very noisy and inaccurate. The CPHderived position solution is much more accurate and less noisy. But such a solution needs multiple dualfrequency receivers, i.e. through forming doubledifferenced carrier phase measurements to derive the baseline solution. Such an implementation is too expensive and awkward for a MEMS-based sensor system. The implementation in the range domain is relative sophisticated. The range acceleration can be derived from the Doppler measurements or from the CPH measurements. The L1 carrier phase is used in our system to obtain the range acceleration, and to then compute the point solution of the receivers acceleration. To do so, the L1 carrier phase measurement is differenced once to derive the range-rate, and differenced twice to obtain the range acceleration, which is further used to derive the point acceleration solution. Two Al-Alaoui differentiators are used in cascaded mode to accomplish the second-order differentiation of range. The GPS satellite acceleration is calculated from the broadcast ephemeris data. With the range acceleration derived from the carrier phase, the receivers acceleration is calculated from four or more tracked GPS satellites using the least-squares method. Fig. 4 Static 3D acceleration derived from L1 CPH

(11)

The analysis shows that the Al-Alaoui differentiator approximates the ideal differentiator up to 0.78 of the Nyquist frequency with 2% error, and has an excellent approximation to a linear phase response (Al-Alaoui, 1993). Since it is a first-order system, only the data of the previous epoch needs to be stored in the system memory, and the computational load is very small, thus it well suited for real-time applications. Fig. 3 Magnitude response of differentiators
1 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 0 0.1
Ideal InvS 1-Back Al-Alaoui

Magnitude

0.2

0.3

0.4 0.5 0.6 Frequency

0.7

0.8

0.9

The magnitude of the three differentiators are compared in Fig. 3. They are: (1) Al-Alaoui differentiator (Lime); (2) 1-order backward differentiator (bright green); (3) Inverse Simpson differentiator (Red). The blue line depicts the ideal differentiator. Obviously the Al-Alaoui differentiator has the best approximation to the ideal response over the whole frequency range. The inverse Simpson has the poorest high frequency response. The first-order backward differentiator has a better high frequency response than that of the inverse Simpson, but the inverse Simpson has a better response in the middle frequency band (0.4 to 0.6 of the Nyquist

Fig.4 depicts the least-squares solution of the acceleration derived from the GPS L1 carrier phase data in a static test. The test was for a period of

1530 seconds. A Trimble 5700 GPS receiver was used to obtain the high quality L1 carrier phase measurements needed to verify the algorithm. It must be emphasised again that in this prototype system a SuperStar II low-cost GPS OEM board is used rather than a high quality geodetic-grade GPS receiver. The carrier phase is output at a rate of 1Hz. The solution is derived from 9 visible satellites, including the position from the carrier smoothed pseudorange, the velocity from the first-order derivative of L1 CPH, and the acceleration from the second-order derivative of the L1 CPH. The results of the statistical analysis are listed in Table 2. It shows the solution can reach an accuracy of less than 0.01m/s2. The averages along the three axes are very small (all are far smaller than the carrier phase accuracy, i.e. mm/s2 level). Thus it can be concluded that there is no bias in the acceleration solutions. Table 2. Result of the GPS-derived acceleration item ave std ae (m/s2) -1.27E-06 0.00258 an (m/s2) 6.04E-05 0.00329 au (m/s2) 5.73E-07 0.00717

requested data packets, e.g. the packet #03 containing the tri-axial magnetic field vector, the tri-axial acceleration, and the tri-axial gyro angular rate. The sensors body fixed frame (BF) is defined in Fig. 6. The reference system is the local northeast-down (NED) system. The 3(yaw)-2(pitch)1(roll) rotation sequence is applied for the coordinate system transformation between BF and NED. Fig. 6 Sensors body fixed coordinate system definition

Fig. 5 depicts the acceleration (in the north component) derived from L1 CPH in a kinematic environment. The north velocity derived from L1 CPH is depicted in the same figure. The raw measurement data was collected from a rowing boat. The Trimble 5700 receiver was used with the same settings as in the static tests. The figure illustrates that the L1 CPH-derived solution can track the rowers beat rowing very well. Further kinematic accuracy analyses will be conducted by comparing the solution with the dual-frequency baseline solution. Fig. 5 Kinematic north velocity and acceleration derived from L1 CPH
4 3 2 1 0 -1 -2 278900

During the 10 minute test the software recorded 44563 epochs of data (outputting at 100Hz). The TRIAD algorithm is used to derive the Euler angles from the 100Hz raw data. The result is depicted in Fig. 7. The statistical characteristics are listed in Table 3. The standard deviations of roll, pitch and yaw are 0.14 deg, 0.13 deg, and 0.91 deg, respectively. This demonstrates that the solution has an accuracy of better than 1 deg, 5 times higher than the nominal accuracy of the system (see Table 1). The solution is very tidy and no jumps are found in the solution. From Table 3, we can see that the yaw solution has the minimum deviation of -3.78 deg from the average and maximum deviation of 3.983 deg, and the deviation in pitch is in the range [-0.88, 0.491] deg, and the deviation in pitch is in the range [-0.531, 0.539] deg. Fig. 7 The 100Hz static attitude solution

Vel and Acc

vn (m/s) an (m/s^2)

278920

278940 278960 Time (sec)

278980

279000

6. Experiments
6.1 Static tri-axial attitude determination The software is developed in Visual C++ to communicate with the 3DM-G sending out the

Table 3. Results of the 100Hz tri-axial attitude test (deg) item roll pitch ave -1.099 -0.48 std 0.14 0.13 min -1.63 -1.36 max -0.56 0.011 (Total epochs = 44563) yaw 58.38 0.91 54.60 62.363

Fig.9a Trajectory of the test

Start point

All gyroscopes are prone to drift due to their biases. The bias is the output value of the gyroscope when it is stationary. Fig. 8 depicts the time history of the biases of the gyroscopes in the static test. During the period of about 347 seconds, the x-axial gyroscope drifted from -0.6749 rad/s to -0.1545 rad/s, with a rate of 0.001499 rad/s2. This result demonstrates that the gyroscope data is not useful until its bias is compensated for. A bias capture procedure can be designed in the software to log the instantaneous bias once the platform is detected as not rotating. Another option is to obtain the bias estimate during the initial calibration. An external sensor such as GPS can aid in correcting the bias on-the-fly. Fig. 8 Raw gyroscope data vs. time

end point

Fig. 9b Device setup in the test Forward direction of the car

Y_BF

X_BF The acceleration derived from the L1 carrier phase is shown in Fig. 10. Because the speed of the car in the test was below 15m/s, the corresponding Coriolis acceleration is smaller than 3mm/s2, and thus can be neglected. As shown in Fig. 10, the acceleration is less than 1.6m/s2, which would cause an angular error of less than 9 deg if the dynamics were neglected in the computations. This error is tolerable for just observing the performance of the system during the first stage of development. Fig. 10 GPS-derived horizontal acceleration 6.2 Kinematic test
2.5

In the kinematic test the 3DM-G is placed in the rear seat of the car and the GPS antenna is mounted on the roof. The trajectory of the car is depicted in Fig. 9a. There are three right turns and one left. The device setup is shown in Fig. 9b. The 1Hz pseudorange and L1 CPH as well as the broadcast ephemeris of the GPS satellites were recorded in the test. The 100Hz raw data of the 3DM-G were recorded the same time. The period was for about 130 seconds from UTC time 173476s to 173606s, in total 11497 epochs. Some data were lost due to communication errors.

2 1.5 accleration (m/s^2) 1 0.5 0 -0.5 -1 -1.5 -2

east north

-2.5 173460 173480 173500 173520 173540 173560 173580 173600 173620 Time (sec)

The TRIAD algorithm is used to derive the attitude solution from the 100Hz raw accelerometer data and magnetometer data. Fig. 11 depicts the Euler angle solution with the dynamics neglected. It shows the solution can track the angular maneuvers

well. This point is also demonstrated by the heading solution. Fig. 11 The 100Hz kinematic attitude solution

the test the 3DM-G is installed in the car and its xaxis points to the rear of the car (Fig. 9b) which results in an about 180 deg difference between yaw and heading. Because the 3DM-Gs x-axis is not strictly parallel to the cars driving direction, an unknown misalignment error appears. The GPS heading completely depends on the velocity. The faster the speed, the higher the accuracy of the heading. The GPS heading begins to wander when the car slows to a stop. Therefore, in the test the GPS heading is not output when the speed of the car is less than 0.2m/s (=0.76km/hr). This is why the GPS heading in Fig. 12 appears discontinuous.

7. Conclusions and further work

The paper describes a MEMS-AHRS and GPS integrated system. The tri-axial attitude solution has been successfully derived from the raw data of the magnetometers and accelerometers. The static result indicates that the accuracy of the solution can be less than 1 deg (0.14 deg in roll, 0.13 deg in pitch, and 0.91 deg in yaw) for 100Hz update rate. GPS is introduced into the system to aid the attitude determination in dynamic environments. The GPS L1 carrier phase is used to derive the range-rate and range acceleration via the first-order Al-Alaoui IIR differentiator. The standalone GPS acceleration solution is further derived from the range accelerations. The tests have demonstrated that the algorithm can operate efficiently in both static and kinematic tests. The static result shows that the accuracy of the solution can reach better than 1cm/s2 (2.6mm/s2 in east, 3.3mm/s2 in north, and 7.2mm/s2 in up) for 1Hz update rate. Comparison of the GPS-derived heading and the heading derived from the raw data of the 3DM-G demonstrates that the attitude determination algorithm works well in a kinematic environment. All turns in the driving test can be identified from the solution. This demonstrates the capability of the algorithm to track the maneuvers of the platform. The result has demonstrated the feasibility of the system design and the success of the attitude determination and the GPS-derived acceleration. Future development includes: (1) Estimating the gyroscope bias and drift, and their real-time compensation. (2) Including the GPS acceleration in the real-time attitude calculation.

6.3 Comparison of heading solutions Comparison of the heading solutions enables an evaluation of the performance of the attitude determination algorithm, especially when there is no reference device. Fig. 12 depicts two heading solutions, the 1Hz GPS result and the 100Hz result derived from the raw data of the 3DM-G. Fig. 12 Comparison of heading solutions
250 200 150 Heading (deg) 100 50 0 -50 -100 -150 173476
3DM-G GPS

173496

173516

173536 173556 Time (sec)

173576

173596

To clearly explain the solution, the roll-over of 180 deg in the heading has been corrected. As a result, a rising edge in Fig. 12 describes right turn, and a falling edge in Fig. 12 describes the left turn. One can see that there are three right turns and one left turn in the trajectory. This can be confirmed from Fig. 9a. Two terms cause up/down displacements between two heading solutions. The first is the declination, which is the difference between true north and magnetic north. For example, the declination is 12.6 deg in Canberra, Australia (USGS, 2005). The GPS heading is referenced to true north, and the 3DM-G heading is referenced to the Earths magnetic north. The declination is not applied to the 3DM-G heading solution here. The second term is the misalignment error. In

References
Antoniou, A. (1993), Digital Filters: Analysis, Design and Applications, New York, MacGraw-Hill. Al-Alaoui, M.A. (1993), Novel digital integrator and differentiator, Electronics Letters, 29 (4), 376-378.

Brown, A.K., and Lu, Y. (2004), Performance test results of an integrated GPS/MEMS inertial navigation package, Proceedings of ION GNSS 2004, 21-24 September, California, 825-832. Brutton, A.M., Glennie, C.L., and Schwarz, K.P. (1999), Differentiation for high-precision GPS Velocity and acceleration determination, GPS Solutions, 2 (4), 7-21. Cho, S.Y., and Park, C.G. (2005), A calibration technique for a two-axis magnetic compass in telematics devices, ETRI Journal, 27(3), 280 288. Collin, J., Lachapelle, G., and Kappi, J. (2002), MEMS-IMU for personal positioning in a vehicle A gyro-free approach, Proceedings of ION GPS 2002, 24-27 September, Portland, 1153-1161. Crossbow (2000), DMU-HDX-AHRS Users Manual, Revision B, Document 6001-004, Crossbow Technology, Inc, CA, USA. Ellum, C., and El-Sheimy, N. (2002), Inexpensive kinematic attitude determination from MEMSbased accelerometers and GPS-derived accelerations, NAVIGATION: Journal of Institute of Navigation, 49 (3), 117-126. Honeywell (1995), Compass Heading Using Magnetometers, AN-203, rev. A, Honeywell. Website accessed December 2003, http://www.ssec.honeywell.com/magnetic/datash eets/an203.pdf MicroStrain (2003), 3DM-G User Manual, Ver1, MicroStran, Inc., VT, USA. MicroStrain (2005), 3DM-G Flyer, website accessed October 2005. http://www.microstrain.com/pdf/3DMGflier9.pdf USGS (2005), Geomagnetsim program summary, http://geomag.usgs.gov/program.html, accessed October 2005. Wertz, J.R. (1984), Spacecraft Attitude Determination and Control, D. Reidel Publishing Company, Holland.