- Email: [email protected]
magnetometer integrated navigation system
Available online at www.sciencedirect.com
Advances in Space Research 45 (2010) 1350–1357 www.elsevier.com/locate/asr
Kalman filtering for GPS/magnetometer integrated navigation system Hang Guo a,*, Min Yu b, Chengwu Zou b, Wenwen Huang b b
a Nanchang University, Nanchang 330031, Jiangxi, China Jiangxi Normal University, Nanchang 330022, Jiangxi, China
Received 14 August 2008; received in revised form 6 January 2010; accepted 11 January 2010
Abstract This paper investigated the data processing method for a GPS/IMU/magnetometer integrated system with Kalman filtering (KF). As a result of GPS/IMU/magnetometer land vehicle system, dead-reckoning of magnetometer and accelerometer integrated subsystem bridged very well the GPS signal outage due to the trees on the two sides of the road. Both differential GPS data processing method and the carrier-phase method with magnetometers’ outputs for predicting the car position, velocity, and acceleration (PVA) are presented. The results from DGPS with Kinematical Positioning (KINPOS) software shown that the averages of the north, east, and down direction standard deviation (short for “std”) are 0.014, 0.010, and 0.018 m, respectively. The std of velocities and accelerations derived by the position and velocity differentiation are 10, 7, 13 mm/s, 7, 5, 9 mm/s2, respectively. This method for getting velocities and accelerations requires higher accurate position coordinates. But the position accuracy has frequently been degraded in this case when the car drove under the trees or other similar kinematical environments. That caused the larger velocity and acceleration errors. While the results from the carrier-phase method are std of the velocities = 2.1 mm/s, 1.3 mm/s, 3.7 mm/s in north, east, down, and std of the accelerations = 1.5 mm/s2, 0.9 mm/s2, 2.3 mm/s2 for the static test period; as compared with KINPOS software results, std of the velocity difference between the carrier-phase method and the DGPS method = 7 mm/s, 6.9 mm/s, 9.7 mm/s in north, east, down direction, and std of acceleration difference = 5.0 mm/s2, 4.5 mm/s2, 7.5 mm/s2 in north, east, down direction for the kinematical test period. Obviously, errors come from both the carrier-phase method and DGPS velocity and acceleration results derived directly by the position differentiation. In addition, better accuracy of positions than that before KF has been got by means of velocities and accelerations derived by the carrier-phase method after KF. Ó 2010 COSPAR. Published by Elsevier Ltd. All rights reserved. Keywords: DGPS; IMU; Magnetometer; Accelerometer; Carrier-phase method; Kalman filtering
1. Introduction There are different kinds of integrated navigation systems for a variety of applications, such as GPS/IMU, pseudollite, magnetometer, odometer, etc (Langley, 2003). GPS/magnetometer integrated system has the advantages of magnetometer’s low cost, GPS high accuracy, and mutual benefits on the vehicle orientation determinations. Amundson (2006) presented the compass assisted GPS for location-based services (LBS) applications. When GPS is not available for LBS, the electronic compass will provide the direction, and MEMS accelerometers will be *
Corresponding author. Tel.: +86 791 2941198. E-mail address: [email protected] (H. Guo).
a kind of odometer to measure the distances along the route. Honeywell’s dead reckoning systems has good location accuracy, and can be used in the halls or inside buildings (Caruso, 2007). In this project, we tested two GPS receivers and a threeaxis magnetometer/three-axis accelerometer integrated system (MicroStrain 3DM-GX1) on a car near University of New Brunswick (UNB), Canada. GPS receivers are lined as a baseline on the top of car to measure the car orientation as the reference, while the magnetometers were put in the center of the short baseline (1.22 m), which measures the car orientation as well. Both GPS and magnetometer’s data were collected. GPS data was processed by DGPS with Kinematical Positioning (KINPOS) software (by Dr. Gerald Mader, Chief, Geosciences, Research Division,
0273-1177/$36.00 Ó 2010 COSPAR. Published by Elsevier Ltd. All rights reserved. doi:10.1016/j.asr.2010.01.011
H. Guo et al. / Advances in Space Research 45 (2010) 1350–1357
NGS, USA), and a carrier-phase method. Magnetic data were preprocessed by our own developed analyzing method before it can be integrated into a GPS/IMU/magnetometer system. In Kalman filtering (KF) calculation, the observations are the position, velocity, and acceleration (PVA) outputs from both methods and the differences between GPS headings and the magnetic headings with the magnetic declination reduction. The observation equation for GPS and magnetic headings was derived specially, and composed of the horizontal magnetic components. And the observation noises were derived by both GPS outputs and magnetic heading errors. The dynamical equation used the constant acceleration model with the rate of the acceleration as the model error. Two results from both differential GPS (DGPS) method and the carrier-phase method have been obtained. Due to the drawback of deriving velocities/accelerations directly from the position differentiation, the DGPS method has a lower accurate velocity and acceleration solution. However, a carrier-phase method based on the differentiation of the carrier-phase observations and satellites’ positions overcame the above drawback (Serrano et al., 2004). Also Wang et al. (2007) reported that a 10 m error of the user’s position caused a 1.6 mm/s error of the range rate between the user and the satellite only. The method has used the IGS satellite orbit’s SP3 file for calculating the vehicle’s velocities and accelerations. In this case, the carrier-phase method got more accurate velocities and accelerations. In fact, GPS carrier-phase method for calculating the vehicle’s velocities and accelerations has been presented by Serrano et al. (2004) and Kennedy (2002). It was proven that the accuracy of the vehicle’s velocity and acceleration obtained with the carrier-phase method is at the level of several mm/s or mm/s2 both in static test and kinematical test on the car. Specially, it is a high accurate velocity/ acceleration calculation method suitable for real time computation when the broadcasting ephemeris is used. In this paper, an IGS final orbit (SP3 file) has been used for calculating the vehicle’s velocity and acceleration. And few mm/ s and mm/s2 standard deviation accuracy has been got
1351
again. In addition, the heading reliability has been improved since magnetic outputs have been combined with GPS headings. That made it possible for the magnetic heading with accelerometer data to bridge the GPS gaps with the help of Kalman filtering to obtain the continuous navigation. 2. Experimental details The practical field test was performed near UNB, Fredericton, NB, Canada. A field test for a GPS/IMU integrated system included collecting GPS raw data and IMU outputs of gyros, accelerometers, and magnetometers, which followed the below procedure: (1) drive a car along two circles on the parking plot for IMU magnetometers hard iron calibration and computation, (2) drive a car in the streets for about 10 min to calculating the trajectory and analyzing the results. A car-based GPS/IMU integrated system includes the following items: (1) two GPS receivers and antennas, (2) 3DM-GX1 IMU device, (3) a laptop with at least three interfaces, (4) a power converter from car to all above equipments, and (5) a shelf to hold all above equipments on the top of a car. Two GPS receivers and magnetometers were installed on the car. Car test trajectory and setting up an IMU/electronic compass magnetometer between two GPS antennae are shown in Fig. 1. The whole test trajectory included a static period of about 51 s, a calibration period, and a low speed driving along the streets. During the static period, the standard deviations of GPS coordinates, headings, and magnetic headings are obtained. During the calibration period of 43 s, the car circled twice for the magnetic hard and soft iron compensation calculation. Then, the car was driven down the road about 2652 m at a rate of several meters to a dozen meters with the average velocity of 4.6 m/s. 3. State equations The state equation we used for the vehicle test is a socalled constant acceleration model as follows:
Fig. 1. Car test trajectory and setting up an IMU/electronic compass magnetometer between two GPS antennae.
1352
H. Guo et al. / Advances in Space Research 45 (2010) 1350–1357
1 1 pkþ1 ¼ pk þ Dt vk þ Dt2 ak þ Dt3 J 2 6 1 2 vkþ1 ¼ vk þ Dt ak þ Dt J 2 akþ1 ¼ ak þ Dt J
ð1Þ
bkþ1 ¼ bk þ Bkþ1 Bk þ wb
4. Observation equations
where p is the position vector with three components of north, east, and down; v is the velocity vector, and a for the acceleration vector; k is the time point, and Dt for the time interval from k to k + 1 points (= 1 s in this case); b = [bx, by] is a predicting vector of the magnetic horizontal components; J, Jerk = Da, is the rate of the acceleration as the part of model error, which is described as a vector equal to wa = [1/6 (Jn), 1/6 (Je), 1/6 (Jd), 1/2 (Jn), 1/2 (Je), 1/2 (Jd), Jn, Je, Jd]T, while Bx, By are the outputs of the magnetometer, and wb = [0.0012, 0.0012]T Gauss for the magnetic components’ noises, wk = [wa, wb]T a vector of model noises; n, e, d are coordinate variables of north, east, down; vn, ve, vd are velocity variables of north, east, down; an, ae, ad are accelerations. So the matrix expression for state equation is as follows:
3 n 6 e7 6 7 6 7 6d 7 6 7 6v 7 6 n7 6 7 6 ve 7 6 7 6 7 6 vd 7 6 7 6 an 7 6 7 6 7 6 ae 7 6 7 6 ad 7 6 7 6 7 4 bx 5 2
by
2
kþ1
1 60 6 6 60 6 60 6 6 60 6 6 ¼ 60 6 60 6 6 60 6 60 6 6 40
0 0
1
0 0
0:5
0
0
0
1 0
0
1 0
0
0:5
0
0
0 1
0
0 1
0
0
0 0
1
0 0
1
0
0
0
0 0
0
1 0
0
1
0
0
0 0 0 0
0 0
0 1 0 0
0 1
0 0
1 0
0 0
0 0
0
0 0
0
1
0
0
0 0
0
0 0
0
0
1
0
0 0
0
0 0
0
0
0
1
0
0 0
0
0 0
0
0
0
0
0:5 0
1 varðJ n Þ 20
6 0 6 6 6 0 6 6 1 varðJ Þ 68 n 6 0 Qk ¼ 6 6 6 0 6 61 6 varðJ n Þ 66 6 0 4 0
1 varðJ n Þ 8
0
1 varðJ e Þ 20
0
0
0
1 varðJ d Þ 20
0
0
Am Agps þ 18:2 By Bx ¼ bx b y þ e h h q
ð4Þ B
which is derived by the equation tan Am ¼ Byx , where Agps is GPS heading (unit: degrees); Am is the magnetic heading; the magnetic declination = 18.2° at the test time, q = 180/ p; Bx, By are the outputs of the magnetometer in the horizontal plane at the time point k; h ¼ B2x þ B2y ; Bx and By are the predicted magnetic components; ek is the observation qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi2 ðAm Agps Þ ¼ 1:5 , noise, standard deviation (std) ðek Þ ¼ n which comes from the real data analysis in the paper of Guo et al. (2008). And GPS results of positions, velocities,
3 2
0
0
The observation equations for this vehicle test included the relation between the GPS and magnetic heading as follows:
3 2 0 n 7 7 6 6 0 07 6 e 7 6 7 6 7 6 6d 7 6 0 07 7 6 7 6 6 7 6 0 07 7 6 vn 7 6 7 6 7 6 6 ve 7 6 0 07 7 6 7 6 7 6 7 6 0 0 7 6 vd 7 þ 6 7 6 7 6 6 an 7 6 0 07 7 6 7 6 7 6 7 6 0 0 7 6 ae 7 6 7 6 7 6 6 ad 7 6 0 07 7 6 7 6 7 6 7 6 4 Bxkþ1 Bxk 0 5 4 bx 5 Bykþ1 By k by k 1 0
The error covariance matrices for the constant acceleration model designed by Jin (1996), and magnetic components are:
2
where var(J) = var(a)/sqrt (2), since a central differentiation has been applied; the magnetic component J ðkÞ ¼ aðkþ1Þaðk1Þ 2 noises Qbx, Qby come from the specification of Microstrain 3 DM-GX1.
3 7 7 7 7 7 7 7 7 7 7 7 7 þ wk 7 7 7 7 7 7 7 7 7 5
ð2Þ
accelerations, and magnetic components are used as observations as well.
0
1 varðJ n Þ 6
0
1 varðJ e Þ 8
0
0
1 varðJ e Þ 6
0
0
1 varðJ d Þ 8
0
0
1 varðJ n Þ 3
0
0
1 varðJ n Þ 2
0
1 varðJ e Þ 8
0
0
1 varðJ e Þ 3
0
0
1 varðJ e Þ 2
0 0
1 varðjd Þ 8
0 0
1 varðjd Þ 3
0
0 1 varðjn Þ 2
0
0 varðJ n Þ
0 0
1 varðJ e Þ 6
0
0
1 varðJ e Þ 2
0
0
varðJ e Þ
0
1 varðJ d Þ 6
0
0
1 varðJ d Þ 2
0
0
0 0
3 7 7 7
1 varðJ d Þ 7 6 7
7 7 7 7 0 7 7 1 varðjd Þ 7 2 7 7 0 7 7 0 5 varðJ d Þ ;Q
bk ¼
ð3Þ
"
Qbx
0
0
Qby
#
H. Guo et al. / Advances in Space Research 45 (2010) 1350–1357
1353
getting the satellites’ velocities and accelerations. It generated the measurement velocities (rates) and accelerations from the differentiation of the carrier-phase measurements. Specially, it required lower accurate users’ positions, and can get users’ velocities and accelerations of several mm/ s, mm/s2 level, respectively. The basic equations are U_ uu ¼ huu ðvu vu Þ þ B_ u þ eU_ € u ¼ h_ u ðvu vu Þ þ hu ðau au Þ þ B € u þ eU€ U u u u
Fig. 2. Standard deviations of the vehicle position, velocity, and acceleration using KINPOS after KF.
Table 1 Steady standard deviations for the vehicle using KINPOS after KF. Direction
North
East
Down
Position (mm) Velocity (mm/s) Acceleration (mm/s2)
9.0 5.7 4.4
6.5 4.0 3.2
11.8 7.4 6.0
5. KF results based on KINPOS outputs The vehicle positions, velocities, accelerations, the GPS headings derived by the KINPOS, and magnetic headings are input into a Kalman filter, which has detailedly been described above. Through the software output, std (Jn) = 0.0049 m/s3, std (Je) = 0.0035 m/s3, std (Jd) = 0.0064 m/s3 have been obtained. The output results are plotted and tabulated in Fig. 2 and Table 1. The accuracies of positions, velocities, and accelerations are improved after KF. 6. GPS carrier-phase method outputs A GPS carrier-phase method has been explained by Serrano et al. (2004) and Kennedy (2002) in detail. The carrier-phase method was used for obtaining the vehicle velocity and acceleration, which has applied the central differentiation to the satellites’ positions and velocities for
ð5Þ
where U_ uu is the carrier-phase differential value (rate value), _ UðkÞ ¼ Uðkþ1ÞUðk1Þ ; huu is the unit direction vector between 2 u u ðkÞ is the directional the user and the satellite; huu ðkÞ ¼ x ðkÞx ruu ðkÞ cosine vector as well; xs, xu, ruu are, respectively, the satellite coordinates, the user coordinates which were got by an approximate GPS solution, and the geometric range between the satellite and user; vs the satellite velocity vector; vu the user velocity vector; B_ u the user’s receiver € u is clock drift; eU_ the differential carrier-phase noise; U u _Uðkþ1ÞUðk1Þ _ € ; the carrier-phase acceleration value, UðkÞ ¼ 2 _hðkÞ ¼ hðkþ1Þhðk1Þ; au the satellite acceleration vector; au 2 € u the user’s receiver clock the user acceleration vector; B drift rate; eU€ the phase acceleration noise. In Eq. (5), vec€ u are to be predicted, all others are tors vu, B_ u and au, B the known (see Serrano et al. (2004) and Kennedy (2002) for details). As the unknown parameters, the user’s velocities and accelerations have been estimated by a least-squares _ € method. That is, vu Bu , au Bu can be got by calculating differential carrier-phase observation values, while the unit direction vector, the satellites’ velocities and accelerations in Eq. (5) come from a GPS approximate solution and satellites’ coordinates in an IGS SP3 file with a Lagrange interpolating polynomial. Here are the results for the vehicle velocities and accelerations predicted and compared with the KINPOS outputs in Figs. 3 and 4 as follows. Standard deviations of the velocity and acceleration of the vehicle are listed in Tables 2 and 3. The static values were got by the true value – zero directly when the car did not move. The kinematical values were got by comparing with the KINPOS results, which included errors of both the carrier-phase method and KINPOS. 7. KF results based on GPS carrier-phase method outputs In this section, the carrier-phase method results with the combination of the GPS/magnetic heading data are only used as the inputs to perform a Kalman filtering for improving the vehicle’s PVA. The GPS carrier-phase method’s PVA outputs have been got by the above section. As known std (Jn) = 0.0035 m/s3, std (Je) = 0.0032 m/s3, std (Jd) = 0.0053 m/s3. After KF, the results have been improved as Fig. 5 and Table 4 shows.
1354
H. Guo et al. / Advances in Space Research 45 (2010) 1350–1357
Fig. 3. The vehicle velocity differences between the carrier-phase method and KINPOS.
Supposing having 1 m position errors in the observation equation before KF, the position accuracy of 0.1 m has still been got after Kalman filtering (see Fig. 6, north, east, and
down direction std are similar, and go down from 1 m to 0.1 m). That verified that the vehicle’s position accuracy has greatly been improved in our research.
H. Guo et al. / Advances in Space Research 45 (2010) 1350–1357
Fig. 4. Acceleration differences between the carrier-phase method and KINPOS.
1355
1356
H. Guo et al. / Advances in Space Research 45 (2010) 1350–1357
Table 2 Standard deviations of the velocity (mm/s). Standard deviation of velocities
North
East
Down
Static test period Kinematical test period compared with KINPOS software results
2.1 7.0
1.3 6.9
3.7 9.7
Table 3 Standard deviations of acceleration (mm/s2). Standard deviation of accelerations
North
East
Down
Static test period Kinematical test period compared with KINPOS software results
1.5 5.0
0.9 4.5
2.3 7.5
Fig. 6. Position standard deviation upgraded after KF.
GPS gap
North
East Fig. 7. Vehicle trajectory with a GPS outage.
8. Demonstration of magnetometer data bridging the GPS outage
Fig. 5. Standard deviations for the vehicle position, velocity, and acceleration using carrier-phase method after KF.
Table 4 Steady standard deviations of the vehicle using carrier-phase method after KF. Direction
North
East
Down
Position (mm) Velocity (mm/s) Acceleration (mm/s2)
8.2 4.2 3.2
6.5 3.9 2.9
10.8 6.0 4.7
In our vehicle test, there is a GPS outage due to the leaves shading of the trees along the road shown in Fig. 7. We used the magnetometer data and accelerometer data to dead reckon the trajectory shown in Fig. 7. The gyro data were not used since they drifted very fast, instead the magnetic headings and twofold integrations of accelerations from the accelerometers were calculated to get the vehicle coordinates. The results show that the magnetometer/accelerometer subsystem bridged GPS gaps very well in Fig. 8, and demonstrated the benefits of using magnetometer and acceleration data. 9. Conclusions In the vehicle navigation solution, the magnetic headings with accelerometer deriving distances bridged the GPS gap. It demonstrated the benefits of using the magnetic data during GPS signal outages very well.
H. Guo et al. / Advances in Space Research 45 (2010) 1350–1357
1357
Acknowledgements
North
The paper was funded by International Cooperation Project of Jiangxi Provincial Department of Science & Technology, No. 2009BHA18300; NSF of China Project, No. 40774005; Jiangxi NSF, No. 2007gzw2077; and National Basic Research Program, No. 2006CB303006. Thanks very much to Prof. Richard B. Langley, Dr. Dong Hyun Kim, and Dr. Marcelo Santos at Department of Geodesy and Geomatics Engineering, UNB, Fredericton, N.B. for their advices and suggestions to this research, and for helping completing the field test on a car. Thanks also to anonymous reviewers’ comments and advices, editor in Chief Peggy Ann Shea. East
Fig. 8. Magnetic headings and accelerometer data bridging the GPS outage in the vehicle trajectory.
Specially the carefully designed state equations and observation equations for the GPS/IMU/magnetometer vehicle navigation was successfully integrated into a Kalman filter. The noise co-variances of both the system model and observation equations were calculated based on real GPS/magnetometer data error analysis or calibration calculation (Guo et al., 2008; Jin, 1996; Gebre-Egziabher et al., 2001). The carrier-phase method, including Lagrange interpolating method for satellite positions from a precise orbit’s SP3 file, and the central differentiation method, is successful for obtaining the vehicle’s navigation parameters in this paper. All velocity and acceleration results predicted are at the level of several mm/s or mm/s2 finally. Kalman filtering has improved the accuracy of both DGPS and the carrier-phase method results. The standard deviations of PVA obtained are average about 50–60% of that before KF. Through upgrading the position’s accuracy from 1 m to 0.1 m by KF, the carrier-phase method investigated here shown its advantage, and is suitable for the precise point position (PPP) and other GPS vehicle navigation situations.
References Amundson, M. Compass Assisted GPS for LBS Applications, Honeywell, http://www.honeywell.com/magneticsensors, Ó2006 Honeywell International Inc., 2006. Caruso, M.J. Application of magnetic sensors for low cost compass systems. Available from:, March 9, 2007. Gebre-Egziabher, D., Elkaim, G.H., Powell, J.D., Parkinson, B.W. A non-linear, two-step estimation algorithm for calibrating solid-state strapdown magnetometer, in: Proceedings of the of the International Conference on Integrated Navigation Systems, St. Petersburg, Russia, pp. 290–297, May 2001. Guo, H., Kim, D., Langley, R. A new method for electronic compass magnetometer calibration for improving azimuth calculation in magnetometer/GPS integrated system, in: ION GNSS 2008, Savannah, Georgia, USA, pp. 1641–1648, September 16–19, 2008. Jin, X.X. Theory of Carrier Adjusted DGPS Positioning Approach and Some Experimental Results. Delft University Press, 1996. Kennedy, S. Precise acceleration determination from carrier phase measurements, in: Proceedings of the ION GPS 2002, Portland, Oregon, pp. 962–972, September 2002. Langley, R.B. Getting your bearings: the magnetic compass and GPS, Innovation, GPS world, pp. 70–72, 74, 76, 78, 80, September 2003. Serrano, L., Kim, D., Langley, R.B. A single GPS receiver as a real-time, accurate velocity and acceleration sensor, in: Proceedings of the ION GNSS 2004, 17th International Technical Meeting of The Institute of Navigation, Long Beach, CA, pp. 2021–2034, September 2004. Wang, F.H. Zhang, X.H., Huang, J.S. Accuracy analysis of GPS PPP derived velocity after SA, in: Proceedings of the Ninth GNSS Annual Meeting of China, Beijing (in Chinese), pp. 63–70, November 2007.
Advances in Space Research 45 (2010) 1350–1357 www.elsevier.com/locate/asr
Kalman filtering for GPS/magnetometer integrated navigation system Hang Guo a,*, Min Yu b, Chengwu Zou b, Wenwen Huang b b
a Nanchang University, Nanchang 330031, Jiangxi, China Jiangxi Normal University, Nanchang 330022, Jiangxi, China
Received 14 August 2008; received in revised form 6 January 2010; accepted 11 January 2010
Abstract This paper investigated the data processing method for a GPS/IMU/magnetometer integrated system with Kalman filtering (KF). As a result of GPS/IMU/magnetometer land vehicle system, dead-reckoning of magnetometer and accelerometer integrated subsystem bridged very well the GPS signal outage due to the trees on the two sides of the road. Both differential GPS data processing method and the carrier-phase method with magnetometers’ outputs for predicting the car position, velocity, and acceleration (PVA) are presented. The results from DGPS with Kinematical Positioning (KINPOS) software shown that the averages of the north, east, and down direction standard deviation (short for “std”) are 0.014, 0.010, and 0.018 m, respectively. The std of velocities and accelerations derived by the position and velocity differentiation are 10, 7, 13 mm/s, 7, 5, 9 mm/s2, respectively. This method for getting velocities and accelerations requires higher accurate position coordinates. But the position accuracy has frequently been degraded in this case when the car drove under the trees or other similar kinematical environments. That caused the larger velocity and acceleration errors. While the results from the carrier-phase method are std of the velocities = 2.1 mm/s, 1.3 mm/s, 3.7 mm/s in north, east, down, and std of the accelerations = 1.5 mm/s2, 0.9 mm/s2, 2.3 mm/s2 for the static test period; as compared with KINPOS software results, std of the velocity difference between the carrier-phase method and the DGPS method = 7 mm/s, 6.9 mm/s, 9.7 mm/s in north, east, down direction, and std of acceleration difference = 5.0 mm/s2, 4.5 mm/s2, 7.5 mm/s2 in north, east, down direction for the kinematical test period. Obviously, errors come from both the carrier-phase method and DGPS velocity and acceleration results derived directly by the position differentiation. In addition, better accuracy of positions than that before KF has been got by means of velocities and accelerations derived by the carrier-phase method after KF. Ó 2010 COSPAR. Published by Elsevier Ltd. All rights reserved. Keywords: DGPS; IMU; Magnetometer; Accelerometer; Carrier-phase method; Kalman filtering
1. Introduction There are different kinds of integrated navigation systems for a variety of applications, such as GPS/IMU, pseudollite, magnetometer, odometer, etc (Langley, 2003). GPS/magnetometer integrated system has the advantages of magnetometer’s low cost, GPS high accuracy, and mutual benefits on the vehicle orientation determinations. Amundson (2006) presented the compass assisted GPS for location-based services (LBS) applications. When GPS is not available for LBS, the electronic compass will provide the direction, and MEMS accelerometers will be *
Corresponding author. Tel.: +86 791 2941198. E-mail address: [email protected] (H. Guo).
a kind of odometer to measure the distances along the route. Honeywell’s dead reckoning systems has good location accuracy, and can be used in the halls or inside buildings (Caruso, 2007). In this project, we tested two GPS receivers and a threeaxis magnetometer/three-axis accelerometer integrated system (MicroStrain 3DM-GX1) on a car near University of New Brunswick (UNB), Canada. GPS receivers are lined as a baseline on the top of car to measure the car orientation as the reference, while the magnetometers were put in the center of the short baseline (1.22 m), which measures the car orientation as well. Both GPS and magnetometer’s data were collected. GPS data was processed by DGPS with Kinematical Positioning (KINPOS) software (by Dr. Gerald Mader, Chief, Geosciences, Research Division,
0273-1177/$36.00 Ó 2010 COSPAR. Published by Elsevier Ltd. All rights reserved. doi:10.1016/j.asr.2010.01.011
H. Guo et al. / Advances in Space Research 45 (2010) 1350–1357
NGS, USA), and a carrier-phase method. Magnetic data were preprocessed by our own developed analyzing method before it can be integrated into a GPS/IMU/magnetometer system. In Kalman filtering (KF) calculation, the observations are the position, velocity, and acceleration (PVA) outputs from both methods and the differences between GPS headings and the magnetic headings with the magnetic declination reduction. The observation equation for GPS and magnetic headings was derived specially, and composed of the horizontal magnetic components. And the observation noises were derived by both GPS outputs and magnetic heading errors. The dynamical equation used the constant acceleration model with the rate of the acceleration as the model error. Two results from both differential GPS (DGPS) method and the carrier-phase method have been obtained. Due to the drawback of deriving velocities/accelerations directly from the position differentiation, the DGPS method has a lower accurate velocity and acceleration solution. However, a carrier-phase method based on the differentiation of the carrier-phase observations and satellites’ positions overcame the above drawback (Serrano et al., 2004). Also Wang et al. (2007) reported that a 10 m error of the user’s position caused a 1.6 mm/s error of the range rate between the user and the satellite only. The method has used the IGS satellite orbit’s SP3 file for calculating the vehicle’s velocities and accelerations. In this case, the carrier-phase method got more accurate velocities and accelerations. In fact, GPS carrier-phase method for calculating the vehicle’s velocities and accelerations has been presented by Serrano et al. (2004) and Kennedy (2002). It was proven that the accuracy of the vehicle’s velocity and acceleration obtained with the carrier-phase method is at the level of several mm/s or mm/s2 both in static test and kinematical test on the car. Specially, it is a high accurate velocity/ acceleration calculation method suitable for real time computation when the broadcasting ephemeris is used. In this paper, an IGS final orbit (SP3 file) has been used for calculating the vehicle’s velocity and acceleration. And few mm/ s and mm/s2 standard deviation accuracy has been got
1351
again. In addition, the heading reliability has been improved since magnetic outputs have been combined with GPS headings. That made it possible for the magnetic heading with accelerometer data to bridge the GPS gaps with the help of Kalman filtering to obtain the continuous navigation. 2. Experimental details The practical field test was performed near UNB, Fredericton, NB, Canada. A field test for a GPS/IMU integrated system included collecting GPS raw data and IMU outputs of gyros, accelerometers, and magnetometers, which followed the below procedure: (1) drive a car along two circles on the parking plot for IMU magnetometers hard iron calibration and computation, (2) drive a car in the streets for about 10 min to calculating the trajectory and analyzing the results. A car-based GPS/IMU integrated system includes the following items: (1) two GPS receivers and antennas, (2) 3DM-GX1 IMU device, (3) a laptop with at least three interfaces, (4) a power converter from car to all above equipments, and (5) a shelf to hold all above equipments on the top of a car. Two GPS receivers and magnetometers were installed on the car. Car test trajectory and setting up an IMU/electronic compass magnetometer between two GPS antennae are shown in Fig. 1. The whole test trajectory included a static period of about 51 s, a calibration period, and a low speed driving along the streets. During the static period, the standard deviations of GPS coordinates, headings, and magnetic headings are obtained. During the calibration period of 43 s, the car circled twice for the magnetic hard and soft iron compensation calculation. Then, the car was driven down the road about 2652 m at a rate of several meters to a dozen meters with the average velocity of 4.6 m/s. 3. State equations The state equation we used for the vehicle test is a socalled constant acceleration model as follows:
Fig. 1. Car test trajectory and setting up an IMU/electronic compass magnetometer between two GPS antennae.
1352
H. Guo et al. / Advances in Space Research 45 (2010) 1350–1357
1 1 pkþ1 ¼ pk þ Dt vk þ Dt2 ak þ Dt3 J 2 6 1 2 vkþ1 ¼ vk þ Dt ak þ Dt J 2 akþ1 ¼ ak þ Dt J
ð1Þ
bkþ1 ¼ bk þ Bkþ1 Bk þ wb
4. Observation equations
where p is the position vector with three components of north, east, and down; v is the velocity vector, and a for the acceleration vector; k is the time point, and Dt for the time interval from k to k + 1 points (= 1 s in this case); b = [bx, by] is a predicting vector of the magnetic horizontal components; J, Jerk = Da, is the rate of the acceleration as the part of model error, which is described as a vector equal to wa = [1/6 (Jn), 1/6 (Je), 1/6 (Jd), 1/2 (Jn), 1/2 (Je), 1/2 (Jd), Jn, Je, Jd]T, while Bx, By are the outputs of the magnetometer, and wb = [0.0012, 0.0012]T Gauss for the magnetic components’ noises, wk = [wa, wb]T a vector of model noises; n, e, d are coordinate variables of north, east, down; vn, ve, vd are velocity variables of north, east, down; an, ae, ad are accelerations. So the matrix expression for state equation is as follows:
3 n 6 e7 6 7 6 7 6d 7 6 7 6v 7 6 n7 6 7 6 ve 7 6 7 6 7 6 vd 7 6 7 6 an 7 6 7 6 7 6 ae 7 6 7 6 ad 7 6 7 6 7 4 bx 5 2
by
2
kþ1
1 60 6 6 60 6 60 6 6 60 6 6 ¼ 60 6 60 6 6 60 6 60 6 6 40
0 0
1
0 0
0:5
0
0
0
1 0
0
1 0
0
0:5
0
0
0 1
0
0 1
0
0
0 0
1
0 0
1
0
0
0
0 0
0
1 0
0
1
0
0
0 0 0 0
0 0
0 1 0 0
0 1
0 0
1 0
0 0
0 0
0
0 0
0
1
0
0
0 0
0
0 0
0
0
1
0
0 0
0
0 0
0
0
0
1
0
0 0
0
0 0
0
0
0
0
0:5 0
1 varðJ n Þ 20
6 0 6 6 6 0 6 6 1 varðJ Þ 68 n 6 0 Qk ¼ 6 6 6 0 6 61 6 varðJ n Þ 66 6 0 4 0
1 varðJ n Þ 8
0
1 varðJ e Þ 20
0
0
0
1 varðJ d Þ 20
0
0
Am Agps þ 18:2 By Bx ¼ bx b y þ e h h q
ð4Þ B
which is derived by the equation tan Am ¼ Byx , where Agps is GPS heading (unit: degrees); Am is the magnetic heading; the magnetic declination = 18.2° at the test time, q = 180/ p; Bx, By are the outputs of the magnetometer in the horizontal plane at the time point k; h ¼ B2x þ B2y ; Bx and By are the predicted magnetic components; ek is the observation qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi2 ðAm Agps Þ ¼ 1:5 , noise, standard deviation (std) ðek Þ ¼ n which comes from the real data analysis in the paper of Guo et al. (2008). And GPS results of positions, velocities,
3 2
0
0
The observation equations for this vehicle test included the relation between the GPS and magnetic heading as follows:
3 2 0 n 7 7 6 6 0 07 6 e 7 6 7 6 7 6 6d 7 6 0 07 7 6 7 6 6 7 6 0 07 7 6 vn 7 6 7 6 7 6 6 ve 7 6 0 07 7 6 7 6 7 6 7 6 0 0 7 6 vd 7 þ 6 7 6 7 6 6 an 7 6 0 07 7 6 7 6 7 6 7 6 0 0 7 6 ae 7 6 7 6 7 6 6 ad 7 6 0 07 7 6 7 6 7 6 7 6 4 Bxkþ1 Bxk 0 5 4 bx 5 Bykþ1 By k by k 1 0
The error covariance matrices for the constant acceleration model designed by Jin (1996), and magnetic components are:
2
where var(J) = var(a)/sqrt (2), since a central differentiation has been applied; the magnetic component J ðkÞ ¼ aðkþ1Þaðk1Þ 2 noises Qbx, Qby come from the specification of Microstrain 3 DM-GX1.
3 7 7 7 7 7 7 7 7 7 7 7 7 þ wk 7 7 7 7 7 7 7 7 7 5
ð2Þ
accelerations, and magnetic components are used as observations as well.
0
1 varðJ n Þ 6
0
1 varðJ e Þ 8
0
0
1 varðJ e Þ 6
0
0
1 varðJ d Þ 8
0
0
1 varðJ n Þ 3
0
0
1 varðJ n Þ 2
0
1 varðJ e Þ 8
0
0
1 varðJ e Þ 3
0
0
1 varðJ e Þ 2
0 0
1 varðjd Þ 8
0 0
1 varðjd Þ 3
0
0 1 varðjn Þ 2
0
0 varðJ n Þ
0 0
1 varðJ e Þ 6
0
0
1 varðJ e Þ 2
0
0
varðJ e Þ
0
1 varðJ d Þ 6
0
0
1 varðJ d Þ 2
0
0
0 0
3 7 7 7
1 varðJ d Þ 7 6 7
7 7 7 7 0 7 7 1 varðjd Þ 7 2 7 7 0 7 7 0 5 varðJ d Þ ;Q
bk ¼
ð3Þ
"
Qbx
0
0
Qby
#
H. Guo et al. / Advances in Space Research 45 (2010) 1350–1357
1353
getting the satellites’ velocities and accelerations. It generated the measurement velocities (rates) and accelerations from the differentiation of the carrier-phase measurements. Specially, it required lower accurate users’ positions, and can get users’ velocities and accelerations of several mm/ s, mm/s2 level, respectively. The basic equations are U_ uu ¼ huu ðvu vu Þ þ B_ u þ eU_ € u ¼ h_ u ðvu vu Þ þ hu ðau au Þ þ B € u þ eU€ U u u u
Fig. 2. Standard deviations of the vehicle position, velocity, and acceleration using KINPOS after KF.
Table 1 Steady standard deviations for the vehicle using KINPOS after KF. Direction
North
East
Down
Position (mm) Velocity (mm/s) Acceleration (mm/s2)
9.0 5.7 4.4
6.5 4.0 3.2
11.8 7.4 6.0
5. KF results based on KINPOS outputs The vehicle positions, velocities, accelerations, the GPS headings derived by the KINPOS, and magnetic headings are input into a Kalman filter, which has detailedly been described above. Through the software output, std (Jn) = 0.0049 m/s3, std (Je) = 0.0035 m/s3, std (Jd) = 0.0064 m/s3 have been obtained. The output results are plotted and tabulated in Fig. 2 and Table 1. The accuracies of positions, velocities, and accelerations are improved after KF. 6. GPS carrier-phase method outputs A GPS carrier-phase method has been explained by Serrano et al. (2004) and Kennedy (2002) in detail. The carrier-phase method was used for obtaining the vehicle velocity and acceleration, which has applied the central differentiation to the satellites’ positions and velocities for
ð5Þ
where U_ uu is the carrier-phase differential value (rate value), _ UðkÞ ¼ Uðkþ1ÞUðk1Þ ; huu is the unit direction vector between 2 u u ðkÞ is the directional the user and the satellite; huu ðkÞ ¼ x ðkÞx ruu ðkÞ cosine vector as well; xs, xu, ruu are, respectively, the satellite coordinates, the user coordinates which were got by an approximate GPS solution, and the geometric range between the satellite and user; vs the satellite velocity vector; vu the user velocity vector; B_ u the user’s receiver € u is clock drift; eU_ the differential carrier-phase noise; U u _Uðkþ1ÞUðk1Þ _ € ; the carrier-phase acceleration value, UðkÞ ¼ 2 _hðkÞ ¼ hðkþ1Þhðk1Þ; au the satellite acceleration vector; au 2 € u the user’s receiver clock the user acceleration vector; B drift rate; eU€ the phase acceleration noise. In Eq. (5), vec€ u are to be predicted, all others are tors vu, B_ u and au, B the known (see Serrano et al. (2004) and Kennedy (2002) for details). As the unknown parameters, the user’s velocities and accelerations have been estimated by a least-squares _ € method. That is, vu Bu , au Bu can be got by calculating differential carrier-phase observation values, while the unit direction vector, the satellites’ velocities and accelerations in Eq. (5) come from a GPS approximate solution and satellites’ coordinates in an IGS SP3 file with a Lagrange interpolating polynomial. Here are the results for the vehicle velocities and accelerations predicted and compared with the KINPOS outputs in Figs. 3 and 4 as follows. Standard deviations of the velocity and acceleration of the vehicle are listed in Tables 2 and 3. The static values were got by the true value – zero directly when the car did not move. The kinematical values were got by comparing with the KINPOS results, which included errors of both the carrier-phase method and KINPOS. 7. KF results based on GPS carrier-phase method outputs In this section, the carrier-phase method results with the combination of the GPS/magnetic heading data are only used as the inputs to perform a Kalman filtering for improving the vehicle’s PVA. The GPS carrier-phase method’s PVA outputs have been got by the above section. As known std (Jn) = 0.0035 m/s3, std (Je) = 0.0032 m/s3, std (Jd) = 0.0053 m/s3. After KF, the results have been improved as Fig. 5 and Table 4 shows.
1354
H. Guo et al. / Advances in Space Research 45 (2010) 1350–1357
Fig. 3. The vehicle velocity differences between the carrier-phase method and KINPOS.
Supposing having 1 m position errors in the observation equation before KF, the position accuracy of 0.1 m has still been got after Kalman filtering (see Fig. 6, north, east, and
down direction std are similar, and go down from 1 m to 0.1 m). That verified that the vehicle’s position accuracy has greatly been improved in our research.
H. Guo et al. / Advances in Space Research 45 (2010) 1350–1357
Fig. 4. Acceleration differences between the carrier-phase method and KINPOS.
1355
1356
H. Guo et al. / Advances in Space Research 45 (2010) 1350–1357
Table 2 Standard deviations of the velocity (mm/s). Standard deviation of velocities
North
East
Down
Static test period Kinematical test period compared with KINPOS software results
2.1 7.0
1.3 6.9
3.7 9.7
Table 3 Standard deviations of acceleration (mm/s2). Standard deviation of accelerations
North
East
Down
Static test period Kinematical test period compared with KINPOS software results
1.5 5.0
0.9 4.5
2.3 7.5
Fig. 6. Position standard deviation upgraded after KF.
GPS gap
North
East Fig. 7. Vehicle trajectory with a GPS outage.
8. Demonstration of magnetometer data bridging the GPS outage
Fig. 5. Standard deviations for the vehicle position, velocity, and acceleration using carrier-phase method after KF.
Table 4 Steady standard deviations of the vehicle using carrier-phase method after KF. Direction
North
East
Down
Position (mm) Velocity (mm/s) Acceleration (mm/s2)
8.2 4.2 3.2
6.5 3.9 2.9
10.8 6.0 4.7
In our vehicle test, there is a GPS outage due to the leaves shading of the trees along the road shown in Fig. 7. We used the magnetometer data and accelerometer data to dead reckon the trajectory shown in Fig. 7. The gyro data were not used since they drifted very fast, instead the magnetic headings and twofold integrations of accelerations from the accelerometers were calculated to get the vehicle coordinates. The results show that the magnetometer/accelerometer subsystem bridged GPS gaps very well in Fig. 8, and demonstrated the benefits of using magnetometer and acceleration data. 9. Conclusions In the vehicle navigation solution, the magnetic headings with accelerometer deriving distances bridged the GPS gap. It demonstrated the benefits of using the magnetic data during GPS signal outages very well.
H. Guo et al. / Advances in Space Research 45 (2010) 1350–1357
1357
Acknowledgements
North
The paper was funded by International Cooperation Project of Jiangxi Provincial Department of Science & Technology, No. 2009BHA18300; NSF of China Project, No. 40774005; Jiangxi NSF, No. 2007gzw2077; and National Basic Research Program, No. 2006CB303006. Thanks very much to Prof. Richard B. Langley, Dr. Dong Hyun Kim, and Dr. Marcelo Santos at Department of Geodesy and Geomatics Engineering, UNB, Fredericton, N.B. for their advices and suggestions to this research, and for helping completing the field test on a car. Thanks also to anonymous reviewers’ comments and advices, editor in Chief Peggy Ann Shea. East
Fig. 8. Magnetic headings and accelerometer data bridging the GPS outage in the vehicle trajectory.
Specially the carefully designed state equations and observation equations for the GPS/IMU/magnetometer vehicle navigation was successfully integrated into a Kalman filter. The noise co-variances of both the system model and observation equations were calculated based on real GPS/magnetometer data error analysis or calibration calculation (Guo et al., 2008; Jin, 1996; Gebre-Egziabher et al., 2001). The carrier-phase method, including Lagrange interpolating method for satellite positions from a precise orbit’s SP3 file, and the central differentiation method, is successful for obtaining the vehicle’s navigation parameters in this paper. All velocity and acceleration results predicted are at the level of several mm/s or mm/s2 finally. Kalman filtering has improved the accuracy of both DGPS and the carrier-phase method results. The standard deviations of PVA obtained are average about 50–60% of that before KF. Through upgrading the position’s accuracy from 1 m to 0.1 m by KF, the carrier-phase method investigated here shown its advantage, and is suitable for the precise point position (PPP) and other GPS vehicle navigation situations.
References Amundson, M. Compass Assisted GPS for LBS Applications, Honeywell, http://www.honeywell.com/magneticsensors, Ó2006 Honeywell International Inc., 2006. Caruso, M.J. Application of magnetic sensors for low cost compass systems. Available from: