Thesis for the degree of Licentiate of Engineering

GNSS-aided INS for land vehicle positioning and navigation

Isaac Skog

Signal Processing School of Electrical Engineering KTH (Royal Institute of Technology) Stockholm 2007

Skog, Isaac GNSS-aided INS for land vehicle positioning and navigation Copyright c 2007 Isaac Skog except where otherwise stated. All rights reserved.

TRITA-EE 2007:066 ISSN 1653-5146

Signal Processing School of Eletrical Engineering KTH (Royal Institute of Technology) SE-100 44 Stockholm, Sweden Telephone + 46 (0)8-790 7790

Abstract
This thesis begins with a survey of current state-of-the art in-car navigation systems. The pros and cons of the four commonly used information sources GNSS/RF-based positioning, vehicle motion sensors, vehicle models and map information are described. Common lters to combine the information from the various sources are discussed. Next, a GNSS-aided inertial navigation platform is presented, into which further sensors such as a camera and wheel-speed encoder can be incorporated. The construction of the hardware platform, together with an extended Kalman lter for a closed-loop integration between the GNSS receiver and the inertial navigation system (INS), is described. Results from a eld test are presented. Thereafter, an approach is studied for calibrating a low-cost inertial measurement unit (IMU), requiring no mechanical platform for the accelerometer calibration and only a simple rotating table for the gyro calibration. The performance of the calibration algorithm is compared with the Cramr-Rao bound for cases where a mechanical platform is used to rotate the IMU into different precisely controlled orientations. Finally, the effects of time synchronization errors in a GNSS-aided INS are studied in terms of the increased error covariance of the state vector. Expressions for evaluating the error covariance of the navigation state vector are derived. Two different cases are studied in some detail. The rst considers a navigation system in which the timing error is not taken into account by the integration lter. This leads to a system with an increased error covariance and a bias in the estimated forward acceleration. In the second case, a parameterization of the timing error is included as part of the estimation problem in the data integration. The estimated timing error is fed back to control an adjustable fractional delay lter, synchronizing the IMU and GNSS-receiver data.

Acknowledgements
First of all, I would like to express my deepest gratitude to my advisor, Professor Peter H ndel, for his ideas, inspiration and enormous support. I look forward to a working with you for another couple of years! I would like to thank my colleagues at plan 4 for making work a pleasure. To my friends, who have repeatedly asked me what a PhD student actually does and what I am working on and, though they may not have fully understood my answers, still support me. Put simple, the work of a PhD student can be summarized as follows: Choose a topic (in my case land vehicle navigation), read one hundred papers on it, write a new paper with a couple of amendments so that the next person in line will have to read one hundred and one papers, present your results at a conference in a carefully chosen location and, lastly, iterate the process several times. Thanks for bringing a lot of joy and fun into my life. Finally, and most importantly, I would like to thank my mother, Margareta, and my father, Rolf, for letting me as a child bring home and take apart all the old televisions and stereos I could nd - thats how it all started. I owe it all to you. To my brother, Elias, and my half-sister, Julia, I love you the most!

iii

Contents
Abstract Acknowledgements Contents i iii v

I Introduction
Introduction 1 Contributions of the Thesis . . . . . . . . . . . . . . . . . . . . . 2 Related papers not included in the thesis . . . . . . . . . . . . . .

1
1 1 4

II Included papers
A State-of-the art and future in-car navigation systems a survey 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . 2 State-of-the art systems . . . . . . . . . . . . . . . . . . . . 3 Global Navigation Satellite Systems and Augment Systems . 4 Vehicle Motion Sensors . . . . . . . . . . . . . . . . . . . . 4.1 Dead reckoning and inertial navigation . . . . . . . 5 Vehicle models and motions . . . . . . . . . . . . . . . . . 6 Map information . . . . . . . . . . . . . . . . . . . . . . . 7 Information Fusion . . . . . . . . . . . . . . . . . . . . . . 7.1 Non-linear ltering . . . . . . . . . . . . . . . . . . 8 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

5
A1 A1 A3 A5 A8 A13 A16 A18 A20 A21 A22 A23

B A low-cost GPS aided inertial navigation system for vehicle applications B1 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . B1 2 Navigation Dynamics . . . . . . . . . . . . . . . . . . . . . . . . B2 v

2.1 Navigation equations . . . . . . . 2.2 Error equations . . . . . . . . . . 3 Discretization . . . . . . . . . . . . . . . 3.1 Discrete time navigation equations 3.2 Discrete time error equations . . . 4 Extended Kalman Filtering . . . . . . . . 5 Design and Conclusions . . . . . . . . . 5.1 Hardware Design . . . . . . . . . 5.2 Simulation results . . . . . . . . References . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . .

. B2 . B3 . B5 . B5 . B5 . B6 . B8 . B9 . B9 . B11 C1 . C1 . C2 . C2 . C4 . C8 . C9 . C11 . . . . . . . . . . D1 D1 D2 D6 D8 D9 D9 D10 D11 D15 D15

C A Versatile PC-Based Platform For Inertial Navigation 1 Introduction . . . . . . . . . . . . . . . . . . . . . . 2 System Overview . . . . . . . . . . . . . . . . . . . 3 Sensors . . . . . . . . . . . . . . . . . . . . . . . . 4 Software Algorithm . . . . . . . . . . . . . . . . . . 5 Results . . . . . . . . . . . . . . . . . . . . . . . . . 6 Conclusions an Further Work . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . D Calibration of a MEMS inertial measurement unit 1 Introduction . . . . . . . . . . . . . . . . . . . 2 Sensor Error Model . . . . . . . . . . . . . . . 3 Calibration . . . . . . . . . . . . . . . . . . . 4 Cram r Rao Lower Bound . . . . . . . . . . . e 5 Results . . . . . . . . . . . . . . . . . . . . . . 5.1 Performance Evaluation . . . . . . . . 5.2 Calibration of IMU . . . . . . . . . . . 6 Conclusions . . . . . . . . . . . . . . . . . . . Appendix A . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

E Time synchronization errors in GPS-aided inertial navigation systems E1 1 Nomenclature . . . . . . . . . . . . . . . . . . . . . . . . . . . . E1 2 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . E3 3 Covariance of the estimation error . . . . . . . . . . . . . . . . . E4 3.1 Closed-Loop Error . . . . . . . . . . . . . . . . . . . . . E6 3.2 Timing Errors in Closed-Loop . . . . . . . . . . . . . . . E7 3.3 Example: Single-axis GPS-aided INS . . . . . . . . . . . E9 4 Modelling the timing error in the integration lter . . . . . . . . . E13 4.1 Example: Single-axis GPS-aided INS, revisited . . . . . . E17 5 Implementing a variable delay in the navigation lter . . . . . . . E17 6 Time synchronization applied to a low-cost GPS-aided INS . . . . E20 6.1 Simulated data . . . . . . . . . . . . . . . . . . . . . . . E21 vi

6.2 Real-world data . . . . . 7 Observability of time delay error 8 Results and Conclusions . . . . Appendix A . . . . . . . . . . . . . . Appendix B . . . . . . . . . . . . . . References . . . . . . . . . . . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

E23 E34 E35 E36 E38 E39

vii

Part I

Introduction

Introduction
In-car navigation involves three distinguished processes: estimation of the vehicles position and velocity relative to a known reference, path planing, and route guidance. The rst capability, positioning, is essential for successful path planing and route guidance capability. Nowadays, the area of high-performance positioning systems and methods is well developed. The challenge is to develop highperformance system solutions using low-cost sensor technology. This is the topic of the thesis, consisting of the following ve papers. Paper A: I. Skog and P. H ndel, State-of-the art and future in-car navigation a systems a survey, submitted to IEEE Transactions on Intelligent Transportation Systems. Paper B: I. Skog and P. H ndel, A low-cost GPS aided inertial navigation system a for vehicle applications, in Proc. EUSIPCO 2005, (Antalya, Turkey), Sept. 2005. Paper C: I. Skog, A. Schumacher and P. H ndel, A Versatile PC-Based Platform a For Inertial Navigation, in Proc. NORSIG 2006, (Reykjavik, Iceland), June. 2006. Paper D: I. Skog and P. H ndel, Calibration of a MEMS inertial measurement a unit, in Proc. XVII IMEKO World Congress, (Rio de Janeiro, Brazil), Sept. 2006. Paper E: I. Skog and P. H ndel, Time synchronization errors in GPS-aided ina ertial navigation systems, submitted to IEEE Transactions on Intelligent Transportation Systems.

1 Contributions of the Thesis

The contributions in this thesis appears in terms of ve papers, devoted to different areas associated with the development of low-cost in-car navigation solutions. An introduction to land vehicle navigation is provided in paper A, written as a survey of the current state-of-the art in-car navigation technology; to mediate a

2I NTRODUCTION

understanding of the limitations and problems associated with the current in-car navigation systems. The remaining four papers make contributions to the following topics. Development of versatile navigation platforms. Papers B and C, presents the construction of a GNSS aided INS platform, into which further sensors such as a camera, wheel-speed encoder etc., are easily incorporated. Calibration of low-cost IMUs. The main contribution in paper D is the proposed simplied method to calibrate low-cost IMUs, together with the derivation of the Cram r-Rao bound for the standard calibration method, e where a turn-table is used to rotate the IMU into different orientations. Time synchronization in GNSS aided INSs. Paper E deals with the problem of time synchronization in a GNSS aided INS. Expressions for the increased error covariance of the system, due to the synchronization error is derived. A method to compensate for the time synchronization error is proposed. The papers are summarized in the following sections. Paper A: State-of-the art and future in-car navigation systems a survey A survey of the information sources and information fusion technologies used in the current in-car navigation systems is presented. The pros and cons of the four commonly used information sources GNSS/RF-based positioning, vehicle motion sensors, vehicle models and map information are described. Common lters to combine the information from the various sources are discussed. A prediction of possible tracks in the further development of in-car navigation systems concludes the survey. Paper B: A low-cost GPS aided inertial navigation system for vehicle applications In this paper an approach for integration between GPS and inertial navigation systems (INS) is described. The continuous-time navigation and error equations for an earth-centered earth-xed INS system are presented. Using zero order hold sampling, the set of equations is discretized. An extended Kalman lter for closed loop integration between the GPS and INS is derived. The lter propagates and estimates the error states, which are fed back to the INS for correction of the internal navigation states. The integration algorithm is implemented on a host PC, which receives the GPS and inertial measurements via the serial port from a tailor made hardware platform, which is briey discussed. Using a battery operated PC the system is fully mobile and suitable for real-time vehicle navigation. Simulation results of the system are presented.

1 C ONTRIBUTIONS

OF THE

T HESIS 3

Paper C: A Versatile PC-Based Platform For Inertial Navigation A GPS aided inertial navigation platform is presented, into which further sensors such as a camera, wheel-speed encoder etc., can be incorporated. The construction of the platform is described and an introduction to the sensor fusion approach is given. Results from a eld-test is presented, indicating which error sources that needs to be modelled more accurately. Paper D: Calibration of a MEMS inertial measurement unit An approach for calibrating a low-cost IMU is studied, requiring no mechanical platform for the accelerometer calibration and only a simple rotating table for the gyro calibration. The proposed calibration methods utilize the fact that ideally the norm of the measured output of the accelerometer and gyro cluster are equal to the magnitude of applied force and rotational velocity, respectively. This fact, together with model of the sensors is used to construct a cost function, which is minimized with respect to the unknown model parameters using Newtons method. The performance of the calibration algorithm is compared with the Cram r-Rao e bound for the case when a mechanical platform is used to rotate the IMU into different precisely controlled orientations. Simulation results shows that the mean square error of the estimated sensor model parameters reaches the Cram r-Rao e bound within 8 dB, and thus the proposed method may be acceptable for a wide range of low-cost applications. Paper E: Time synchronization errors in GPS-aided inertial navigation systems The effects of time synchronization errors in a GPS-aided inertial navigation system (INS) are studied in terms of the increased error covariance of the state vector. Expressions for evaluating the error covariance of the navigation state vector given the vehicle trajectory and the model of the INS error dynamics are derived. Two different cases are studied in some detail. The rst case considers a navigation system in which the timing error is not included in the integration lter. This leads to a system with an increased error covariance and a bias in the estimated forward acceleration. In the second case, a parameterization of the timing error is included as a part of the estimation problem in the data integration. The estimated timing error is fed back to control an adjustable fractional delay lter, synchronizing the inertial measurement unit (IMU) and GPS-receiver data. Simulation results show that by including the timing error in the estimation problem, almost perfect time synchronization is obtained and the bias in the forward acceleration is removed. The potential of the proposed method is illustrated with tests on real-world data that are subjected to timing errors. Moreover, through an observability analysis, it is shown that the timing error is observable for all trajectories that include turns or non-zero accelerations.

4I NTRODUCTION

2 Related papers not included in the thesis

The following two papers have not been included, even though partly related to the work described in the thesis. Paper F: J. Rantakokko, P. H ndel, F. Ekl f, B. Boberg, M. Junered, D. Akos, I. a o Skog, H. Bohlin, F. Nereg rd, F. Hoffmann, D. Andersson, M. Jansson, and a P. Stenumgaard, Positioning of emergency personnel in rescue operations possibilities and vulnerabilities with existing techniques and identication of needs for future R&D, Technical report, Royal Institute of Technology, Stockholm, Sweden. Paper G: P. H ndel, Y. Yao, N. Unkuri, and I. Skog, A framework for moose a early warning driver assistance systems, Technical report, Royal Institute of Technology, Stockholm, Sweden.

Part II

Included papers

Paper A
State-of-the art and future in-car navigation systems a survey
Isaac Skog and Peter H ndel a Submitted to IEEE Transactions on Intelligent Transportation Systems

c 2007 IEEE The layout has been revised

State-of-the art and future in-car navigation systems a survey

Isaac Skog and Peter H ndel a

Abstract A survey of the information sources and information fusion technologies used in the current in-car navigation systems is presented. The pros and cons of the four commonly used information sources GNSS/RF-based positioning, vehicle motion sensors, vehicle models and map information are described. Common lters to combine the information from the various sources are discussed. A prediction of possible tracks in the further development of in-car navigation systems concludes the survey.

1 Introduction
Today a large share of private cars is delivered from the factory with a GPS-based in-car navigation system. Owners of used cars can at, a reasonable cost, install one of the many third party in-car navigation systems on the market. These navigation aids are designed to support the driver by showing the vehicles current location on a map and by giving both visual and audio information on how to efciently get from one location to another, i.e., route guidance. Moreover, many vehicles used in professional services, such as taxis, buses, ambulances, police cars and re trucks, are today equipped with navigation systems that not only show the current location but also constantly communicate the vehicle location to a monitoring center. Operators at the center can use this information to direct the vehicle eet as efciently as possible. To further improve the usefulness of these in-car navigation systems, for example, with information such as when, where and how to make lane changes with respect to the planned course changes, the accuracy of both the navigation systems and digital maps has to be improved [1, 2]. Increasing the accuracy and robustness of the navigation systems implies that the trafc coordinators could guide their vehicle eets even more efciently in terms of the trafc ow in different road lanes, etc. Refer to [3] for a discussion of robustness enhancement of

A2

S TATE - OF - THE ART AND FUTURE IN - CAR

NAVIGATION SYSTEMS

A SURVEY

Information sources GNSS/RF-based Positioning

PSfrag replacements

Vehicle Motion Sensors Road Maps

User Information Information Fusion Vehicle State Guidance Trafc Situation Information

Vehicle Models

ADAS

Camera/Radar/Laser
Figure 1: Conceptional description of the available information sources and information ow for a in-car navigation system. The block with dashedlines are in general not an apart of current in-car navigation systems but will likely be a major part of next generation in-car navigation systems and advanced driver assistant systems (ADAS).

a bus eet monitoring system. Moreover, further development of advanced driver assistance systems (ADASs) and safety applications such as automatized highway systems, lane/road departure detection and warning systems, and collision avoidance requires not only navigation systems with higher accuracy but also better reliability and integrity, i.e., redundant information sources are needed [4]. With reference to Fig. 1, looking at the in-car navigation problem from an information perspective there are basically ve different sources of information available: the various Global Navigation Satellite Systems (GNSSs) and other RFbased navigation systems, sensors observing the vehicle dynamics, road maps and vehicle models. The GNSS receiver and vehicle motion sensors provide observations for estimation of the vehicle state. The vehicle model and road map put constraints on the dynamics of the system and allow past information to be projected forward in time and to be combined with the current observation information [5]. The fth type of information source - visual, radar, or laser information - is generally not used in current systems, but plays a major role in the development of ADASs, etc. Details on the incorporation of visual information into vehicle navigation systems and safety application systems are found in [6]. For designers of in-car navigation systems, the problem is to choose which of these information sources, if not all, to use and how to combine the information to meet performance

2 S TATE - OF - THE ART SYSTEMS

A3

requirements. This necessitates a balance between the cost, complexity and performance of the system. When evaluating the performance of a navigation system, it is important to remember that accuracy is only one of four performance measurements characterizing the system. The performance measurements are [7, 8]: Accuracy the degree of conformity of information concerning position, velocity, etc. provided by the navigation system relative to actual values Integrity measure of the trust that can be put in the information from the navigation system, i.e., the likelihood of undetected failures in the specied accuracy of the system. Availability a measure of the percentage of the intended coverage area in which the navigation system works Continuity of service the systems probability of continuously providing information without non-scheduled interruptions during the intended working period. Before entering into a discussion on possible ways to achieve increased navigation performance, it is important to point out that the area of high-performance navigation is well developed. Nowadays, the challenge is to develop high-performance navigation system solutions using low-cost sensor technology [9]. The purpose of this paper is to present a survey of current in-car navigation technology: possibilities, limitations and various design approaches. Section 2 describes state-of-the art in-car navigation systems and their pitfalls. Sections 3 to 6 describe the idea of operation, together with pros and cons of the four commonly used information sources in current in-car navigation. Section 7 is devoted to the problem of combining information from the different sources. Section 8 concludes the survey with a prediction of different tracks in the further development of in-car navigation systems.

2 State-of-the art systems

Generally, current commercially available in-car navigation systems match the information from a GPS receiver with that of a digital map, so called mapmatching [1, 2, 10, 11]. That is, by comparing the trajectory and position information from the GPS receiver with the roads in the digital map, the most likely position of the vehicle on the road is estimated. In urban environments, buildings may partly block satellite signals, forcing the GPS receiver to work with a poor geometric constellation of satellites and thereby reducing the accuracy of the position estimates [1215]. Even worse, less than four satellites may become available, making position xes impossible and interrupting the continuity of the navigation solution. Moreover, multi-path propagation of the radio signal due to reection in

A4

S TATE - OF - THE ART AND FUTURE IN - CAR

NAVIGATION SYSTEMS

A SURVEY

surrounding objects may lead to decreased position accuracy without notication by the GPS receiver, thereby reducing the integrity of the navigation solution [15]. Therefore, to counteract navigation solution degradation in situations with poor satellite constellation geometry, shadowing and multipath propagation of the satellite signals, advanced in-car navigation systems use information from additional sensors such as accelerometers, gyroscopes and odometers. To give an example, Siemens car navigation system uses a gyroscope and odometer to perform dead reckoning (DR). The trajectory estimated from dead reckoning is then projected onto the digital map. If the estimated position is between several roads, several projections are done and the likelihood of each projection is estimated based on the information from the GPS receiver and the development of the trajectory over time [10, 11]. Including additional sensors is not merely a question of giving the navigation system higher accuracy, better integrity or providing a more continuous navigation solution. It also allows the update rate of the system to be increased and provides more information such as acceleration, roll and pitch, depending on which types of sensors are used. The typical update rate for a GPS receiver is less than 20 times per second [16], whereas modern low-cost accelerometers and gyroscopes have update rates (bandwidths) of hundreds of Hertz. This means that even the high-frequency dynamics of the vehicle can be captured by the in-car navigation system.

To give absolute gures on the accuracy of state-of-the art in-car navigation systems and navigation systems in general is difcult, since the performance of the systems depends not only on the characteristics of the sensors, GPS receiver, vehicle model and map information but also on the trajectory dynamics and surrounding environment. However, an indication of the achievable performance that can be expected from an in-car navigation system based on fusion of GPS-position estimates with an odometer and gyroscope based dead reckoning system (DRS) (no map-matching or vehicle model) can be found in an excellent paper [17]. The authors evaluate how much the error in each individual sensor contributes to the total error in the position estimates of a land vehicle traveling at constant speed along a straight road. The sensitivity analysis shows that when GPS-position data is available, 90% or more of the long- and cross-track positioning error is due to GPS-positioning errors. Further, performance during GPS outages is mainly determined by the drift characteristics and accuracy with which the DR sensors were calibrated before the outage. The implication of this nding is that in order to design a robust navigation system from low-cost dead reckoning sensors, a high-accuracy positioning aiding system is needed. Hence, the accuracy of the in-car navigation system is highly dependent on available low-cost GPS receiver solutions.

3 G LOBAL NAVIGATION S ATELLITE S YSTEMS AND AUGMENT S YSTEMS

A5

3 Global Navigation Satellite Systems and Augment Systems

Currently there are two global navigation satellite systems available: the Russian GLONASS1 and the American Global Positioning System (GPS) [18]. Further, the European satellite navigation system Galileo is under construction and is scheduled to be fully operational by 2010-2012. Up-to-date information regarding the Galileo project is available from the home page of the European Space Agency [19]. These three systems have and will have a number of similarities and the GPS and Galileo system will be directly compatible, whereas the GLONASS system requires a somewhat different receiver structure. Further, the difference in orbit plans of the satellite constellations in the systems provides good coverage in different regions. The GPS system provides good coverage at mid latitudes, whereas the GLONASS system gives better coverage at higher latitudes [18]. The basic operational idea of the GNSS is that receivers measure the time-ofarrival of satellite signals and compare it to the transmission time, to calculate the signals propagation time. The time propagations are used to estimate the distances from the GNSS receiver to the satellites, so-called range estimates. From the range estimates, the GNSS receivers calculate position by means of triangulation. This is illustrated in Fig. 2. The accuracy of the position estimates is dependent on both the accuracy of the range measurements and the geometry of the satellites used in the triangulation [8, 15]. Errors in range estimates can be grouped together, depending on their spatial correlation, as common mode and non-common mode errors [16, 20]. Common mode errors are highly correlated between GNSS receivers in a local area (50200 km) and are due to ionospheric radio signal propagation delays, satellite clock and ephemeris2 errors, and tropospheric radio signal propagation delays. Noncommon mode errors depend on the precise location and technical construction of the GNSS receiver and are due to multi-path radio signal propagation and receiver noise. In Table 1, the typical standard deviation of these errors in the ranging estimates of a single-frequency GPS receiver, working in standard precision service (SPS) mode, is given [16]. Depending on the geometry of the available satellite constellation, the error budget for the standard deviation of the user equivalent range error (UERE) can be mapped to a prediction of the corresponding horizontal position accuracy as [16]: CEP = ln(2) HDOP UERE. (1)

Here, CEP (circular error probable) denotes the radius of a circle that contains 50% of the expected horizontal position errors. Further, HDOP is the horizontal dilution of precision, reecting the geometry of the satellite constellation. It is
Navigatsionnaya Sputnikovaya Sistema. ephemeris errors are due to the deviations in the satellite orbits, resulting in a difference between the actual and theoretically calculated satellite locations.
2 The 1 GLObalnaya

A6

S TATE - OF - THE ART AND FUTURE IN - CAR

NAVIGATION SYSTEMS

A SURVEY

Satellit

Satellit

Pseudo Range

Uncertainty Region

Receiver

Satellit

True Range

PSfrag replacements

Figure 2: Conceptional description of the positioning of a GNSS receiver. Under ideal circumstances, the propagation times of the satellite signals calculated by the GNSS receiver correspond to the true ranges between the receiver and the satellites, and the position of the receiver is given by the interception of the circles (spheres in 3-dim). Due to errors in the range estimates, there is no single interception point, but rather an interception region reecting the possible positions of the receiver.

worth noting that (1) is based on several assumptions such as uncorrelated range estimates and circular Gaussian-distributed position estimation errors, which more or less hold true [21]. Therefore (1) should only be used as a rough indication of position error. Since common mode errors are the same for all GNSS receivers in a restricted local area, they can be compensated by having a stationary GNSS receiver at a known location that estimates common mode errors and transmits correction information to rover GNSS receivers. This technology is commonly referred to as differential GNSS (DGNSS). The correlation of the common mode error decreases with the distance between the reference station and the rover unit. This will also be the case with the system performance [22]. The problem can be solved by a network of reference stations over the intended coverage area. The errors observed by these stations are constantly sent to a central processing station, where a map of the ionospheric delay,

3 G LOBAL NAVIGATION S ATELLITE S YSTEMS AND AUGMENT S YSTEMS

A7

Table 1: Standard deviations of errors in the range measurements in a singlefrequency GPS receiver [16].

Error Source Common mode Ionospheric Clock and ephemeris Tropospheric Non-common mode Multi-path Receiver noise Total (UERE) CEP with a horizontal dilution of precision, HDOP=1.2

Standard deviation [m] 7.0 3.6 0.7 0.13.0 0.10.7 7.98.5 6.67.1

together with ephemeris and satellite clock corrections, is calculated. The correction map is then relayed to the user terminals (GPS and GLONASS receivers), which can calculate correction data for their specic location [8, 23]. There are several satellite-based augmentation systems (SBASs) that, through geostationary satellites, regionally provide correction information free of charge for the GPS and GLONASS systems. In North America, there is the Wide Area Augmentation System (WAAS), in Europe the European Geostationary Navigation Overlay Service (EGNOS) and in Japan the Multi-functional Satellite Augmentation System (MSAS). Further, the GAGAN system for India and SNAS system for China are under development [2325]. In addition to providing correction data, the SBASs also provide information regarding the integrity of the signals from the various satellites. They also serve as additional satellites and thereby enhance the available satellite constellation. In [25], an illustrative example of the enhancement of the HDOP for a GPS receiver in an alpine canyon environment using EGNOS data is given. All SBASs are designed to be interoperable. The geostationary satellites of the augmentation systems transmit correction data using the L1 (1575.42 MHz) frequency of the GPS system, and therefore only the software for GPS receivers has to be modied to receive correction data. Many low-cost GPS receivers are able to use correction data from the SBASs [24]. In areas where obstruction prevents the reception of the EGNOS signal from any of the geostationary satellites, the information may be obtained from the EGNOS data access system, broadcasting the information via Internet-SISNeT (Signals in Space through the Internet) [26, 27]. Test results, based on correction data from the WAAS and EGNOS systems, demonstrate position accuracy in the range of 1-2 m in the horizontal plane and 2-4 m in the vertical plane at a 95% condence interval [28]. A more thorough description on how the SBAS operates and correction data is calculated can be found in [8]. Further, information regarding the EGNOS project is available

A8

S TATE - OF - THE ART AND FUTURE IN - CAR

NAVIGATION SYSTEMS

A SURVEY

from 20 fact sheets from the European Space Agency (ESA) at [29]. It should be pointed out that the discussion above about performance characteristics and augmentation systems for the GPS system has focused on single-frequency receiver units. Using dual frequency receivers and charier-phase measurements supported by various augmentation systems, it is possible to achieve real-time position accuracy on a decimeter level [7, 22, 24, 30, 31]. However, the required receiver units are currently far too costly for use in commercial in-car navigation systems. In [24], a discussion of the performance and cost of single- and two-frequency GPS receivers and various augment systems is presented. In [15], software developed to predict the position accuracy of a GNSS receiver along a predened trajectory in an urban environment is described. Even if the GNSS receivers positioning accuracy is enhanced by various augmentation systems, the problems of poor satellite constellations, satellite signal blockages, and signal multipath propagation in urban environments remain. With the start-up of the Galileo system, the number of accessible satellites will increase and the probability of poor satellite geometry and signal blockages in urban environments will be reduced. Further, the integrity of the provided navigation solution will increase since two (three) separate systems are available for navigation. Still, there will be areas such as tunnels where reliable GNSS receiver navigation solutions will not be available. The problem can be reduced by ground-based stations acting as additional satellites, so-called pseudolites. By locating the pseudolites at favorable sites, the accuracy and continuity of the GNSS receivers navigation solution can be enhanced [20, 32]. However, usage of pseudolites has some inherent drawbacks: it only solves the coverage problem locally, it requires an additional infrastructure, and the GNSS receiver must be designed to handle the additional pseudolite signals. Other radio-based navigation aids that are under extensive research include positioning in wireless sensor networks, cellular networks and WLANs. An overview of the various techniques, possibilities and limitations of positioning in wireless networks can be found in [3335]. The inherent weakness of all radio signal-based navigation methods is their reliance on information from external sources that may become erroneous or disturbed. In order to overcome these pitfalls and create a robust navigation system, they should be combined with information from other sensors or navigation systems.

4 Vehicle Motion Sensors

There are a number of sensors, wheel odometers, magnetometers, accelerometers, etc. that can provide information about a vehicles state that may be used in combination with a GNSS receiver or other absolute positioning systems. In Table 2, the most commonly used sensors, together with the information they provide, are summarized.

4 V EHICLE M OTION S ENSORS

A9

Table 2: Sensors commonly used as a complement to GNSS-receivers for enhancement of in-car navigation systems.

Sensor Steering encoder Odometer Velocity encoders Electronic compass Accelerometer Gyroscope

Measurement Front wheel direction Travelled distance Wheel velocities (Indirectly, heading) Heading relative magnetic north Acceleration Angular velocity

A steering encoder measures the angle of the steering wheel. Hence, it provides a measure of the angle of the front wheels relative to the forward direction of the vehicle platform. Together with information on the wheel speeds of the front wheel pair, the steering angle can be used to calculate the heading rate of the vehicle. An odometer provides information on the traveled curvilinear distance of a vehicle by measuring the number of full and fractional rotations of the vehicles wheels [17]. This is mainly done by an encoder that outputs an integer number of pulses for each revolution of the wheel. The number of pulses during a time slot is then mapped to an estimate of the traveled distance during the time slot through multiplication with a scale factor depending on the wheel radius. A velocity encoder provides a measurement of the vehicles velocity by observing the rotation rates of the wheels. If separate encoders are used for the left and right wheel of either the rear or front wheel pair, an estimate of the heading change of the vehicle can be found through the difference in wheel speeds. Information on the speed of the different wheels is often available through the sensors used in the anti-lock breaking system (ABS). See [3638] for details. For a kinematic vehicle model as illustrated in Fig. 3, the left and right rear wheel velocities vlr and vrr , respectively, together with the track width, tw, can be mapped to a heading rate estimate as: vrr vlr . (2) = tw By measuring the velocity of the left and right front wheels, vlf and vrf , respectively and observing the steering angle , the yaw rate can be estimated as: vrf vlf = . tw cos() (3)

The dependency of the steering angle is due to the variation in efcient track width with the radius of the turn [38]. These ideas on how to estimate traveled distance, velocity and heading of the vehicle are all based on the assumption that the wheel revolutions can be translated

A10

S TATE - OF - THE ART AND FUTURE IN - CAR

NAVIGATION SYSTEMS

A SURVEY

vlf - Velocity left front wheel PSfrag replacements vlr - Velocity left rear wheel Center of gravity tw - Track width v -Velocity vector - Direction of velocity Steering angle

- Yaw rate
vrr - Velocity right rear wheel

vrf - Velocity right front wheel

Figure 3: A simple kinematic vehicle model for translation of wheel speeds to heading changes [37]. It is assumed that the vehicle moves in a planar environment and that wheel speeds are solely in the direction the wheels are heading. Depending on whether the steering angle is observed or not, the velocity of the front or rear wheels may be used in the calculation of heading changes.

into linear displacements relative to the ground. However, there are several sources of inaccuracy in the translation of the wheel encoder readings to traveled distance, velocity and heading change of the vehicle. They are [17, 37, 39]: wheel slips, uneven road surfaces, skidding, changes in wheel diameter due to variations in temperature, pressure, tread wear and speed, unequal wheel diameters between the left and right wheels, uncertainties in efcient wheelbase (track width), and limited resolution and sample rate of the wheel encoders. The rst three error sources are terrain dependent and occur in a non-systematic way. This makes it difcult to predict and limit their negative effect on the accuracy of the estimated traveled distance, velocity and heading. The four remaining error sources occur in a systematic way, and their impact on the traveled distance, velocity and heading estimates are more easily predicted. The errors due to changes in

4 V EHICLE M OTION S ENSORS

A11

wheel diameter, unequal wheel diameter and uncertainties in efcient wheelbase can be reduced by including them as parameters estimated in the sensor integration. An electronic compass is an electronic device, constructed from magnetometers, that provides heading measurements relative to the earths magnetic north by observing the direction of the earths local magnetic eld [17]. To convert the compass heading into an actual north heading, the declination angle (i.e., the angle between the geographic and magnetic north) is needed, which is position dependent. Thus, knowledge of the compass position is necessary to calculate the heading relative to geographic north. Generally, the compass is constructed around three magneto-resistive or uxgate magnetometers, together with pitch and roll sensors [18]. The pitch and roll measurements are needed to determine the attitude between the coordinate system spanned by the magnetic sensors sensitivity axes and the local horizontal plane, so that the horizontal component of the earths magnetic eld can be calculated. For a vehicle moving in a planar environment experiencing only small pitch and roll angles, a compass constructed from only two magnetometers with perpendicular sensitivity axes lying approximately in the horizontal plane may be sufcient and cost-effective. In [18] and [40], details about compasses based on ux-gate magnetometers can be found. A review of magnetic sensors is found in [41]. Power lines, metal structures such as bridges and buildings, along the trajectory of the vehicle cause variations in the local magnetic eld, resulting in large and unpredictable errors in the heading estimates of the compass. Therefore, the usefulness of magnetic compasses in in-car navigation systems can be questioned [17]. However, there are other applications of magnetic sensors in in-car navigation systems. See [4], where magnetic sensors are used to detect the vehicles location with the help from magnets distributed along a highway. An accelerometer provides information about the acceleration of the object to which it is attached. More strictly speaking, an accelerometer measures the acceleration of the object to which it is attached relative to the inertial frame of reference and projects it along its sensitivity axis. Information about an objects orientation and rotation may be obtained by using a gyroscope, which measures the angular velocity of the object relative to the inertial frame of reference. Hence, by equipping the vehicle with inertial sensors, i.e., accelerometers and gyroscopes, information about the vehicles acceleration and rotation is obtained and can be mapped into estimates of the vehicles attitude, velocity and position. There are many different ways to construct inertial sensors. In [18], a description of common technologies and their typical performance parameters can be found. A description of the trends in inertial sensor technology is offered in [42]. Historically, inertial sensors have mostly been used in high-end navigation systems for missile, aircraft and marine applications, due to the high cost, size and power consumption of the sensors. However, with the progress in microelectromechanical-system (MEMS) sensor technology it has become possible to construct inertial sensors meeting the cost and size demands needed for low-cost

A12

S TATE - OF - THE ART AND FUTURE IN - CAR

NAVIGATION SYSTEMS

A SURVEY

commercial electronics, such as vehicle navigation systems. However, the price paid (with currently available sensors) is a reduced performance characteristic. An illustrative description of developments in MEMS technology and its many applications are offered in [43]. In chapter 7 of [18], an introduction to the MEMS inertial sensor technology can be found. In [44], a discussion of the usefulness of MEMS sensors in vehicle navigation and their limitations is presented. Their usefulness in navigation primarily depends on MEMS gyroscope development. Unlike odometers, velocity encoders, and magnetic compasses, whose errors are partly related to the terrain in which the vehicle is traveling, inertial sensors are fully self-contained. Moreover, if the inertial sensors are mounted in the package holding the GNSS receiver, the need for an electrical connection between the navigation system and vehicle is reduced. Therefore the MEMS inertial sensors are attractive as a complement to the GNSS receiver, especially for third-party in-car navigation systems which must be easy to install. There are several error sources associated with inertial sensors which must be considered. The most signicant inertial sensor errors can be categorized as [18, 20]: biases, scale factors, nonlinearities, and noise. The bias error occurs as non-zero output from the sensor for a zero input. Scale factor and nonlinearity errors describe the uncertainty in linear and non-linear scaling between the input and output, respectively. Each of these error categories in general includes some or all of the following components: xed terms, turn-on to turn-on varying terms, random walk terms, and temperature varying terms. The xed terms, and to a large extent the temperature varying terms, can be estimated and compensated by calibration of the sensors; refer to [4548] for several calibration approaches. Turn-on to turn-on terms differ from time to time when the sensor is turned on, but stay constant during the operation time, whereas the random walk error slowly varies over time. The sensors turn-on to turn-on and random walk error characteristics are therefore of major concern in the choice of sensors and information fusion method.

4 V EHICLE M OTION S ENSORS

A13

In order to measure the vehicles dynamics in both the long- and cross-track direction, a cluster of inertial sensors is needed, referred to as an inertial sensor assembly (ISA). Depending on the construction of the navigation system, the ISA may consist of solely accelerometers, but more frequently a combination of accelerometers and gyroscopes is used. See [49] and [50] for examples of all accelerometer-based navigation systems. In general, a six-degree-of-freedom ISA, i.e., an inertial measurement unit (IMU) designed for unconstrained navigation in three dimensions, consists of three accelerometers and three gyroscopes, where the sensitivity axes of the accelerometers are mounted to be orthogonal and span a three-dimensional space, and the gyroscopes measure the rotation rates around these axes.

4.1

Dead reckoning and inertial navigation

Velocity encoders, accelerometers and gyroscopes all provide information on the rst or second order derivative of the position and attitude of the vehicle. Further, the odometer only gives information of the traveled distance of the navigation system. Hence, except for the magnetometer, all the measurements of the sensors in Table 2 only contain information on the relative movement of the vehicle and no absolute positioning or attitude information. The translation of these sensor measurements into position and attitude estimates will therefore be of an integrative nature requiring the initial state of the vehicle to be known, and for which the measurement errors will accumulate with time or, for the odometer, with the traveled distance. This translation process is generally referred to as DR, or if only involving inertial sensors inertial navigation. Precisely how these translations of sensor measurements into information on the vehicle state are done depends on the sensor conguration, if the navigation is done in two or three dimensions and the constraints on the movements of the vehicle. Basically, they all include three steps: 1. The estimation of attitude (3-dim) or heading (2-dim) of the vehicle relative the navigation coordinate system. 2. The translation of the traveled distance, velocity and acceleration into navigation coordinates using the attitude or heading information. 3. The integration of traveled distance, velocity and acceleration over time to obtain position and velocity estimates in the navigation coordinate frame. In Fig. 4, the method of DR in two dimensions is illustrated in terms of vector addition [10]. The position (xi , yi ) of the vehicle at time i is calculated based on information on the heading i and the traveled distance i from the last known location (xi1 , yi1 ). The traveled distance of the vehicle is estimated by an odometer or by integrating the output of a velocity encoder over time. The heading may be observed by measuring the speed difference between the left and right wheel, a

A14

S TATE - OF - THE ART AND FUTURE IN - CAR

NAVIGATION SYSTEMS

A SURVEY

y PSfrag replacements (x1 , y1 ) 1

1 2

(x2 , y2 ) x

(x0 , y0 )
Figure 4: Dead-reckoning in terms of vector addition. The position (x i , yi ) at time i is calculated based upon information about the heading i and the travelled distance i from the last known location (xi1 , yi1 ).

magnetometer (electronic compass), a gyroscope or a combination of these methods and sensors. Refer to [10, 11, 36, 37] for details on how dead reckoning is done in vehicle navigation systems. In Fig. 5, a block diagram of a strap-down3 inertial navigation system (INS) is shown. The INS comprises two distinct parts, the IMU and the computational unit. The former provides information on the accelerations and angular velocities of the navigation platform relative to the inertial coordinate frame of reference. The angular rates observed by the gyroscopes are used to track the relation between the coordinate system associated with the navigation platform and the coordinate frame in which the system is navigating. This information is then used to transform the specic force observed in platform coordinates into the navigation frame, where the gravity acceleration is subtracted from the observed specic force. What remains are the accelerations in navigation coordinates. To obtain the position of the navigation platform, the accelerations are integrated twice with respect to time; refer to [16, 18, 20, 45, 46, 5153] for a thorough treatment of the subject of inertial navigation. In [54], a survey of inertial systems terminology can be found.
3 The term strap-down referees to that the gyroscopes and accelerometers are rigidly attached to the navigation platform. In a gimballed INS the sensors are mounted on a platform isolated from the rotations of the vehicle [20].

PSfrag replacements
4 V EHICLE M OTION S ENSORS A15

INS
Accelerometers

Navigation Equations

Gravity Model

Coordinate Rotation

dt

dt

Position Velocity Attitude

dt

IMU Gyroscopes Attitude Determination

Figure 5: Conceptional sketch of a strap-down INS.

The integrative nature of the navigation calculations in DR and inertial navigation systems gives the systems a low-pass lter characteristic that suppresses highfrequency sensor errors but amplies low-frequency sensor errors. This results in a position error that grows without bound as a function of the operation time or traveled distance, and where the error growth depends on the error characteristics of the sensors. In general, it holds that for an INS a bias in the accelerometer measurements causes error growth proportional to the square of the operation time, and a bias in the gyroscopes causes error growth proportional to the cube of the operation time [44, 49, 55, 56]. The detrimental effect of the gyroscope errors on the navigation solution is due to the direct reections of the errors on the estimated attitude. The attitude is used to calculate the current gravity in navigation coordinates and cancel its effect on the accelerometer measurements. Since in most land vehicle applications the vehicles accelerations are signicantly smaller than the gravity acceleration, small errors in attitude may cause large errors in estimated accelerations. These errors are then accumulated in the velocity and position calculations. Hence, the error characteristics of the gyroscopes used in the IMU are of major concern when designing an INS. To summarize, the properties of DRSs and INSs are complimentary to those of the GNSSs and other radio-based navigation systems. These properties are: They are self-contained, i.e., they do not rely on any external source of information that can be disturbed or blocked. The update rate and dynamic bandwidth of the systems are mainly set by the systems computational power and the bandwidth of the sensors. The integrative nature of the systems results in a position error that grows without bound as a function of the operation time or traveled distance.

A16

S TATE - OF - THE ART AND FUTURE IN - CAR

NAVIGATION SYSTEMS

A SURVEY

Contrary to these properties, the GNSS and other radio-based navigation systems give position and velocity estimates with a bounded error but at relatively low rate and depend on information from an external source that may be disturbed. The complimentary features of the two types of systems make their integration favorable and if properly done results in navigation systems with higher update rates, accuracy, integrity and ability to provide a more continuous navigation solution under various conditions and environments. Odometers and velocity and steering encoders have proven to be very reliable DR sensors. For movements in a planar environment, they can provide reliable navigation solutions during several minutes of GNSS outages. However, in environments that signicant violate the assumption of a planar environment, accuracy is drastically reduced [56]. An INS constructed around a full-six-degreeof-freedom IMU does not include any assumption of the motion of the navigation system and therefore is independent of the terrain in which vehicle is traveling. Moreover, it provides three-dimensional position, velocity and attitude information, and if situated in the package of the GNSS receiver reduces the need for vehicle xed sensors. In combination with decreasing cost, power consumption and size of the MEMS inertial sensors, this makes vehicle navigation systems incorporating MEMS IMUs attractive. However, current ultra low-cost MEMS inertial sensors have an error characteristic causing position errors in the range of tens of meters during 30 seconds of stand-alone operation [9, 14, 44, 57]. This is also illustrated in Fig. 6, where the root mean square (RMS) horizontal position error during a 30-second GNSS signal outage in a GNSS-aided INS is shown. In the simulation, the IMU sensors were modeled as ideal sensors, except from having measurement noises, turn-on to turn-on and time varying biases reect current ultra low-cost MEMS inertial sensors.

5 Vehicle models and motions

Under ideal conditions, a vehicle moving in a planar environment experiences no wheel slip and no motions in the direction perpendicular to the road surface. Thus, in vehicle coordinates, the downward and sideways velocity components should be close to zero. In [14, 44, 56], this type of non-holonomic constraint has been applied to the navigation solution of the vehicle-mounted GNSS-aided INSs. The results show a great reduction in position error growth during GNSS outages and increased attitude accuracy when imposing non-holonomic constraints on the navigation solution. In Fig. 6, the reduction in error growth using non-holonomic constraints in a GPS-aided INS using a MEMS IMU is illustrated. The case when observing the forward velocity from a simulated velocity encoder is also shown. In the case of both non-holonomic constraints and forward velocity aiding, error growth during the outage is negligible. From an estimation-theoretical perspective, sensors and vehicle-model information play an equivalent role in the estimation of the vehicle state [5]. If there

5 V EHICLE

MODELS AND MOTIONS

A17

RMS horizontal position error

10 8 6 4
PSfrag replacements

NOC NHC NHC+VA

2 0

300

305

310

315 s

320

325

330

Figure 6: Empirical root mean square (RMS) horizontal position error growth during a 30-second satellite signal blockage in a low-cost GPSaided INS. NOC - No constraints, NHC - Non-holonomic constraints, NHC+VA - Non-holonomic constraints and velocity aiding.

were a perfect vehicle model, such that the vehicle state could be perfectly predicted from control inputs, sensor information would be superuous. Contrarily, if there were such things as perfect sensors, the vehicle model would provide no additional information. Neither of these extremes exists. It is clear, however, that navigation system performance can be enhanced by utilizing vehicle models. Moreover, the incorporation of a vehicle model in the navigation system may allow the use of less costly sensors without degradation in navigation performance. There are numerous vehicle model and motion constraints, ranging from the above-mentioned non-holonomic constraints to more advanced models incorporating wheel slip, tire stiffness, etc. Different vehicle models and constraints of varying complexity can be found in [5, 12, 44, 56, 5860]. In [5], a theoretical framework for analyzing the impact of various vehicle models is developed. The results show that there is a lot to gain from using more rened vehicle models, especially in the accuracy of the orientation estimate. However, it is difcult to nd good vehicle models, independent of the driving situation [59]. More advanced models require knowledge about several parameters such as vehicle type, tires,

A18

S TATE - OF - THE ART AND FUTURE IN - CAR

NAVIGATION SYSTEMS

A SURVEY

Table 3: Bandwidth of the true motion dynamics of a land-vehicle as estimated by [61].

Motion Acceleration x-axis (forward) y-axis (sideways) z-axis (downwards) Angular velocity x-axis (roll) y-axis (pitch) z-axis (yaw)

Bandwidth [Hz] <2 <2 <8 <8 <8 <2

and environmental specics [56]. To adapt the model to different driving conditions, these parameters must be estimated in real-time. Alternatively, the driving conditions must be detected and used to switch between different vehicle models. An example of this, using an interactive multimodel extended Kalman lter, can be found in [59]. Another way to incorporate knowledge about the vehicle dynamics into the navigation system is through pre-ltering/denoising of the sensors measurements using the efcient bandwidth of the vehicles motion dynamics and characteristics of the sensor noises [6163]. In Table 3, the bandwidth of the actual motion dynamics of a land vehicle as estimated by [61] is shown. The wider bandwidth of the pitch and roll angular velocity and z-axis acceleration dynamics is due to road irregularities. In [63] and [61], these bandwidths, together with a noise model, are used to develop de-noising algorithms that are tested on three IMUs of different quality. The results show a 56% reduction in attitude errors during GNSS outages in the case of the MEMS IMU, and even more with high-quality IMUs. Since the attitude error of an INS is directly related to position error growth, a reduction in attitude error also implies a reduced position error. In [62], a deeper description of the idea behind the de-noising approach is given together with test results on a ight-mounted GPS-aided INS. The results are similar to those in [61, 63].

6 Map information
Under normal conditions, the location and trajectory of a car is restricted by the road network. Hence, a digital map of the road network can be used to impose constraints on the navigation solution of the in-car navigation system, a process referred to as map-matching. Traditionally, map-matching has been a unidirectional process, where the position and trajectory estimated by the GNSS receiver, vehicle motion sensors and vehicle model information have been used as input to produce a position and trajectory consistent with the road network of the digital

6 M AP INFORMATION

A19

arc candidates region

Node (Intersection) Position PSfrag replacements Estimate

Shape point

Node (dead-end)

Figure 7: Road network described by a planar model. The street system is represented by a set of arcs (i.e., curves in R2 ). Generally, a set of candidate arcs/segments close to the position estimate are selected rst, then the likelihood of the candidates is evaluated. Finally, the position on the most likely arc (road segment) is determined.

map. With improved map quality, the possibility of a bidirectional information ow in the map-matching has become feasible, viewing the map information as observations in the estimation of the information fusion [6]. This type of bidirectional map-matching is found in [6466]. Commonly, the road network is represented by a planar model in the digital maps, where the street system is represented by a set of arcs (i.e., curves in R 2 ) [67, 68]. Each arc represents a road in the network and is assumed to be piece-wise linear, such that it can be described by a nite set of points (see Fig. 7). The rst and last points in the set are referred to as nodes and the rest as shape points. The nodes describe the beginning and termination of the arc, indicating a start, dead-end or an intersection (i.e., a point where it is possible to go from one arc to another) in the street system. Matching the output of the navigation system to the road-network of the digital map generally involves three steps. First, a set of candidate arcs or segments are selected. Second, the likelihood of the candidate arcs/segments is evaluated using geometrical and topological information. Finally, the vehicle location on the most likely road segment is determined. The geometrical information includes measures like closeness between the position estimate and nearest road in the map; the difference in heading as indicated by the navigation system and road segments of concern; and the difference in the shape of the road segments with respect to estimated trajectory. Refer to [67] for a discussion and description of common measures such as point-topoint, point-to-curve, and curve-to-curve matching to extract geometrical information. The topological information criterion determines the connectivity of the

A20

S TATE - OF - THE ART AND FUTURE IN - CAR

NAVIGATION SYSTEMS

A SURVEY

candidate roads (arcs), e.g., the vehicle cannot suddenly move from one road segment to another if there is no intersection point in between the segments. The likelihood of the road segment candidates is found by assigning different weights to the geometrical and topological information measures and combining them. Refer to [10, 64, 65, 68, 69] for various weighting and combining approaches such as belief theory, fuzzy network and state machines. In [70] a survey of the current state-of-the art map-matching algorithms is found, together with ideas on further research directions.

7 Information Fusion
Numerous lters can be used to fuse the information from the different information sources into an estimate of the vehicle state: various versions of extended Kalman lters (EKFs) are used in [4, 36, 59, 71]; Sigma-Point lters are used in [7274]; particle lters are used in [66, 75]; a Neural Network in [76]. They all have their pros and cons but share one common idea, to utilize the different error properties of the information sources to compute a reliable estimate of the vehicle state. The lters can be used in basically two ways, a direct integration or a complimentary ltering approach. In the direct method, information from all sources is used as observations for a lter housing a vehicle model, relating the observation to an estimate of the vehicles state. The dynamics of most vehicles include highly deterministic components, which are difcult to model in the stochastic framework of many lters [16]. This is avoided in the complimentary ltering approach. In the complimentary ltering, illustrated in Fig. 8, the vehicle dynamic sensors, together with the vehicle model equations (navigation equations for pure DRS or INS), are used to produce vehicle state estimates and serve as the major navigation system. Estimated vehicle states are mapped into predictions of the outputs from the other information sources. The prediction residuals are used as input to a lter trying to separate the errors of the various information sources to calculate the errors in the vehicle state estimates and the vehicle dynamic sensors outputs. For the lter to successfully separate the different errors, it must incorporate appropriate models of the different errors, and the error characteristics of the information sources may only partly overlap. Modeling the error dynamics of the navigation system, rather than the vehicle motions in the fusion lter, results not only in a model that better ts into the statistical framework but also in a smaller bandwidth of the lter, since it estimates the slowly changing errors and not the full navigation stage. Hence, the noise sensitivity of the lter is reduced. In [77], a deeper description of the concept of complimentary ltering, together with an example of information fusion in an underwater vehicle, is found.

PSfrag replacements
7 I NFORMATION F USION A21

Vehicle Dynamics Sensors

Sensor errors

Navigation Equations
Navigation errors

Navigation Solution Mapping and Down sampling + + Aiding Information

Filter

Figure 8: Information fusion using a complimentary ltering approach with feedback. The vehicle motion sensors, together with the navigation equations, provide the major navigation solution, and the other information sources aid the DR/INS system through estimations and corrections of errors in the calculated navigation solution.

7.1

Non-linear ltering

The most widely used nonlinear ltering approach, due to its simplicity, is EKF in its various varieties. The idea behind EKF is to linearize the navigation and observation equations around the current navigation solution and turn the nonlinear ltering problem into a linear problem. Assuming Gaussian distributed noise sources, the minimum mean square error (MMSE) solution to the linear problem is then provided by the Kalman lter [78]. For non-Gaussian distributed noise sources, the Kalman lter provides the linear MMSE solution to the ltering problem. Unfortunately, linearization in the EKF means that the original problem is transformed into an approximated problem which is solved optimally, rather than approximating the solution to the correct problem. This can seriously affect the accuracy of the obtained solution or lead to divergence of the system. Therefore, in systems of a highly nonlinear nature and non-Gaussian noise sources, more rened nonlinear ltering approaches such as Sigma-Point lters (Unscented Kalman lters), particle lters (sequential Monte Carlo methods) and exact recursive nonlinear lters, which keep the nonlinear structure of the problem, may signicantly improve system performance [73, 79]. The inherent weakness of these nonlinear ltering approaches is the curse of dimensionality. That is, in general the computational complexity of the lter grows exponentially with the dimension of the state vector to be estimated [79]. Therefore, even with todays computational capacity, nonlinear lters can be unfeasible for navigation systems with high-dimensional state vectors. However, since the navigation equations in many navigation systems are only partial nonlinear, the ltering problem can be divided into a linear and nonlinear part, where the linear part, under the assumption of Gaussian-distributed noise entries, may be solved using a Kalman lter, hence reducing the computa-

A22

S TATE - OF - THE ART AND FUTURE IN - CAR

NAVIGATION SYSTEMS

A SURVEY

tional complexity of the system [80, 81]. A short introduction to nonlinear ltering and the advantages and disadvantages of various algorithms are given in [79]. In [66], a framework for positioning, navigation and tracking using particle lters is developed, and its usage is illustrated through examples of car positioning by map-matching, terrain navigation of aircraft, etc.

8 Conclusions
A survey of information sources and information fusion technologies used in incar navigation systems has been presented. The pros and cons of the four commonly used information sources - GNSS/RF-based positioning, vehicle motion sensors, vehicle models and map information - have been examined. Common lter techniques to combine the information from different sources have been briey discussed. Summarizing the survey, the following items are likely to improve in nextgeneration in-car navigation systems: In a scenario where all GNSSs have reached their full constellations, i.e., 30 Galileo satellites, 24 GLONASS satellites, and 35 GPS satellites, hybrid system GNSS receivers will have more than 25 satellites in view at any one time. Thus, the risk of blockages and poor satellite constellations is highly reduced. Three separate systems will also contribute to a higher integrity level. The modernization of GNSSs with multiple civil-user frequencies, together with development of low-cost multi-frequency receivers and carrier phase augmentation systems, will allow for decimeter-level positioning accuracy at a cost suitable for in-car navigation systems. Further developments in MEMS inertial sensor technology, especially MEMS gyroscopes, will allow for ultra low-cost micro IMUs that can bring full 3-dimensional attitude information to in-car navigation systems. Rened digital maps with lane information etc. will allow map-matching procedures with a bidirectional information ow, not only producing a position and trajectory consistent with the road network but also feeding back information from the map-matching to the sensor fusion. Increased processing power at reduced power consumption and cost levels will allow for usage of rened vehicle models and non-linear ltering technologies. Further, until this stage both OEM and third-party in-car navigation systems primarily have been developed to provide positioning and route-guidance information to the user. As the technological development of in-car navigation systems

R EFERENCES

A23

progresses, two tracks can be distinguished with different target applications and constraints on the navigation solution. One track is small handheld personal navigation devices, e.g., incorporated into mobile phones, used for positioning route guidance for both pedestrian and vehicles. The second track is OEM navigation systems, designed not only for route guidance but also to provide highly reliable inputs to advanced driver assistance and safety systems, such as automatized highway system, lane keeping and collision avoidance These systems not only require navigation systems with higher accuracy, update rates and availability but also good integrity for fast detection of sensor and sub-system failures jeopardizing the reliability of the advanced driver assistance systems.

References
[1] J. Du and M. Barth, Bayesian probabilistic vehicle lane matching for link-level invehicle navigation, in Proc. of IEEE Int. Veh. Symp., Tokyo, Japan, June 2006, pp. 522526. [2] , Lane-level positioning for in-vehicle navigation and automated vehicle location (AVL) systems, in Proc. of IEEE Int. Transport Sys. Conf., Washington, D.C, USA, Oct. 2004, pp. 3540. [3] M. Tsakiri, M. Stewart, T. Forward, D. Sandison, and J. Walker, Urban eet monitoring with GPS and GLONASS, Navigation, J. of The Institute of Navigation, vol. 51, no. 3, pp. 382393, Sept. 1998. [4] Y. Yang and J. Farrell, Magnetometer and differential carrier phase GPS-aided INS for advanced vehicle control, IEEE Trans. Robotics and Automation, vol. 19, no. 2, pp. 269282, Apr. 2003. [5] S. Julier and H. Durrant-Whyte, On the role of process models in autonomous land vehichle navigation systems, IEEE Trans. Robotics and Automation, vol. 19, no. 1, pp. 114, Feb. 2003. [6] J. Wang, S. Schroedl, K. Mezger, R. Ortloff, A. Joos, and T. Passegger, Lane keeping based on location technology, Proc. of IEEE Int. Transport Sys. Conf., vol. 6, no. 3, pp. 351356, Sept. 2005. [7] G. Hein, From GPS and GLONASS via EGNOS to Galileo-positioning and navigation in the third millennium, GPS Solutions, vol. 3, no. 4, pp. 3947, Apr. 2000. [8] P. Enge, T. Walter, S. Pullen, C. Kee, Y. Chao, and Y. Tsai, Wide area augmentation of the global positioning system, Proceedings of the IEEE, vol. 84, no. 8, pp. 1063 1088, Aug. 1996. [9] A. Brown and Y. Lu, Preformance test results of an integrated GPS/MEMS inertial navigation package, in Proc. ION GNSS Conf., California,USA, Mar. 2004, pp. 1251 1256. [10] D. Obradovic, H. Lenz, and M. Schupfner, Fusion of sensor data in Siemens car navigation system, IEEE Trans. on Veh. Techn., vol. 56, no. 1, pp. 4350, Jan. 2007. [11] , Fusion of map and sensor data in a modern car navigation system, Journel of VLSI Signal Processing, vol. 45, no. 1-2, pp. 111122, Nov. 2006.

A24

S TATE - OF - THE ART AND FUTURE IN - CAR

NAVIGATION SYSTEMS

A SURVEY

[12] J. Huang and H.-S. Tan, A low-order DGPS-based vehicle positioning system under urban enviroment, IEEE/ASME Trans. Mechatronics, vol. 5, no. 1-2, pp. 567575, Oct. 2006. [13] B. Boberg, Robust navigation, Swedish Journal of Military Technology, no. 3, pp. 2328, 2005. [14] S. Godha and M. Cannon, GPS/MEMS INS integrated system for navigation in urban areas, GPS Solutions, vol. 11, no. 3, pp. 193203, July 2007. [15] J. Marias, M. Berbineau, and M. Heddebaut, Land mobile GNSS availability and multipath evaluation tool, IEEE Trans. on Veh. Techn., vol. 54, no. 5, pp. 16971704, Sept. 2005. [16] J. Farrell and M. Barth, The Global Positioning System and Inertial Navigation. McGraw-Hill, 1998. [17] E. Abbott and D. Powell, Land-vehicle navigation using GPS, Proceedings of the IEEE, vol. 87, no. 1, pp. 145162, Jan. 1999. [18] D. Titterton and J. Weston, Strapdown Inertial Navigation Technology-2nd Edition. IEE, 2004. [19] ESA, http://www.esa.int/esana/index.html, Sept. 2007. [20] M. Grewal, L. Weill, and A. Andrews, Global Positioning Systems, Inertial Navigation and Integration. Wiley, 2001. [21] F. Diggelen, GNSS Accuracy: Lies, Damm Lies, and Statistics, GPS World, no. 1, pp. 2632, Jan. 2007. [22] D. Lapucha, R. Barker, and H. Zwaan, Wide area carrier phase positioning, European Journal of Navigation, vol. 3, no. 1, pp. 1016, Feb. 2005. [23] R. Prieto-Cerdeira, J. Samson, J. Traveset, and B. Rastburg, Ionospheric information broadcasting methods for standardization in SBAS L5, in Proc. IEEE/ION Position, Location, And Navigation Symposium, SAN DIEGO, CA, USA, Apr. 2006. [24] K. Dixon, Satellite positioning systems: Efciencies, Performance and Trends, European Journal of Navigation, vol. 3, no. 1, pp. 5863, Feb. 2005. [25] A. Gorski and G. Gerten, Do we need augmentation systems, European Journal of Navigation, vol. 5, no. 4, pp. 612, Sept. 2007. [26] F. Tor n, J. Jantura-Traveset, R. Luca, C. Echazarreta, C. Seynat, and et al, The a EGNOS data acces system (EDAS), European Journal of Navigation, vol. 5, no. 3, pp. 2025, July 2007. [27] M. Opitz and R. Weber, Development of a SISNeT simulation software-SISSIM, European Journal of Navigation, vol. 4, no. 2, pp. 4448, May 2006. [28] R. Chen and X. Li, Virtual differential GPS based on SBAS signal, GPS Solutions, vol. 8, no. 4, pp. 238244, Sept. 2004. [29] ESA, http://www.egnos-pro.esa.int/publications/fact.html, Sept. 2007. [30] P. Enge and P. Misra, Special issue on global positioning system, Proceedings of the IEEE, vol. 87, no. 1, pp. 315, Jan. 1999. [31] P. Misra, B. Burke, and M. Pratt, GPS performance in navigation, Proceedings of the IEEE, vol. 87, no. 1, pp. 6585, Jan. 1999.

R EFERENCES

A25

[32] H. Lee, J. Wang, C. Rizos, D. Grejner-Brzezinska, and C. Toth, GPS/Pseudolite/INS integration: concept and rts test, GPS Solutions, vol. 6, no. 12, pp. 3446, Oct. 2002. [33] G. Sun, J. Chen, W. Guo, and K. Ray Liu, Signal processing techniques in networkaided positioning: a survey of state-of-the-art positioning designs, IEEE Signal Processing Magazine, vol. 22, no. 4, pp. 1223, July 2005. [34] A. Sayed, A. Tarighat, and N. Khajehnouri, Network-based wireless location: challenges faced in developing techniques for accurate wireless location information, IEEE Signal Processing Magazine, vol. 22, no. 4, pp. 2440, July 2005. [35] F. Gustafsson and F. Gunnarsson, Mobile positioning using wireless networks: possibilities and fundamental limitations based on available wireless network measurements, IEEE Signal Processing Magazine, vol. 22, no. 4, pp. 4153, July 2005. [36] R. Carlson, J. Gerdes, and J. Powell, Practical position and yaw rate estimation with GPS and differential wheelspeeds, in Proc. AVEC 2002 6th Int. Symp. Advanced Vehicle Control, Hiroshima, Japan, Sept. 2002. [37] , Error sources when land vehicle dead reckoning with differential wheelspeeds, Navigation, the J. of The Institute of Navigation, vol. 51, no. 1, pp. 1227, 2004. [38] C. Hay, Turn, turn, turn, GPS world, pp. 3742, Oct. 2005. [39] J. Borenstein and L. Feng, Measurement and correction of systematic odometry errors in mobile robots, IEEE Trans. Robotics and Automation, vol. 12, no. 6, pp. 869880, Dec. 1996. [40] T. Peters, Automobile navigation using a magnetic ux-gate compass, IEEE Trans. on Veh. Techn., vol. 35, no. 2, pp. 4147, May 1986. [41] J. Lenz, A review of magnetic sensors, Proceedings of the IEEE, vol. 78, no. 6, pp. 973989, June 1990. [42] N. Barbour and G. Schmidt, Inertial sensor technology trends, IEEE Sensors Journal, vol. 1, no. 4, pp. 332339, Dec. 2001. [43] J. Bryzek, S. Roundly, B. Bircumshaw, C. Chung, K. Castellino, J. Stetter, and M. Vestel, Marvelous MEMS, IEEE Circuits and Devices Magazine, vol. 22, no. 2, pp. 828, MarchApril 2006. [44] N. El-Sheimy and X. Niu, The promise of MEMS to the navigation cummunity, InsideGPS, pp. 4656, March/April 2007. [45] Chateld, Fundamentals of High Accuracy Inertial Navigation. AIAA, 1997. [46] R. Rogers, Applied Mathematics in Integrated Navigation systems. AIAA Education Series, 2003. [47] P. Aggarwal, Z. Syed, X. Niu, and N. El-Sheimy, Cost-efcient testing and calibration of low cost MEMS sensors for integrated positioning, navigation and mapping systems, in Proc. XXIII FIG Congress, Munich, Germany, Oct. 2006. [48] I. Skog and P. H ndel, Calibration of a MEMS inertial measurement unit, in Proc. a XVII IMEKO WORLD CONGRESS, Rio de Janeiro, Brazil, Sept. 2006. [49] C. Tan and S. Park, Design of accelerometer-based inertial navigation systems, IEEE Trans. Instrumentation and Measurement, vol. 54, no. 6, pp. 25202530, Dec. 2005.

A26

S TATE - OF - THE ART AND FUTURE IN - CAR

NAVIGATION SYSTEMS

A SURVEY

[50] J. Chen, S. Lee, and D. DeBra, Gyroscope free strapdown inertial measurement unit by six linear accelerometers, Journal of Guidance, Control and Dynamics, vol. 17, no. 2, pp. 286290, 1994. [51] P. Savage, Strapdown inertial navigation integration algorithm design, part 1: attitude algorithms, Journal of Guidance, Control and Dynamics, vol. 21, no. 1, pp. 1928, Jan. 1998. [52] , Strapdown inertial navigation integration algorithm design, part 2: velocity and position algorithms, Journal of Guidance, Control and Dynamics, vol. 21, no. 2, pp. 208221, Mar. 1998. [53] M. Kuritsky, M. Goldstein, I. Greenwood, H. Lerman, J. McCarthy, T. Shanahan, M. Silver, and J. Simpson, Inertial navigation, Proceedings of the IEEE, vol. 71, no. 10, pp. 11561176, Oct. 1983. [54] R. Curey, M. Ash, L. Thielman, and C. Barker, Proposed IEEE inertial systems terminology standard and other inertial sensor standards, in Proc. Position Location and Navigation Symposium, 2004. PLANS 2004, Apr. 2004. [55] S. Sukkarieh, E. Nebo, and H. Durrant-Whyte, A high integrity IMU/GPS navigation loop for autonomous land vehicle applications, IEEE Trans. Robotics and Automation, vol. 15, no. 3, pp. 572578, June 1999. [56] G. Dissanayake, S. Sukkarieh, E. Nebot, and H. Durrant-Whyte, The aiding of a low-cost strapdown inertial measurement unit using vehicle model constraints for land vehicle applications, IEEE Trans. Robotics and Automation, vol. 17, no. 5, pp. 731 747, Oct. 2001. [57] X. Niu, S. Nasser, C. Goodall, and N. El-Sheimy, A universal approach for processing any MEMS inertial sensor conguration for land-vehicle navigation, Navigation, J. of The Institute of Navigation, vol. 60, no. 2, pp. 233245, May 2007. [58] D. Bevly, J. Ryu, and J. Gerdes, Integrating INS sensors with GPS measurements for continuous estimation of vehicle sideslip, roll, and tire cornering stiffness, Proc. of IEEE Int. Transport Sys. Conf., vol. 7, no. 4, pp. 483493, Dec. 2006. [59] R. Toledo-Moreo, M. Zamora-Izquierdo, B. beda Miarro, and A. GmezSkarmeta, High-integrity IMM-EKF-based road vehicle navigation with low-cost GPS/SBAS/INS, Proc. of IEEE Int. Transport Sys. Conf., vol. 8, no. 4, Sept. 2007. [60] S. Julier and H. Durrant-Whyte, Process models for the high-speed navigation of road vehicles, in Proc. IEEE International Conference on Robotics and Automation, Nagoya, Japan, May 1995. [61] K. Chiang, H. Hou, X. Niu, and N. El-Sheimy, Improving the positioning accuracy of DGPS/MEMS IMU integrated systems utilizing cascade de-noising algorithm, in Proc.17th International Technical Meeting of the Satellite Division of the Institute of Navigation ION GNSS 2004, Long Beach, CA, USA, Sept. 2004. [62] J. Skaloud, A. Bruton, and K. Schwarz, Detection and ltering of short-term (1/f) noise in inertial sensors, Navigation, the J. of The Institute of Navigation, vol. 46, no. 2, pp. 97108, Summer 1999. [63] K. Chiang, C. Goodall, and N. El-Sheimy, Improving the attitude accuracy of INS/GPS integrated systems, European Journal of Navigation, vol. 4, no. 2, pp. 31 40, May 2006.

R EFERENCES

A27

[64] M. El Najjar and P. Bonnifait, A road-matching method for precise vehicle localization using belief theory and kalman ltering, Auton. Robots, vol. 19, no. 2, pp. 173191, 2005. [65] , Road selection using multicriteria fusion for the road-matching problem, Proc. of IEEE Int. Transport Sys. Conf., vol. 8, no. 2, pp. 279291, June 2007. [66] F. Gustafsson, F. Gunnarsson, N. Bergman, U. Forssell, J. Jansson, R. Karlsson, and P.-J. Nordlund, Particle lters for positioning, navigation, and tracking, IEEE Trans. on Signal Processing, vol. 50, no. 2, pp. 425437, Feb. 2002. [67] D. Bernstein and A. Kornhauser, An Introduction to ing for personal navigation assistants. New Jersy (http://www.njtide.org/reports/mapmatchintro.pdf), 1996. Map MatchTIDE Center

[68] M. Quddus, W. Ochieng, L. Zhao, and R. Noland, A general map matching algorithm for transport telematics applications, GPS Solutions, vol. 7, no. 3, pp. 157167, Dec. 2003. [69] S. Kim and J. Kim, Adaptive fuzzy-network-based c-measure map-matching algorithm forcar navigation system, Trans. on Industrial Electronics, vol. 48, no. 2, pp. 432441, Apr. 2001. [70] M. Quddus, W. Ochieng, and R. Noland, Current map-matching algorithms for transport applications: State-of-the art and future research directions, ELSEVIER, Transportation Research Part C: Emerging Technologies, vol. 15, no. 5, pp. 312328, Oct. 2007. [71] I. Skog and P. H ndel, A low-cost GPS aided inertial navigation system for vehicle a applications, in Proc. EUSIPCO 2005, Antalya, Turkey, Sept. 2005. [72] E. Shin and N. El-Sheimy, Unscented kalman lter and attitude errors of low-cost inertial navigation systems, Navigation, J. of The Institute of Navigation, vol. 54, no. 1, pp. 19, Spring 2007. [73] R. Merwe, E. Wan, and S. Julier, Sigma-point Kalman lters for nonlinear estimation and sensor-fusion: Applications to integrated navigation, in AIAA Guidance, Navigation, and Control Conference and Exhibit, Rhode Island, USA, Aug. 2004. [74] S. Cho and W. Choi, Robust positioning technique in low-cost DR/GPS for land navigation, IEEE Trans. on Veh. Techn., vol. 55, no. 4, pp. 11321142, Aug. 2006. [75] N. Yang, W. Tian, Z. Jin, and C. Zhang, Particle lter for sensor fusion in a land vehicle navigation system, Meas. Sci. Technol., vol. 16, no. 3, pp. 677681, Mar. 2005. [76] J. Wang and Y. Gao, GPS-based land vehicle navigation system assisted by a low-cost gyro-free INS using neural network, Navigation, J. of The Institute of Navigation, vol. 57, no. 3, pp. 417428, Oct. 2004. [77] A. Pascoal, I. Kaminer, and P. Oliveira, Navigation system design using time-varying complementary lters, IEEE Trans. on Aerospace and Electronic Systems, vol. 36, no. 4, pp. 10991114, Oct. 2000. [78] T. Kailath, A. Sayed, and B. Hassibi, Linear Estimation. Prentice Hall, 1999. [79] F. Daum, Nonlinear lters: Beyond the Kalman lter, IEEE Aerospace and Electronic Systems Magazine, vol. 20, no. 8, pp. 5769, Aug. 2005.

A28

S TATE - OF - THE ART AND FUTURE IN - CAR

NAVIGATION SYSTEMS

A SURVEY

[80] T. Sch n, F. Gustfasson, and P.-J. Nordlund, Marginalized particle lters for mixed o linear/nonlinear state-space models, IEEE Trans. on Signal Processing, vol. 53, no. 7, pp. 22792289, July 2005. [81] R. Karlsson, T. Sch n, and F. Gustafsson, Complexity analysis of the marginalized o particle lter, IEEE Trans. on Signal Processing, vol. 53, no. 11, pp. 44084411, Nov. 2005.

Paper B
A low-cost GPS aided inertial navigation system for vehicle applications
Isaac Skog and Peter H ndel a Published in Proceedings of European Signal Processing Conference 2005

c 2005 EURASIP The layout has been revised

A low-cost GPS aided inertial navigation system for vehicle applications

Isaac Skog and Peter H ndel a

Abstract
In this paper an approach for integration between GPS and inertial navigation systems (INS) is described. The continuous-time navigation and error equations for an earthcentered earth-xed INS system are presented. Using zero order hold sampling, the set of equations is discretized. An extended Kalman lter for closed loop integration between the GPS and INS is derived. The lter propagates and estimates the error states, which are fed back to the INS for correction of the internal navigation states. The integration algorithm is implemented on a host PC, which receives the GPS and inertial measurements via the serial port from a tailor made hardware platform, which is briey discussed. Using a battery operated PC the system is fully mobile and suitable for real-time vehicle navigation. Simulation results of the system are presented.

1 Introduction
Today many vehicles are equipped with global positioning system (GPS) receivers that constantly can provide the driver with information about the vehicles position with an accuracy in the order of 15-100 meters [1]. However, the GPS receiver has two major weaknesses. The slow update rate, only once a second for most receivers and the sensitivity to blocking of the satellite signals. An inertial navigation system (INS) is an alternative tool for positioning and navigation. A classical reference on low-cost INS for mobile robot applications is [2]. In the opposite of the GPS receiver an INS is self contained and can provide position, velocity and attitude estimates at a high rate, typically 100 times per second [3]. However, due to the integrative nature of the INS, low frequency noise and sensor biases are amplied. The unaided INS may therefore have unbounded position and velocity errors [2]. These complementary properties make an integration of the two systems suitable, which is the topic of this paper. The primary motivation for the reported work is the in-house need for a GPS aided INS test-bed for education and research in the area of vehicle navigation and performance analysis. A goal is to develop a hardware platform and additional software, which together with a standard host-computer will work as a basic GPS aided INS. From the software skeleton the student/researcher can then build an application meeting their demands. Related work

B2

LOW- COST

GPS

AIDED INERTIAL NAVIGATION SYSTEM FOR VEHICLE APPLICATIONS

INS input

INS H
-

Navigation output

KF

GPS input

Figure 1: Loosely coupled position aided closed loop implementation of a GPS aided INS system. KF denotes the extended Kalman lter, and H the map between navigation output and GPS data.

do exist, for example in [4] a eld evaluation of a low-cost GPS aided INS installed in a car is presented. In [4], the strap-down INS is integrated with two different GPS solutions (pseudo range and carrier phase differential GPS, respectively) using a Kalman lter. In this paper a loosely coupled position aided method is proposed which allows the designer to keep the costs low by using an off-the-shelf GPS receiver that provides position estimates employing NMEA (National Marine Electronics Association) data transmission protocol and sentence format. In Section 2, the INS equations and the corresponding error models are introduced. Next the navigation dynamics are discretized in Section 3. Section 4 presents an indirect extended Kalman lter (EKF) for the integration between the GPS and INS data. The INS provides the reference trajectory and output. The EKF then estimates the errors, which are fed back to the INS for correction of its internal states, resulting in a closed loop integration, see Figure 1. Implementation aspects, simulations results and conclusions are presented in Section 5.

2 Navigation Dynamics
A strap-down INS comprises two distinguished parts. The inertial measurement unit (IMU) housing the accelerometers and gyros. The computational part, consisting of several differential equations, translates the measurements into position, velocity and attitude estimates. The calculations are performed in two steps. From the gyro measurements the directional cosine matrix relating the body coordinate frame to the used navigation frame is propagated. The coordinate rotation matrix is then used when solving the differential equations relating accelerations in the body frame to the navigation coordinate system. An earthcentered earth-xed (ECEF) navigation coordinate system implementation of the INS has the advantage of producing positions estimate in the same coordinate system as used by the GPS system, which simplies the integration between the two systems [1].

2.1

Navigation equations

The continuous-time navigation equations in the ECEF frame are [5]

2 NAVIGATION DYNAMICS

B3

re ve b Re

= ve = Re f b 2 e v e + g e b ie = R e b b eb (1)

where re and ve denote the position and velocity in 3-dimensional ECEF coordinates, respectively. The superscripts e, b and i (that will be used below) are used to denote in which coordinate frame a variable is resolved in, that is the ECEF, body or inertial frame. Further, f b is the measured acceleration in the body-frame, g e is the position dependent, but known earth acceleration in ECEF coordinates, that may be compensated for. The rotation matrix, Re transforms a vector in the body frame to the ECEF frame. Although R e has nine elb b ements, it has only three degrees of freedom and can be uniquely described by the three Euler angles, in the sequel gathered in the vector [6]. The matrices b and e are the ie eb b e skew-symmetric matrix representations of the angular rates eb and ie , dened such that b e a eb = b a and a ie = e a, where a is a 3 1 dimensional vector. The matrix ie eb b reads eb b = eb 0 b ebz b eby
b ebz 0 b ebx b eby b ebx 0

b b b where the elements ebx , eby and ebz are the angular rates of the body (vehicle) frame relative to the ECEF frame, resolved in the body navigation frame. The matrix, e has the ie b structure of (2) with the components of eb replaced by the corresponding components of e e the earth rotational rate ie . Since variations in the earth rotational rate ie are neglectable, e b ie is assumed constant and known, while eb depends on the body to ECEF angular b b b b rates, eb = [ebx eby ebz ] and thus is time-varying. Here () denotes the transpose operation. The body to ECEF angular rates are obtained by subtracting the angular rate of b the earth, resolved in body coordinates from the gyro outputs ib , that is e b e eb = ib Rb ie e

where the rotation matrix from the ECEF to body coordinates is Rb = (Re )1 = (Re ) e b b (4)

The last equality is a result of the fact that the directional cosine matrix is an orthonormal matrix (that is, Re Re = I). b b With reference to Figure 1, the INS inputs are the 3-dimensional measured acceleration in the body frame f b and the 3-dimensional angular rates of the body frame with respect b to the inertial frame of reference ib . Further the navigation outputs of Figure 1 are the position, velocity and Euler angles of (1).

2.2

Error equations

Even though the inertial instruments have been calibrated the measured IMU signals will be erroneous, due to environmental variations and instrument degradation. As a result, there are biases in the position and velocity estimates as well as a misalignment between the estimated and true coordinate rotation matrices. The IMU measurement errors can be modelled as a random level, and white Gaussian noise [5], describing the bias and the

(2)

(3)

B4

LOW- COST

GPS

AIDED INERTIAL NAVIGATION SYSTEM FOR VEHICLE APPLICATIONS

measurement noise, respectively. Here the IMU sensors are assumed to be the only noise sources in the system. Hence, the noise enters the system equations only through the attitude and velocity state, that is the two last equations in (1). Dening the error state vector x(t) and the measurement noise vector uc (t) as

x(t) =
e

re

ve u (t) acc

f b

b ib

(5) (6)

uc (t) =

u (t) gyro

where r denotes the error in position, et cetera. The vector is the small angle rotations aligning the actual navigation frame to the computed one. Further, u acc (t) denotes the accelerometer noise and ugyro (t) the gyro noise, respectively. Then, if neglecting gravity errors, the navigation error equations can be written as [5] x(t) = F(t) x(t) + G(t) uc (t) where F(t) is the 15 15 matrix 03 03 03 03 03 I3 2e ie 03 03 03

(7)

F(t) =

03 Fe e ie 03 03 03 Re b 03 03 03 03 03 Re b 03 03

03 Re b 03 03 03

03 03 Re b 03 03

(8)

and G(t) is of size 15 6

G(t) =

(9)

The error equations (7) are time-varying, since Re depends on the attitude and Fe (as b dened below) on the acceleration of the vehicle. In (8), I3 (03 ) denotes the unity (zero) matrix of order 3 and Fe is the skew symmetric matrix, dened as Fe = 0 e f3 e f2
e f3 0 e f1 e f2 e f1 0

where f e denotes the acceleration along the :th coordinate axis in the ECEF frame. The constructed IMU platform houses three separate accelerometers and gyros, therefore the sensor noises are assumed uncorrelated [7]. However, the accelerometers respectively gyros are of the same model and thus assumed to have similar noise characteristics. 2 2 Let acc and gyro denote the variance of the accelerometer and the gyro noise, respectively. Then the covariance matrix, Qc (t) of the Gaussian measurement noise uc (t) in (6) is given by
2 acc I3 03

where ( ) is the Kronecker delta.

E{uc (t + ) u (t)}= c

03 ( ) = Qc ( ) 2 gyro I3

(10)

(11)

3 D ISCRETIZATION

B5

3 Discretization
The implementation of a GPS aided INS system requires that the navigation and error equations are discretized. First the navigation equations are discretized, where special care is taken to preserve the properties of the rotation matrix. Next the zero-order-hold sampling of the error equation is described.

3.1

Discrete time navigation equations

e r e = r e + T s vk k+1 k e vk+1

Zero-order sampling of the position and velocity equations in (1) results in (12)
e vk

e vk

b Ts (Re fk b,k

2e ie

+g )

(13)

When discretizing the attitude equation in (1) care must be taken so that the orthogonality constraints of the directional cosine matrix are maintained. Let Ts denote the sampling interval and assume that b is constant. Then the matrix taking the solution of the attitude eb differential equation from time instant k Ts to (k+1) Ts is exp(b Ts ). Hence, the attitude eb equations can be approximated by
e b Re b,k+1 = Rb,k exp(eb Ts )

(14)

By expanding the matrix exponential into an (n, n) Pad` approximation the orthogonality e constraints of the rotation matrix are preserved [1]. Using a (2, 2) Pad` approximation the e discrete attitude equation becomes
1 b b e Re b,k+1 = Rb,k (2I3 + eb Ts )(2I3 eb Ts )

(15)

3.2

Discrete time error equations

Having a continuous-time equation as in (7) with a known solution at time t 0 , the solution at a time t > t0 can be represented as [8, 9].
t

where the state transition matrix (t, t0 ) is dened as the unique solution to (t, )/t = F(t) (t, ). If the state transition matrix F(t) in (8) is assumed time invariant, the homogenous differential equation has the solution of the matrix exponential function, that is (t, ) = exp (F (t t0 )) [9]. In the case of F(t) being time varying, F(t) can be approximated as a constant matrix F between the sampling instants, if the sample rate is high compared to the rate of change in F(t). Using the power series denition of the matrix exponential, the state transition matrix between time instants kTs and (k + 1)Ts can be approximated as ((k + 1)Ts , kTs ) I15 + F(kTs )Ts Hence, the discrete-time error equation becomes (17)

x(t) = (t, t0 ) x(t0 ) +

(t, ) G( ) uc ( )d
t0

(16)

B6

LOW- COST

GPS

AIDED INERTIAL NAVIGATION SYSTEM FOR VEHICLE APPLICATIONS

xk+1 = k xk + ud,k

(18)

where the state transition matrix k = ((k + 1)Ts , kTs ) is approximated as in (17) and the discrete-time process noise, uk is
(k+1)Ts

Since ud,k is a linear combination of Gaussian noise, it is Gaussian distributed and described by its rst and second order moments. The mean of ud,k is zero, since uc (t) is assumed zero mean. Applying the denition of covariance and assuming T s small, the covariance of the discrete-time noise Qd,k can be approximated as [1] = G(kTs ) Qc (kTs ) G (kTs ) Ts
2 2 diag(03 , acc I3 , gyro I3 , 06 )

Qd,k

where diag() denotes a block diagonal matrix. The last equality is a result of the orthonormality property of the rotation matrix Re , and that Qc (t) is a diagonal matrix according b to (11). The denition of the state observation equation is straightforward since the GPS position estimate is used, and not the pseudo ranges. Let y be the difference between the GPS and INS position estimate and wd,k the error in the GPS position estimates. Then the observation equation can be written as yk = Hk xk + wd,k with the state observation matrix Hk of size 3 15, dened as


Hk =

where denotes the ratio between the INS and GPS sampling frequency.

4 Extended Kalman Filtering

The discrete non-linear navigation equations (12), (13) and (15) can be written as zk+1 = c(zk , ak ) + uk (23)

where c(, ) denotes the dynamics, zk is the navigation system outputs: position, velocity and Euler angles dening the rotation matrix Re , that is the 9-element vector b

Further, the navigation system input is the 6-element vector ak which contains the inputs to navigation system, accelerations and angular rates, that is

ud,k =

((k + 1)Ts , s)G(s)uc (s)ds

kTs

(19)

(20)

(21)

[I3 0312 ], 0315 ,

k = n , n = 1, 2, 3... otherwise

(22)

zk =

re k

e vk

(24)

ak =

b fk

b ib, k

(25)

4 E XTENDED K ALMAN F ILTERING

B7

The vector uk is the measurement noise of the navigation inputs. Linearization of the navigation equations (23) are rst done around a known nominal trajectory, resulting in a linear model for the perturbations away from the true trajectory. To the linear error equations the standard Kalman lter is applied. Then substituting the nominal trajectory with that of the INS estimated trajectory results in an extended Kalman lter. Consider the true state vector zk and the measured input ak to the system written as zk = znom + zk k ak = anom + ak k znom k anom k (26) (27)

where and are the nominal trajectory and input, respectively. The quantity z k is the perturbation away from the true trajectory and ak the bias of the measurements. Assuming that zk and ak are small and applying a rst order Taylor series expansion of c(z, a), equation (23) can be approximated as znom + zk+1 c(znom , anom ) + C1,k zk + C2,k ak + uk k+1 k k where C1,k = c(z, a) z
   

(28)

C2,k =
z=znom k

The Jacobians of c(z, a) are updated with nominal trajectory and input for each sample. Choosing znom and anom to fulll the deterministic difference equation k k znom = c(znom , anom ) k+1 k k and substituting (30) into (28) results in a linear model for the error z k , that is zk+1 = C1,k zk + C2,k ak + uk


, and thus it becomes clear that C1,k and C2,k correspond Note that xk = to the upper part of the navigation error state transition matrix k . The lower 6 6 block matrix of k is a description of how the IMU biases ak develop with time. Since this is a linear model the standard Kalman lter equations can be applied to estimate x k [9]. The Kalman lter equations read z k a k

Kf,k = P H (Hk P H + Rd,k )1 k k k k Pk = (I Kf,k Hk ) P k P = k Pk + Qd,k k k+1

Here k denotes the estimated biases in the measurements, et cetera. Variables with a a minus sign, () are predicted values, and those with superscript nom the one obtained from (30). The matrix Rd,k is the covariance matrix of the error wd,k in the GPS position

 

k z z znom zk = k +Kf,k yk Hk k nom Hk ak k k a a ak

   

zk+1 ak+1

= k

k z k a

  

c(z, a) a

(29)
a=anom k

(30)

(31)

(32)

(33) (34) (35) (36)

B8

LOW- COST

GPS

AIDED INERTIAL NAVIGATION SYSTEM FOR VEHICLE APPLICATIONS

No GPS data available. (k = 100, 200, ...) k a = ak + ak k+1 zk k z = c( , a ) = [k ]10:15,10:15 ak+1 ak Pk+1 = k Pk k + Qd,k GPS data available. (k = 100, 200, ...) Kf,k = P H (Hk P H + Rk )1 k k k k 091 k k z z = + Kf,k yk Hk k a k a 061 k z zk = z + k ak = ak + k a Pk = (I Kf,k Hk ) P k k+1 z = c(k , ak ) z a = [k ]10:15,10:15 k ak+1 P = k Pk + Qd,k k k+1
Table 1: The algorithm for integration between GPS and INS data, with a ratio between the sample rates equal to 100 times.

estimates yk . Now adding znom to both sides of equation (32) and substituting znom with k k the current estimate in all equations result in an extended Kalman lter, where the time and lter update for the estimates are given below k+1 z = c(k , ak ) z = [k ]10:15, 10:15 k ak+1 a


(37) (38) k z 061 (39)

 

The solution to (37) is provided by the INS, since this corresponds to the navigation equa tions. The vector ak is the estimate of the true IMU-signal obtained by subtracting the estimated bias from the measured IMU signal. The only obstacle is the time update of navigation state errors k . If the estimated navigation error states are fed back to the INS for z correction of the INS internal states, the corresponding error states can be set to zero [6]. Hence, = 091 . The nal algorithm for the integration is given in Table 1. zk

5 Design and Conclusions

Discrete navigation equations for a direct ECEF INS implementation and the corresponding error model have been derived. Further, an indirect extended Kalman lter algorithm for integration between the position estimates from an off-the-shelf GPS receiver and the INS has been presented.

k z k a

zk ak

+ Kf,k

yk H k

5 D ESIGN

AND

C ONCLUSIONS

B9

frag replacements

IMU
RS232 RS232

GPS

Host PC

Figure 2: Block diagram of the hardware.

5.1

Hardware Design

A GPS aided INS platform has been developed in-house, consisting of an off-the-shelf GPS receiver and an in-house IMU platform. The IMU platform comprises state-of-the-art MEMS gyros and accelerometers, and a micro-controller to control the data acquisition. The micro-controller controls the GPS-receiver via an RS232 serial interface. The GPS and INS data are synchronized and sent over a second RS232 serial interface to the host PC, see Figure 2. Using a battery operated PC the system is fully mobile and able to perform realtime signal processing [7]. However, at current state procedures for calculating the different calibration parameters are yet to be implemented and therefor no eld tests are available. Below follows a short evaluation of the system for simulated IMU data, corresponding to a typical driving scenario.

5.2

Simulation results

The superiority of the GPS aided system over traditional GPS is illustrated in Figure 3. In Figure 3 the dashed trajectory is the position estimates generated by simulated data as input to the GPS aided INS system. The shown specks are the GPS position and the solid line is the true trajectory. A ratio of 100 times was used between the INS and GPS sample ratio and the GPS position estimates had a standard deviation of ten meters. The biases of the accelerometers and gyros were in the order of 1-2 cm/s2 respectively 5-10 /h. Not surprisingly, the GPS aided system clearly outperforms the GPS-system. Our current work is focused on studying and implementing different sensor error models and calibration methods, making the test-bed available for eld tests. More detailed performance evaluations and results from eld tests will be reported elsewhere.

B10

LOW- COST

GPS

AIDED INERTIAL NAVIGATION SYSTEM FOR VEHICLE APPLICATIONS

150
GPS reading

100

INS/GPS estimate

North [m]

50
True trajectory

PSfrag replacements

50 0 50 100 150 200 250

East [m]
Figure 3: Estimated and true trajectory of a typical driving sequence. First the car is stationary for 100 seconds. Then it makes a wide turn and accelerates to 18 km/h, which it keeps until after the last turn. Finally the car slows down and stops. Worth observing is that the accelerometer biases estimates have converged already before the car starts moving, while the gyro biases have not converged until after the last turn.

R EFERENCES

B11

References
[1] Q. Honghui and J. Moore, Direct kalman ltering approach for GPS/INS integration, IEEE Trans. on Aerospace and Electronic Systems, vol. 38, no. 2, pp. 687693, Apr. 2002. [2] B. Barshan and H. Durrant-Whyte, Inertial navigation systems for mobile robots, IEEE Trans. Robotics and Automation, vol. 11, no. 3, pp. 328342, June 1995. [3] S. Hong, F. Harashima, S. Kwon, S. Choi, M. Lee, and H. Lee, Estimation of errors in inertial navigation systems with GPS measurements, in Proc. ISIE 2001, IEEE International Symposium on Industrial Electronics, Pusan, South Korea, June 2001. [4] R. Dorobantu and B. Zebhauser, Field evaluation of a low-cost strapdown IMU by means GPS, Ortung und Navigation, no. 1, pp. 5165, June 1999. [5] Chateld, Fundamentals of High Accuracy Inertial Navigation. AIAA, 1997. [6] J. Farrell and M. Barth, The Global Positioning System and Inertial Navigation. McGraw-Hill, 1998. [7] I. Skog, Development of a low cost gps aided ins for vehicles, Masters thesis, Dept. of Signals, Sensors and Systems, Royal Institut of Technology (KTH), Sweden, 2005. o [8] K. Astr m and B. Wittenmark, Computer Controlled Systems: Theory and Design. Prentice Hall, 1984. [9] T. Kailath, A. Sayed, and B. Hassibi, Linear Estimation. Prentice Hall, 1999.

Paper C
A Versatile PC-Based Platform For Inertial Navigation
Isaac Skog, Adrian Schumacher and Peter H ndel a Published in Proceedings of IEEE Nordic Signal Processing Symposium 2006

c 2006 IEEE The layout has been revised

A Versatile PC-Based Platform For Inertial Navigation

Isaac Skog, Adrian Schumacher and Peter H ndel a

Abstract
A GPS aided inertial navigation platform is presented, into which further sensors such as a camera, wheel-speed encoder etc., can be incorporated. The construction of the platform is described and an introduction to the sensor fusion approach is given. Results from a eld-test is presented, indicating which error sources that needs to be modelled more accurately.

1 Introduction
The art of car racing is not only a question of having the most highly performable car, but also depends on the skills of the driver and the interaction between the driver and vehicle. This is a well known fact in professional car racing, such as Formula 1, where the behavior of both the car and the driver are constantly monitored for later analysis [1]. For other applications, such systems are far to costly and complex. However, a GPS aided inertial navigation system (INS) constructed around an off-the-shelf GPS receiver and a low-cost inertial measurement unit (IMU) could provide a lot of information about the behavior of the vehicle, for an acceptable cost. Apart from the direct information (position, velocity, acceleration and attitude) calculated by the GPS aided INS, more indirect information can be extracted as well. In [2], a vehicle movement visualizer together with a gear change detector based on the measured acceleration and attitude is described and implemented. In [3], the road bank angle and vehicle roll are estimated with a GPS aided INS. An inertial navigation system is a dead reckoning navigation system, where the systems position, velocity and attitude are continuously calculated through integration of the accelerations and angular rates measured by the IMU. However, due to the integrative nature of the system, the INS has poor long-term accuracy. Small errors caused by biases and noise in the sensors accumulate with time and cause an unbounded position error. To the opposite of the INS is the GPS receiver an absolute navigation system and therefore has a bounded error. However, the GPS navigation solution is noisier than that of an INS, has a lower update rate ( 1 Hz) and normally does not include attitude information [4]. By fusing data from a GPS and an INS, utilizing the two systems complimentary properties, a robust navigation system with both high accuracy and update rate may be obtained.

C2

A V ERSATILE PC-BASED P LATFORM F OR I NERTIAL NAVIGATION

During the last decade many papers about low-cost GPS aided systems for land vehicles have been written, see for example [5, 6] and the references therein. Most of the described work focus on the implemented algorithm used in the sensor fusion, and few give an overview of the total system and its construction and features. This paper gives an overview and presents preliminarily results from the construction of a portable low-cost GPS aided INS platform, intended to be used for example in car racing and driver training. The platform is constructed with the aim of creating a GPS aided INS application skeleton into which further sensors such as a camera, wheel-speed encoder etc., can easily be incorporated. The outline of the paper is as follows: rst, an overview of the platform is given in Section 2, describing its major building blocks and their features. Second, in Section 3 the sensors used in the navigation system are described. Focus is on the in-house constructed IMU hardware and its synchronization with the GPS-receiver. Next, in Section 4 the software algorithm used to fuse the sensor data is briey described. In Section 5, preliminarily results from a rst eld test are presented, indicating which error-sources that need to be more accurately modelled. Finally, in Section 6 conclusions and areas for further work are discussed.

2 System Overview
The navigation platform in Figure 1 is designed with the aim of being both portable and robust, but still inexpensive. It is constructed around ve blocks: signal processing, power supply, communication, IMU/GPS and the external sensors, see Figure 2. The heart of the navigation platform is a Pentium-M 1.7 GHz PC, 512 MB RAM, 60 GB HD equipped with a 7 TFT touch screen. The PC handles all signal processing and interaction with the user. The PC and screen together with the IMU, power supply and wireless communication modules are built into an aluminium case, resulting in a robust system. The power supply module allows the platform to use three different power sources: the mains outlet (230 V AC), an external 11-15 V DC source, or internal batteries. The internal batteries allow for 2 hours stand alone operation of the navigation platform. The wireless communication module consists of a WLAN and GSM/GPRS connection, for realtime transmission of navigation data to remote applications as well as input from wireless sensors. Furthermore, an external GPS receiver is connected via a micro-controller to the PC. The micro-controller synchronizes the sampling of the GPS and IMU, as described in Section 3. The described hardware platform provides a skeleton for the development of a easy to use navigation platform.

3 Sensors
The navigation platform is designed around the IMU and a GPS-receiver. However more sensors can be incorporated into the system through the USB-ports on the front of the navigation platform. Here, two auxiliary sensors are under special consideration, a vehicle speed encoder and a camera. The vehicle speed encoder gives the possibility to include a vehicle model in the sensor-fusion [7], improving the system performance in the absence of GPS-data. The camera provides visual information about the drivers reaction in different situations.

3 S ENSORS

C3

Figure 1: The front of the navigation platform with its touch screen. In the upper left corner there are four USB-, a RCA- and a RS232 serialconnector, for connection of external sensors, an external screen and a GPS receiver, respectively.

C4

A V ERSATILE PC-BASED P LATFORM F OR I NERTIAL NAVIGATION

PSfrag replacements

IMU/GPS
IMU C GPS

Signal processing

External sensors
USB port

PC

Power supply
Battery DC-DC converter

Communication
TouchWLAN GSM/GPRS screen

220V to 12V

Figure 2: Block-diagram of the navigation platform. The platform is constructed around ve modules.

With reference to Figure 3, the IMU has been constructed in-house around MicroElectro-Mechanical System (MEMS) accelerometers and gyros from Analog-Devices. The double and single axled accelerometers ADXL 203 and ADXL 103, respectively have been mounted so that their sensitivity axes are nearly orthogonal and span a 3-dimensional space. Due to impreciseness in the construction, the sensitivity axes will not be perfectly orthogonal. The three ADXRS 150 gyros are mounted to measure the rotational velocity around the accelerometers sensitivity axes. This gives a six degree-of-freedom IMU capable of measuring accelerations and angular rates between 1.5[g] and 150[ /s], respectively. The sensors are controlled and sampled at a rate of 100 Hz by an AVR, ATmega 8 microcontroller, housing a built in 10-bit analog to digital converter. Further, the micro-controller also communicates with the GPS-receiver via a RS232 serial connection, synchronizing the sampling of the IMU and GPS. However, this synchronization does not take into account the processing delay inside the GPS-receiver, which may cause erroneous convergence of the integration algorithm [8]. The IMU and GPS data is enumerated by the controller and sent to the PC via a second RS232 serial connection. Due to impreciseness in the mounting and construction of the MEMS sensors the IMU has some statical errors such as misalignment between the sensitivity axes, scale factors and biases [9]. These errors may be identied and compensated for, reducing the convergence time as well as the complexity of the integration algorithm. A description of a suitable IMU sensor model together with an evaluation of different algorithms for estimating the model parameters are given in [9, 10].

4 Software Algorithm
The algorithm for fusion of the INS and GPS data has previously been presented in [11], where a more thorough description can be found. In the used integration algorithm, the iner-

4 S OFTWARE A LGORITHM

C5

Gyroscope x-axis Gyroscope y-axis Accelerometer z-axis Accelerometer x- and y-axis

PSfrag replacements

60 m

60 mm
Gyroscope z-axis Micro controller
Figure 3: The inhouse constructed inertial measurement unit. In the upper part of the picture the three gyros and the double axed and single axed accelerometer can be seen. In the lower part of the picture the microcontroller can be seen, responsible for sampling of the sensors. Altogether the IMU measures 606025 mm.

C6

A V ERSATILE PC-BASED P LATFORM F OR I NERTIAL NAVIGATION

PSfrag replacements

EKF INS ip p navigationp f equations estimated errors KF f e e r e ve r

IMU

Output

e r
+

GPS

Figure 4: The INS provides the main navigation solution. When GPS data is available the position error is estimated and used as the input to a Kalman lter, estimating the navigation errors. The errors are fed back to the INS for correction of internal states.

tial navigation system provides the main navigation solution. Since the INS is an integrative process the output of the INS is the actual position and a predominantly low-frequency error. When a GPS position estimate is available the difference between the position estimate of the GPS and the INS is calculated. This error signal contains two noise components: the predominantly low-frequency INS component and the predominantly high-frequency GPS component [6]. The error signal is fed to a Kalman lter (KF), designed to attenuate the GPS measurement error and provide an estimate of the INS errors. Hence, the KF will mainly have a low-pass characteristic. The estimated INS error state is the feedback to the INS for correction of its internal states, see Figure 4. The INS and KF together forms an indirect extended Kalman lter (EKF), where the navigation equations are linearized around the current navigation output. The continuous Earth-Centered-Earth-Fixed (ECEF) navigation equations used in the INS system to calculate position re , velocity ve and attitude from the accelerations f p p and angular rates ip are re v p Re
e

= ve = Re p f
p

(1) 2 e ie v +g
e e

(2) (3)

= R e p . p ep

Here and in the sequel, the superscripts e, p, and i denote in which coordinate-frame a quantity is expressed, the ECEF, platform and inertial frame, respectively. Further, Re = Re () denotes the directional cosine matrix transforming a vector from platformp p to ECEF-coordinates. Note that the attitude can be calculated from Re and vice versa. p

4 S OFTWARE A LGORITHM

C7

INS Navigation Equations

ge Acc fp Re p fe Gravity re

e r

t+t ()dt t

re

t+t ()dt t

Navigation Position

IMU

2 e ve ie
t+t ()dt t p ep

Coriolis force

e ie , earth rotation

latitude longitude h height

Navigation Attitude
p ie

Gyros

p ip

roll pitch yaw

Rp e

e ie , earth rotation

re

Figure 5: The block diagram corresponds to ECEF navigation equations used in the INS.

p The matrices p and e are the skew symmetric matrix representations of (ep ) and ep ie e (ie ), respectively. Here () denotes the cross-product operator. The rotational rate bep tween the ECEF- and platform-coordinates, expressed in platform coordinates ep may be p p e e calculated from the gyro measurements as ep = ip Rp ie . The earth rotational rate e ie is constant and may be calculated beforehand. The interpretation of the navigation equations is illustrated in Figure 5. The angular rates measured by the gyros are used to keep track of the coordinate rotation matrix R e , b i.e.olving the difference equation in (3), transforming the specic-force measured in the s platform-frame into a specic-force in the ECEF-frame. Next, the gravity g e and coriolis force 2e ve is subtracted. Remaining is then only the acceleration of the platform. Inteie grating the acceleration twice with respect to time, then the position in ECEF coordinates is obtained. Through mechanization of the navigation equations and neglecting second and higher p order terms a linear model relating the sensor errors f p and ip to the errors re , ve and in the output of the INS may be found. Dening the errors as the perturbation between the calculated value and true value, respectively measured value and true value, then the error equations reads

ve v
e e

= re = Fe + Re f p + ge 2 e ve p ie = Re p
p ip

(4) (5) (6)

e e

e ie

Here F is the skew symmetric matrix representation of (f ) and g is the error in the gravity vector. Worth noting is that the error equations depend on the current navigation

C8

A V ERSATILE PC-BASED P LATFORM F OR I NERTIAL NAVIGATION

state, i.e. Re and f e . In [11] the navigation error equations are discretized and complip mented with a model for how the sensor errors develop with time. A standard Kalman lter algorithm is applied to the error model, resulting in an indirect extended Kalman lter where the navigation equations are linearized around the output of the INS.

5 Results

0m

1000m

Figure 6: Estimated trajectory (blue solid line) and GPS-receiver position estimates (red crosses) from the eld-test. The vehicle was kept stationary (see top) for the rst 30 seconds and then drove along the highway southward.

In Figure 6, the estimated trajectory (blue solid line) and GPS-receiver position estimates (red crosses) from a eld test of the navigation platform is shown. The vehicle was kept stationary for 30 seconds, then made a wide turn and headed along a highway for approximately 3 minutes. No GPS outages occurred during that time, although the position dilution of precision (PDOP) reached values as high as 40 at a few points. 72% of the time the PDOP value was below 5, which can be considered as acceptable.

6 C ONCLUSIONS AN F URTHER W ORK

C9

The IMU sensor errors were modelled as constants in the integration algorithm, i.e. a bias in the measured accelerations and rotational rates. In Figure 7, the estimated sensor biases from the eld test are shown. After approximately 120 seconds, the accelerometer biases have converged. However, there are convergence problems for the gyro biases and a linear growth in the heading error. Possible error sources can be identied in the misalignment of the gyros and a delay between the GPS estimates and IMU samples [8], respectively. An exact position error evaluation is not possible due to the lack of reference trajectory data.

6 Conclusions an Further Work

A versatile GPS aided inertial navigation platform, into which more sensors easily can be incorporated has been described. The use of a standard PC as a signal processing engine gives a navigation platform with extensive processing power for a low-cost, as well as the possibilities to run other applications. Further, the use of a PC allows the navigation software developers to develop the software modules for new sensors on their own computer, and then port it to the navigation platform without the need to consider the hardware specic properties Concerning the navigation algorithm, the error model needs to be extended to take into account more error sources such as the sensors scale factors and the delay in the GPS receiver. In addition, through pre-calibration of the IMU the misalignment between the accelerometer and gyro sensitivity axes should be estimated and compensated. Further work will focus on incorporating a vehicle model together with a velocity encoder into the sensor fusing algorithm. Methods for online estimation of the GPS processing delay will be studied.

C10

A V ERSATILE PC-BASED P LATFORM F OR I NERTIAL NAVIGATION

1 0.5 0

Accelerometer biases
x-axis y-axis z-axis

Bias [m/s2]

0.5 1

0
3

20

40

60

80

5
PSfrag replacements

x 10

Time [s] Gyro biases

100

120

140

160

180

200

Bias [rad/s]

x-axis y-axis z-axis

3 2 1 0 1 0 20 40 60 80 100 120 140 160 180 200

Time [s]

Figure 7: Estimated accelerometer and gyro biases during the eld test. The accelerometer biases start converging directly, while the gyro biases are unobservable when the vehicle is stationary during the rst 30 seconds.

R EFERENCES

C11

References
[1] L. Boland, Formula 1 racing: The Xilinx advantage, Xcell Journal, pp. 4649, Fall 2003. [2] H. Grunell, Visualization and analysis of vehicle movement and driving performance using inertial navigation, Masters thesis, Dept. of Signals, Sensors and Systems, Royal Institut of Technology (KTH), Sweden, 2005. [3] J. Ryu and J. Gerdes, Estimation of vehicle roll and road bank angle, in Proc. Amer. Cont. Conf., Boston, MA, USA, June 2004. [4] B. Boberg, Robust navigation, Swedish Journal of Military Technology, no. 3, pp. 2328, 2005. [5] R. Dorobantu and B. Zebhauser, Field evaluation of a low-cost strapdown IMU by means GPS, Ortung und Navigation, no. 1, pp. 5165, June 1999. [6] J. Farrell, T. Givargis, and M. Barth, Real-time differential carrier phase GPS-aided INS, IEEE Trans. on Control Systems Technology, vol. 8, no. 4, pp. 709721, July 2000. [7] G. Dissanayake, S. Sukkarieh, E. Nebot, and H. Durrant-Whyte, The aiding of a low-cost strapdown inertial measurement unit using vehicle model constraints for land vehicle applications, IEEE Trans. Robotics and Automation, vol. 17, no. 5, pp. 731 747, Oct. 2001. [8] H. Lee, J. Lee, and G. Jee, Calibration of measurement delay in global positioning system/strapdown inertial navigation system, Journal of Guidance, Control and Dynamics, vol. 25, no. 2, pp. 240247, Mar. 2002. [9] A. Kim and M. Golnaraghi, Initial calibration of an inertial measurement unit using an optical position tracking system, in Proc. PLANS 2004, IEEE Position Location and Navigation Symposium, Monterey, CA, USA, Apr. 2004. [10] I. Skog, A. Schumacher, and P. H ndel, A versatile PC-based platform for inertial a navigation, in Proc. NORSIG 2006, Reykjavik, Iceland, June 2006. [11] I. Skog and P. H ndel, A low-cost GPS aided inertial navigation system for vehicle a applications, in Proc. EUSIPCO 2005, Antalya, Turkey, Sept. 2005.

Paper D
Calibration of a MEMS inertial measurement unit
Isaac Skog and Peter H ndel a Published in Proceedings of XVII IMEKO World Congress 2006

c 2006 IMEKO The layout has been revised

Calibration of a mems inertial measurement unit

Isaac Skog and Peter H ndel a

Abstract
An approach for calibrating a low-cost IMU is studied, requiring no mechanical platform for the accelerometer calibration and only a simple rotating table for the gyro calibration. The proposed calibration methods utilize the fact that ideally the norm of the measured output of the accelerometer and gyro cluster are equal to the magnitude of applied force and rotational velocity, respectively. This fact, together with model of the sensors is used to construct a cost function, which is minimized with respect to the unknown model parameters using Newtons method. The performance of the calibration algorithm is compared with the Cram r-Rao bound for the case when a mechanical platform is used to rotate the IMU into e different precisely controlled orientations. Simulation results shows that the mean square error of the estimated sensor model parameters reaches the Cram r-Rao bound within 8 dB, e and thus the proposed method may be acceptable for a wide range of low-cost applications.

1 Introduction
The development in micro-electro-mechanical system (MEMS) technology has made it possible to fabricate cheap single chip accelerometer and gyro sensors, which have been adopted into many applications where traditionally inertial sensors have been too costly. For example the MEMS sensors have made it possible to construct low cost global navigation satellite system (GNSS) aided inertial navigation systems (INS) for monitoring vehicle behavior [1]. The obtained accuracy and convergence time of a GNSS aided INS is highly dependent on the quality of the IMU sensors output [2], and therefore a calibration of the IMU is critical for the over all system performance. Traditionally the calibration of an IMU has been done using a mechanical platform, turning the IMU into different precisely controlled orientations and at known rotational velocities [35]. At each orientation and during the rotations the output of the accelerometers and gyros are observed and compared with the precalculated gravity force and rotational velocity, respectively. However, the cost of a mechanical platform can many times exceed the cost of developing and constructing a MEMS sensor based inertial measurement unit. Therefore, in [6] a calibration procedure using a optical tracking system is studied. In [4, 7] calibration procedures for the accelerometer cluster, where

D2

C ALIBRATION

OF A

MEMS

INERTIAL MEASUREMENT UNIT

the requirements of a precise control of the IMUs orientation is relaxed are proposed. These calibration methods utilize the fact that ideally the norm of the measured output of the accelerometer and gyro cluster should be equal to the magnitude of the applied force and rotational velocity, respectively. However there are one major disadvantage with such a method; not all sensor parameters of the IMU are observable. This implies that these parameters (errors sources) most be taken into account in the integration of the IMU and GNSS data, increasing the computational complexity of the data fusion algorithm. In this paper the problem of calibrating a low cost IMU when the precise orientation of the IMU is unknown is studied. In Section 2, a sensor model applicable both to the accelerometer and gyro sensor cluster are described and related to the in-house constructed IMU. Next, in Section 3 the estimation of the model parameters when the precise orientation of the IMU is unknown is studied and a cost function is proposed. The cost function is numerically minimized using Newtons method. In Section 4, the Cram r Rao lower e bound (CRLB) for the parameter estimation problem is derived when the orientation of the platform is precisely controlled, serving as a bound when evaluating the performance of the estimator. In Section 5, results from a Monte Carlo simulation of the proposed calibration approach is presented. Moreover, the in-house IMU is calibrated, and the estimated model parameters are examined. The conclusions are drawn in Section 6.

2 Sensor Error Model

An inertial measurement unit has been constructed around MEMS accelerometers and gyros from Analog-Devices, see Figure 1. The double and single axed accelerometers ADXL 203 and ADXL 103, respectively have been mounted so their sensitivity axes {x a , y a , z a } span a three dimensional space. The three ADXRS 150 gyros are mounted to measure the angular velocities {x , y , z } around the accelerometers sensitivity axes, see Figure 1.2(a). This gives a six degree-of-freedom IMU capable of measuring accelerations and angular rates between 15[m/s2 ] and 150[ /s], respectively. Ideally, the sensor sensitivity axes should be orthogonal, but due to the impreciseness in the construction of the IMU this is seldom the case [6]. If the nonorthogonal sensitivity axes of the accelerometer clusters differ only by small angles from the orthogonal set of platform coordinate axes, see Figure 1.2(b), the specic force in accelerometer cluster coordinates can be transformed into specic force estimates in platform coordinates as [8] 1 xz xy yz 1 yx zy zx 1

sp = T p sa , a

Tp = a

(1)

where sp and sa denote the specic force in platform and accelerometer coordinates, respectively. Here ij is the rotation of the i-th accelerometer sensitivity axis around the j-th platform axis. By dening the platform coordinate system so that the platform coordinate axis xp coincides with the xa accelerometer sensitivity axis, and so that the y p axis is lying in the plane spanned by xa and y a the angles {xz , xy , yx } become zero. That is (1) is reduced to

2 S ENSOR E RROR M ODEL

D3

Gyroscope x-axis Gyroscope y-axis Accelerometer z-axis Accelerometer x- and y-axis

PSfrag replacements

60 m

60 mm
Gyroscope z-axis Micro controller
Figure 1: The in-house constructed inertial measurement unit. In the upper part of the picture the three gyros and the double axed and single axed accelerometer can be seen. In the lower part of the picture the microcontroller can be seen, responsible for sampling of the sensors. Altogether the IMU measures 606025 mm.

D4

C ALIBRATION

OF A

MEMS

INERTIAL MEASUREMENT UNIT

za
za zp

z y
zy
PSfrag replacements

zx yx yz yp ya

ya

PSfrag replacements

xy xz

x xa
(a) Accelerometer and gyro sensitivity axes.

xp xa

(b) Accelerometer and platform coordinate axes.

Figure 2: The accelerometer sensitivity axes {xa , y a , z a } are mounted to span a 3-dim space and the gyros to measure the angular velocities {x , y , z } around these axes. The nonorthogonal axes of accelerometer cluster can be aligned with the orthogonal platform axes {xp , y p , z p } through the six angles {xy , xz , yx , yz , zx , zy }.

sp = T p sa , a

Tp = a

1 0 0

yz 1 0

zy zx 1

Because the platform coordinate axes already have been dened, six small rotations around the platforms axes are required to dene the rotation from the gyro input axes to the platform axes. That is, three rotations are required to make the sensitivity axes of the gyro cluster orthogonal and three rotations are needed to align the orthogonal coordinate axes with the platform coordinate axes. The relationship reads
g p ip = Tp ig , g

Tp = g

1 xz xy

yz 1 yx

zy zx 1

Here ij is the small rotation of the i-th gyro sensitivity axis around the j-th platform axis. This may equivalently be written as
p g ip = Rp To ig , o g

To = g

1 0 0

yz 1 0

zy zx 1

where To transforms the nonorthogonal gyro sensitivity axes into a set of orthogonal g coordinate axes. The matrix Rp denotes the directional cosine matrix transforming the o

(2)

(3)

(4)

2 S ENSOR E RROR M ODEL

D5

V (f ) [V olt]

V f
PSfrag replacements

non linearity {Ka }i =

V f

bias f [g]

Figure 3: The relationship between the output voltage of the accelerometer(gyro) and the measured force(angular rate) is modelled as a linear function, describing the scaling and bias of the sensors.

angular velocities in orthogonal sensitivity axes coordinates into platform coordinates. The MEMS sensors output a voltage proportional to the physical quantities sensed by the sensors, acceleration and angular rates, respectively. The typical relationship between the output voltage and the physical quantity acting along the sensor sensitivity axes is given by the manufactures data sheet, but the true scaling varies between different specimens and with the input signal (due to inherent nonlinearities of the sensors). Moreover, there is often a small bias in the sensor output signal, that is even though there is no force acting onto the sensor, the sensors produces a non-zero output, see Figure 3. For the MEMS sensors used in our application the nonlinearities are in the order 0.1% of a best t to a straight line and may therefore be neglected. Introduce the accelerometer scale factor matrix K a and bias vector ba dened as Ka = diag(kxa , kya , kza ), b a = [ b x a b y a b z a ]T (5)

where kia and bia are the unknown scaling and bias of the i-th accelerometer output, respectively. Further ()T denotes the transpose operation and diag() the diagonal matrix with the elements given within the parentheses. The measured output of the accelerometer

D6

C ALIBRATION

OF A

MEMS

INERTIAL MEASUREMENT UNIT

cluster may then be modelled as [4] a = Ka (Tp )1 sp + ba + va s a

a

(6)

where s = from (2) was employed. In (6), va is a noise term reecting the measurement noise from the sensors. Applying the same model to the MEMS gyros, the output from the gyro-cluster may be written as (Tp )1 sp a ig g
g Kg ig + bg + vg p Kg (To )1 Ro ip + bg + vg g p

= =

(7)

where Kg and bg are the scale factor matrix and bias vector of the gyro-cluster, respecp tively. Further ip is the true rotational rate of the IMU platform with respect to the inertial frame of reference and vg is the gyro measurement noise. In the second equality in (7) the notation Ro = (Rp )1 has been used to denote the directional cosine matrix, p o rotating a vector from platform coordinates to the coordinate system associated with the orthogonal gyro sensitivity axes. Worth noting is that the misalignment matrices T p and a To are constant matrices only dependent on the physical mounting of the components. g Ka , Kg , ba and bg may be split into a static part, a temperature varying and a random drift part [9]. The temperature varying and random drift part must be taken into account by the integration algorithm, fusing the GNSS and IMU data. Therefore the prime goal of the calibration is to estimate Tp and To and the static parts of the scale factors and biases. a g Both the accelerometer and gyro cluster model t into the more general signal model described by Figure 4. Here the input uk corresponds to the specic force sp at time k in p o the accelerometer cluster model or the angular velocity ip = Ro ip in the gyro cluster p model.

3 Calibration
Traditionally, a mechanical platform rotating the IMU into different precisely controlled orientations and angular rates has been used to calibrate IMUs. Then, observing the output yk and the precalculated specic force or angular velocity uk acting upon the IMU for 12 or more different orientations and rotation sequences, respectively it is straightforward to estimate the misalignment, scaling and bias [35]. Note, that there are 9 and 12 unknowns in the signal models, respectively - three scale factors, three biases, three orthogonal rotations and in addition for the gyro cluster the three rotations aligning the orthogonal gyro coordinate axes with the platform axes. The cost of a mechanical platform often exceeds the cost of developing and constructing a MEMS sensor based IMU. Therefore a calibration procedure is desirable where the requirements of a precisely controlled orientation of the IMU can be relaxed. Based upon the signal model in Figure 4 the natural estimator for the sought input u k based on the sensor output yk is uk = h(yk , ) = T K1 (yk b) (8)

PSfrag replacements

3 C ALIBRATION

D7

b Sought physical quantity uk

vk yk Sensor output

T1

Figure 4: Sensor model including misalignments T1 , scale factors K, biases b, and measurement noise vk .

where the sought parameters are collected in the parameter vector

kx ky kz yz zy zx bx by bz 2

(9)

In order to have a more unied notation throughout the paper the noise variance has been included in the parameter vector in (9). However, the proposed estimator does not depend on the noise variance and it can therefore be omitted in equations (8)-(15). Ideally, independent of the orientation of the IMU, the magnitude of the measured gravity force and angular velocity should be equal to the magnitude of the apparent gravity force and applied angular velocity, respectively. Therefore, the squared error between the squared magnitude of the input uk and the squared magnitude of the output from the compensated IMU output may serve as a cost function when calibrating the IMU. That is = argmin{L()}

(10)

where L() =

K1

k=0

Here, K = M N , where M is the number of orientations or rotations that the platform is exposed too and N the number of samples taken during each rotation or at each orientation. Still, since there are nine unknowns the platform must be exposed to nine or more orientations and rotations, respectively. However, the demand of a precise control of the orientation is relaxed. Worth noting when calibrating the gyros is that uk
2 o = ip 2 p = Ro ip p 2 p = ip 2

where in the last equality the fact that the directional cosine matrix Ro is an orthonormal p matrix, ie (Ro )T (Ro ) = I, has been used. Therefore the three Euler angles relating p p the orthogonal coordinate axes of the gyro cluster and the platform coordinates are unobservable when the magnitudes are used to calibrate the IMU. At each orientation of the IMU, the specic force acting along the accelerometers sensitivity axes are constant, i.e. the input uk is constant during the N samples. This also holds for the gyro cluster model, if assuming that during each rotation of the IMU the rotation velocity is kept constant. The cost function may then be simplied as
M 1

L() =

m=0

( uk

h(yk , ) 2 )2 .

(11)

(12)

( um

h(ym , ) 2 )2

(13)

D8

C ALIBRATION

OF A

MEMS

INERTIAL MEASUREMENT UNIT

where um and ym are the input and sample mean at the m-th orientation and rotation, respectively. This reduced cost function may be minimized off-line using for example Newtons method, that is [10] d2 L() k+1 = k + d d T

1

0 domain of attraction. The cost function in (11) has several local optima and to ensure that the recursion (14) converges to the true parameters the search for the minima must be initialized with a in the domain of attraction of the global minima. According to the data sheet of the accelerometers the scale factors differ less then 6% from there nominal values (unit gain) and the biases are smaller then 1[m/s2 ]. Further the misalignments can be assumed small. Therefore an appropriate initial value for the parameter vector may be
 

0 =

1 1 1 0 0 0 0 0 0 2

4 Cram r Rao Lower Bound e

When evaluating the performance of an estimator often it is of interest to compare the obtained estimation error with that of an optimal (unbiased with minimum variance) estimator. The optimal estimator may not exist, be unknown, or too complex to implement, still the performance of the optimal estimator only depends on the properties of the signal model [11], and may therefore be calculated independently. Given the probability distribution function of the observed data, the Cram r-Rao bound (CRLB) sets the lower limit for e the variance of the estimation error for all unbiased estimators. The parametric CRLB is given by [12] var() = diag(J1 ()) where J()ij = E (16)

log(p(y; )) (17) i j is the Fisher information matrix. Further p(y; ) is the probability density function for the observed data y, parameterized by . The measured output y k in Figure 4 may be described as yk = (, uk ) + vk where the signal part (, uk ) reads (, uk ) = K T1 uk + b. (19)


The measurement noise vk is assumed to be zero mean, Gaussian distributed and uncorrelated both between the sensors and in time. Collecting the measurements y k and signal parts (, sk ) into two vectors, y=

T y0 T y1

...

T yM (N 1)

 

dL() d

(14)
=k

(15)

(18)

(20)

5 R ESULTS

D9

and


() = (, u0 )T (, u1 )T . . . (, uM (N 1) )

(21)

where N is the number of samples at each orientation of the platform and M the number of orientations, the vector y will be Gaussian distributed as y N ((), C()). Assuming the variance of the noise from different sensor specimens to be equal, then C() = 2 I3M N . Here I3M N is the identity matrix of size 3M N . The Fisher information matrix for the general Gaussian case is given by [12] () i
 
T

C() C() 1 . tr C()1 C()1 2 i j

Here tr(), denotes the trace operation. Noting that C() = 2 I3M N in the signal model, equation (17) simplies to 1 2

3M N 2 2 . 2 4 i j

Further, by utilizing that uk is constant for all N samples at each orientation, the Fisher information matrix may be written as N 2
M

J()ij where

Am + ij

m=1

3M N 2 2 2 4 i j

In Appendix A, the elements in the matrix Am for the proposed signal model are given. Equation (16) together with (24) and (25) give the CRLB for the parameter estimation problem, under the assumption of Gaussian measurement noise and when the precise orientation of the IMU is known, i.e. there is full knowledge about uk . However, in the considered calibration approach the precise orientation of the IMU is unknown but the bound still provides a good benchmark when evaluating the performance of the estimator.

5 Results
5.1 Performance Evaluation
The proposed calibration approach has been evaluated by Monte Carlo simulations. In the simulation, the estimation of the accelerometer cluster parameters were studied when the

Am = ij

(, um ) i

(, um ) . j

J()ij

() i

() j

J()ij =

C()1

() j

(22)

(23)

(24)

(25)

D10

C ALIBRATION

OF A

MEMS

INERTIAL MEASUREMENT UNIT

Table 1: Settings used in the Monte Carlo simulation. The settings were chosen too reect the specications in the data sheet for the ADXL 103 accelerometer. The noise variance 2 where set to 0.0095 [m/s2 ], which corresponds to a sensor bandwidth of 30 [Hz].

Axis x y z

Scaling 1.05 0.93 1.06

Bias [m/s2 ] 0.32 0.63 -0.32

Axis yz zy zx

Misalignment [ ] 2 -5 3

Table 2: IMU results. The average estimate of the accelerometer cluster parameters, calculated from 20 calibration of the in-house constructed IMU. At each calibration the platform was rotated into 18 different orientation and at each orientation the sensors were sampled 100 times.

Axis x y z

Scaling 0.998 0.996 1.008

Bias [m/s2 ] -0.435 0.254 0.099

Axis yz zy zx

Misalignment [ ] 0.026 -0.695 1.808

IMU was rotated into 18 different orientations, as proposed in [4]. However, a uniformly distributed error between [5 , 5 ] was added to the proposed orientations to reect the relaxed demands of a precise orientation. In Figure 5, 6 and 7 the empirical mean square error for the scale-factors, misalignment angles and biases estimates are shown, calculated from 1000 simulated calibrations using the proposed calibration approach. The solid line is the CRLB for the calibration, when the the precise orientation of the IMU is known. In Table 1, the settings used in the simulation are summarized. As can be seen from Figure 5, 6 and 7 the performance of the proposed calibration procedure is approximately 2, 8 and 5 decibel bellow the CRLB for the scale factors, misalignments and biases, respectively. Still the root mean square error, when 100 samples are taken at each orientation, of the estimated parameters are less than 10 2 of the magnitude of the parameters, and may therefore normally be considered acceptable for low-cost applications.

5.2

Calibration of IMU

The accelerometer cluster of the in-house constructed IMU has been calibrated using the proposed method. The IMU was by hand placed into 18 different positions, the six sides and the twelve edges of the IMU. At each orientation 100 samples were taken. In Table 2, the average parameter estimate out of 20 calibrations are shown. All the obtained estimates are in the expected region for the used sensors, that is the scale factor diverge less than 6% from the unit gain and the biases are less then 1[m/s2 ]. The estimated misalignment angle yz , as can be seen in Figure 1.2(b) corresponds to the misalignment between the x- and y-axis in the IMU, which should be close to zero. This is due to the fact that the x- and y-accelerometers are mounted into the same MEMS sensor case. The remaining two misalignments angles correspond to the misalignment between the x- and z-axis respectively yand z-axis, which due to the impreciseness when the IMU was constructed, should be much

6 C ONCLUSIONS

D11

larger. See Figure 1.

6 Conclusions
A MEMS sensor based inertial measurement unit has been constructed in-house, intended to be used in a low-cost GNSS aided inertial navigation systems. In order to improve the performance of the GNSS aided INS, which is highly dependent on the accuracy of the IMU, an approach for calibrating the IMU, requiring no mechanical platform for the accelerometer calibration and only a simple rotating table for the gyro calibration has been studied. The performance of the calibration algorithm is compared with the Cram r-Rao e bound for the traditional case when a mechanical platform is used to calibrate the IMU, rotating the IMU into different precisely controlled orientations. Simulation results shows that the mean square error of the parameter estimates of the senor model increases with up to 8 decibel, when utilizing the proposed method. Further, not all parameters in the gyro sensor model are observable with the proposed calibration approach, increasing the computational complexity of the GNSS aided INS. Still the proposed method can be considered acceptable and useful for many low-cost applications where the cost of constructing a mechanical platform many times can exceed the cost of developing the inertial measurement unit.

D12

C ALIBRATION

OF A

MEMS

INERTIAL MEASUREMENT UNIT

90

Scale factors

85

10log10 (1/M SE)

PSfrag replacements

80

75

70

kx ky kz
CRB

65 0.2

0.4

0.6

0.8

1.2

1.4

1.6

1.8

10log10 (N )
Figure 5: Empirical mean square error for the estimation of the scale-factors as a function of the number of samples at each orientation, using the proposed calibration approach. The legend CRB indicates the Cram r e Rao lower bound for the case when the precise orientation of the IMU is known.

6 C ONCLUSIONS

D13

90

Misalignment

85

10log10 (1/M SE)

80

75

PSfrag replacements

70

65

yz zy zx
CRBx CRBy CRBz

60 0.2

0.4

0.6

0.8

1.2

1.4

1.6

1.8

10log10 (N )
Figure 6: Empirical mean square error for the estimation of the misalignment angles as a function of the number of samples at each orientation, using the proposed calibration approach. The legend CRB indicates the Cram r Rao lower bound for the case when the precise orientation e of the IMU is known.

D14

C ALIBRATION

OF A

MEMS

INERTIAL MEASUREMENT UNIT

75

Bias

70

10log10 (1/M SE)

PSfrag replacements

65

60

55

bx by bz
CRB

50 0.2

0.4

0.6

0.8

1.2

1.4

1.6

1.8

10log10 (N )
Figure 7: Empirical mean square error for the estimation of the biases as a function of the number of samples at each orientation, using the proposed calibration approach. The legend CRB indicates the Cram r Rao e lower bound for the case when the precise orientation of the IMU is known.

A PPENDIX A

D15

Appendix A
The nonzero elements for Am are Am = (um + zy um + (yz zx zy )um )2 1,1 x y z Am = (um + zy um + (yz zx zy )um )(kx um + zx kx um ) x y z y z 1,4 Am = (um + zy um + (yz zx zy )um )(kx um ) 1,5 x y z z Am = (um + zy um + (yz zx zy )um )(yz kx um ) 1,6 x y z z Am = (um + zy um + (yz zx zy )um ) 1,7 x y z Am = (um + zx um )2 2,2 y z Am = (um + zx um )ky um 2,6 y z z Am = (um + zx um ) 2,8 y z Am = (um )2 z 3,3 Am = (um ) z 3,9 Am = (kx um + zx kx um )2 4,4 y z Am = (kx um + zx kx um )(kx um ) 4,5 y z z Am = (kx um + zx kx um )(yz kx um ) y z z 4,6 Am = (kx um + zx kx um ) 4,7 y z Am = (kx um )2 5,5 z Am = (kx um )(yz kx um ) 5,6 z z Am = (kx um ) z 5,7 Am = (yz kx um )2 + (ky um )2 6,6 z z Am = (yz kx um ) 6,7 z Am = (ky um ) z 6,8 Am = A m = A m = 1 7,7 8,8 9,9

References
[1] I. Skog, A. Schumacher, and P. H ndel, A versatile PC-based platform for inertial a navigation, in Proc. NORSIG 2006, Reykjavik, Iceland, June 2006. [2] N. El-Sheimy, S. Nassar, and A. Noureldin, Wavelet de-noising for IMU alignment, IEEE Aerospace and Electronic Systems Magazine, vol. 19, no. 10, pp. 3239, Oct. 2004. [3] R. Rogers, Applied Mathematics in Integrated Navigation systems. AIAA Education Series, 2003. [4] Chateld, Fundamentals of High Accuracy Inertial Navigation. AIAA, 1997. [5] J. Hung, J. Thacher, and H. White, Calibration of accelerometer triad of an imu with drifting z -accelerometer bias, in Proc. NAECON 1989, IEEE Aerospace and Electronics Conference, Dayton, OH, USA, May 1989.

D16

C ALIBRATION

OF A MEMS INERTIAL MEASUREMENT UNIT

[6] A. Kim and M. Golnaraghi, Initial calibration of an inertial measurement unit using an optical position tracking system, in Proc. PLANS 2004, IEEE Position Location and Navigation Symposium, Monterey, CA, USA, Apr. 2004. [7] Z. Wu, Z. Wang, and Y. Ge, Gravity based online calibration of monolithic triaxial accelermeters gain and offset drift. in Proc. 4-th World Congress on Intelligent Control and Automation, Shanghai, China, June 2002. [8] K. Britting, Inertial Navigation Systems Analysis. Wiley Interscience, 1971. [9] M. Grewal, L. Weill, and A. Andrews, Global Positioning Systems, Inertial Navigation and Integration. Wiley, 2001. [10] S. Boyd and L. Vandenberghe, Convex Optimization. Cambridge, 2004. [11] N. Bergman, Recursive bayesian estimation, navigation and tracking applications, Ph.D. dissertation, Dept. of Electrical Engineering, Linkopings Univeristy, 1999. [12] S. Kay, Fundamentals of Statistical Signal Processing, Estimation Theory. Hall, 1999. Prentice

Paper E
Time synchronization errors in GPS-aided inertial navigation systems
Isaac Skog and Peter H ndel a Submitted to IEEE Transactions on Intelligent Transportation Systems

c 2007 IEEE The layout has been revised

Time synchronization errors in GPS-aided inertial navigation systems

Isaac Skog and Peter H ndel a

Abstract
The effects of time synchronization errors in a GPS-aided inertial navigation system (INS) are studied in terms of the increased error covariance of the state vector. Expressions for evaluating the error covariance of the navigation state vector given the vehicle trajectory and the model of the INS error dynamics are derived. Two different cases are studied in some detail. The rst case considers a navigation system in which the timing error is not included in the integration lter. This leads to a system with an increased error covariance and a bias in the estimated forward acceleration. In the second case, a parameterization of the timing error is included as a part of the estimation problem in the data integration. The estimated timing error is fed back to control an adjustable fractional delay lter, synchronizing the inertial measurement unit (IMU) and GPS-receiver data. Simulation results show that by including the timing error in the estimation problem, almost perfect time synchronization is obtained and the bias in the forward acceleration is removed. The potential of the proposed method is illustrated with tests on real-world data that are subjected to timing errors. Moreover, through an observability analysis, it is shown that the timing error is observable for all trajectories that include turns or non-zero accelerations.

1 Nomenclature
Subscripts and superscripts ()n ()p ()f ()ext ()f d ()c ()k

Nomenclature
Quantity in navigation coordinates. Quantity in platform coordinates. Quantity in the system with feedback. Quantity in the system with a state model extended to also include the time synchronization error. Quantity in the feedback system with time delay. Continuous time variable Time index, discrete time.

E2

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

[]i:j ()T ()a () k Scalars M Ts Td Td D 2 2 2 acc , gyro , gps Vectors x x u u z = [xT uT ]T d e w r p(t), pk v(t), vk a(t), ak s(t), sk

Element i to j in a vector. Transpose operator. Actual value. Predicted value. Measured value. Estimated value. Average value. Length of the state vector z. Length of the position vector p. Tilt (pitch) angle. Ratio between the sampling rate of the IMU and the GPS-receiver. Sampling period of the IMU. Time synchronization error. Perturbation in the estimate of the time synchronization a error (i.e., Td = Td Td ). Normalized time synchronization error Variance of the accelerometer, gyro, and GPS measurement noise, respectively. State vector, containing the position, velocity, and attitude of the navigation system. Perturbation in the state vector (i.e., x = xa x). Input vector, containing the accelerations and angular rates of the navigation platform. Perturbation in the estimated sensor errors (i.e., u = ua u). Extended state vector. Bias vector due to the time synchronization error. IMU measurement noise. GPS measurement noise. Difference between the position estimated by the GPS and INS. Vector of Euler angles. Position vector in continuous and discrete time, respectively. Velocity vector in continuous and discrete time, respectively. Acceleration vector in continuous and discrete time, respectively. Specic-force vector in continuous and discrete time, respectively.

2 I NTRODUCTION

E3

(t), k Matrices Ij 0i,j (0i ) Kp K H G P Q R Wo Functions xc = fc (xc , uc ) xk+1 = f (xk , uk ) i,j E{} S(a) (k)

Angular-rate vector in continuous and discrete time, respectively. Identity matrix of size j. Zero matrix of size i j (i i). Kalman prediction gain matrix. Kalman lter gain matrix. State transition matrix Observation matrix. Process noise matrix. Covariance matrix. Process noise covariance matrix Observation noise covariance matrix. Observability matrix. Continuous time navigation equations. Discrete time navigation equations. Kronecker delta function. Expectation operator. Skew symmetric matrix representation of the vector a. That is a b = S(a) b. Downsampling function.

2 Introduction
One of the rst problems encountered when investigating the practical construction of lowcost GPS-aided inertial navigation systems (INS) based on commercial off-the-shelf components (COTS) is the problem of time synchronization in the sampling of the GPS-receiver and inertial measurement unit (IMU). Since in most cases there is no direct access to the hardware of the sensor modules and no possibility of modifying their internal software, the data synchronization between GPS and IMU must be done in an ad-hoc manner. Therefore, one often encounters a small timing error between the sampling instances of the GPSreceiver and the IMU. The setup considered is illustrated in Fig. 1. The timing error gives rise to several questions: what is the impact on the accuracy of the navigation system? how large can the errors be for a given accuracy? and how to estimate and compensate for it? The impact of data timing errors in integrated navigation systems was rst studied in [1], where it was shown that the data timing errors may cause a bias in the forward acceleration during the transfer alignment of a strap-down INS. In [2, 3] and [4], the effects of using timedelayed GPS measurements in a GPS-aided strap-down INS were studied by examining the steady-state condition of the Kalman lter that was used to fuse navigation data. Further, in [2], a calibration procedure was proposed based on driving the vehicle with the GPSaided INS according to a predened trajectory. A few methods for synchronizing the data streams at the hardware level are described in [5, 6] and [7]. In this paper, the impact of a time synchronization error in a closed-loop GPS-aided INS is studied in terms of the increased error covariance of the system. In Section 3, by modeling the timing error as a trajectory-dependent bias in the GPS-receivers position estimates and by studying the behavior of navigation errors in the closed-loop GPS-aided INS,

E4

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

PSfrag replacements

IMU accelerometers gyros

t = k Ts

Navigation t = l T s Td , l = k M Filter

Position Velocity Attitude

GPS

Figure 1: Effects of the timing error Td in the synchronization between the sampling of the GPS-receiver and IMU are studied in terms of the navigation systems increased error covariance. Here, the ratio between the sampling rate of the IMU and GPS-receiver is assumed to be an integer m 1.

a stochastic difference equation for the navigation errors in the system with timing error is found. From this equation, an expression for the error covariance of the closed-loop system with timing error is derived. In Section 4, the case when the timing error is included in the estimation problem is studied. By including the timing error in the feedback loop of the navigation lter, almost perfect time synchronization is achieved. The general theoretical ndings in Section 3 and 4 are illustrated by an example of a single-axis GPS-aided INS in Sections 3.3 and 4.1, respectively. In Section 5, a practical approach to the implementation of the time synchronization, using an adjustable fractional delay lter to delay the IMU data, is described. The proposed time synchronization approach, using a fractional delay lter, is included as a part of the in-house developed software for simulations and tests for low-cost GPS-aided INSs. In Section 6, the integration software and the time synchronization are tested with both simulated and real-world data, subjected to synchronization errors. In Section 7, the observability of the timing error is examined. Finally, in Section 8, conclusions are drawn and the results summarized.

3 Covariance of the estimation error

To evaluate the effects of the time synchronization error on the performance of the navigation system, the method that is used to fuse the sensor data must be specied. In this paper, a feedback implementation of a complementary lter is used to fuse the navigation data. That is, given a linear state space model of how the position, velocity, attitude, and sensor errors in the mechanization of the INS develop with time, a standard Kalman lter is used to estimate the error state of the INS. The estimated navigation errors are then fed back into the INS to correct its internal states. Since the model for the error plant of the INS depends on the current navigation state, an indirect extended Kalman lter is created. Hence, to evaluate how the timing error will affect the performance of the navigation sys-

3 C OVARIANCE

OF THE ESTIMATION ERROR

E5

+1

= f (xk xk , uk )

IMU

uk

Sensor error compensation

uk

Navigation Equations

xk+1
Hk

uk

xk
Kk

pINS k+1

GPS

pGPS k+1

Figure 2: Block diagram of a closed-loop GPS-aided INS model. The EKF recursions used to fuse the GPS-receiver and the IMU data may be interpreted as a way of choosing the gain matrices Kk in the feedback.

tem, the state-space model of the error plant and the extended Kalman lter recursions must be studied. Introduce a state vector zk of the form
!

zk =

where x contains the position, velocity, attitude perturbation of the INS, and u k the IMU sensor errors. Then the error plant of the INS can be described by a state-space model of the form zk+1 = k zk + Gk ek (2)

where the state transition matrix k describes how the errors develop from one time instance to another and can be found by a perturbation analysis or linearization of the mechanized navigation equations and sensor models. Refer to the standard text-books [810], or [11] for more details on how this is done. Further, the process noise gain matrix G k describes how the measurement noise ek from the inertial sensors affects the navigation error. Since the GPS and IMU in most cases are sampled at two different rates, introduce the following down-sampling function
#

(k) =

1, 0,

k = nM, n = 1, 2, ... . otherwise

Here, M denotes the ratio between the sampling rate of the IMU and the GPS-receiver, and is taken to be an integer. If the difference between the position estimates of the GPS and the INS is observed, the observation equation for the state-space model is given by rk = Hk zk + (k)wk where the observation matrix Hk is dened as Hk = (k)[I 0,() ]. (5) (4)

"

xk uk

(1)

(3)

E6

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

Table 1: The indirect extended Kalman lter algorithm for integration between GPS-receiver and IMU data, with a ratio between the sample rates equal to M times.

No GPS data available. (k = n M , n = 1, 2, ...) u = uk + u k k x = fd (x , u ) k+1 k k u = u k k+1 P = k P T + G k Q G T k k k+1 k GPS data available. (k = n M , n = 1, 2, ...) Kk = P HT (Hk P HT + Rk )1 k k k k 01 xk x GPS k = Hk ) + Kk (pk uk uk 01 xk = xk + xe k uk = uk + uk Pk = (I Kk Hk ) P k x = fd (xk , xk ) k+1 u = uk k+1 P = k P k T + G k Q G T k k k+1
The integers and denote the length of the state vector zk and position vector rk , respectively. The notations I and 0,() denote a unity matrix of size and a ( ) matrix with zeros, respectively. The GPS-receiver positioning error 1 wk and the measurement noise ek of the inertial sensors are considered as white noises, uncorrelated with each other and with covariance matrices
T R = E[wk wk ]

(6)

and Q = E[ek eT ]. k (7)

For an error model of the form (2)(7) the corresponding extended Kalman lter recursions, used to fuse the IMU and GPS-receiver data in a closed-loop system, are given in Table 1, as derived in [12]. This set of equations may graphically be interpreted as the closed-loop system in Fig. 2, where the feedback gain is chosen as the Kalman lter gain.

3.1

Closed-Loop Error

To nd a difference equation for the error in the closed-loop system described by the indirect extended Kalman lter algorithm in Table 1, it should be noted that when GPS data is available, the current error state is estimated as
1 In reality, the errors in the positioning estimates of the GPS-receiver are correlated both in time and space with a covariance structure depending on the positioning algorithm of the GPS-receiver [8].

3 C OVARIANCE

OF THE ESTIMATION ERROR

E7

zk = K k rk .

(8)

That is, the current perturbation in position, velocity, and attitude x k and the current error in the estimated sensor perturbation uk are equal to the position error times the Kalman lter gain. This error estimate is used to correct the current navigation state of the INS before the time update. Thus, the state error after the feedback is the error before feedback zf,k minus the estimated error zk . With reference to (2), describing how the state errors relate from one time instance to the next, the error of the closed-loop system z f,k is found to be

zf,k+1

= =

k (zf,k zk ) + Gk ek k zf,k k Kk rk + Gk ek . (9)

Equation (9) can be written in a more convenient form by using the denition of the observation vector rk in (4) and by noting that Kp,k = k Kk is the one-step-ahead Kalman prediction gain. Thus (9) may be rewritten as zf,k+1 = = k zf,k Kp,k (Hk zf,k + (k) wk ) + Gk ek (k Kp,k Hk ) zf,k Kp,k wk + Gk ek . (10)

In the second equality, one used that the Kalman gain Kk per denition, and hence the prediction gain Kp,k are zero matrices, whenever the downsampling function (k) in (3) is zero. Equation (10) can be identied as the difference equation describing the error of the one-step-ahead Kalman lter predictor. Indeed, it is shown in [13] that for a linear system, the optimal choice of the feedback gain is the Kalman predictor gain, resulting in a system with an error covariance equal to that of the linear Kalman predictor. Thus, the error covariance of the system with feedback is given by the one-step-ahead Kalman predictor error covariance. In summary, the stochastic difference equation (10) describes how the errors in the estimated position, velocity, attitude, and sensor errors propagate with time in the closedloop implementation of a GPS-aided INS. The dynamics of the errors in the system are determined by the time varying closed-loop system matrix (k Kp,k Hk ), which depends on the trajectory dynamics of the navigation platform. Therefore, analytical results about the convergence of the errors in the navigation system are difcult to present. Further, the error covariance of the closed-loop system is given by the one-step-ahead Kalman predictor covariance and not, as rst may be expected, the Kalman lter covariance. This occurs because the system cannot instantaneously respond to the feedback control [13].

3.2

Timing Errors in Closed-Loop

Now consider the case when there is a timing error between the sampling of the GPSreceiver and the IMU, so that their sampling instants are not perfectly aligned (see Fig. 1). Let this constant misalignment in the sampling instant be denoted Td and assume that it occurs in the sampling of the GPS-receiver. Further, let the function p(t) describe the true trajectory of the navigation platform. Then the position estimates from the GPS-receiver at time k can be described as

E8

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

pGPS = (k)(p(k Ts Td ) + wk ) k

(11)

where wk is the estimation error of the GPS-receiver. Assume that a rough synchronization of the GPS-receiver and IMU data has been done by means of correlation or at hardwarelevel, so that |Td | is in the order of a few sampling periods Ts . Then p(k Ts Td ) in (11) can be approximated by a second-order Taylor expansion around t = k T s , that is,
2 Td (12) 2 where pk , vk , and ak denote the navigation platform position, velocity, and acceleration at sampling instance k, respectively. The symbol denotes equality where only the signicant terms have been retained. Inserting (12) into (11) gives

p(k Ts Td )

p k v k Td + a k

Thus, the true observation ra of the system, which is fed back into the closed-loop system, k can be described as ra k = (k)(pGPS pINS ) k k (k)(pk vk Td + ak
2 Td + wk pINS ). k 2

Here pINS is the position estimate from the INS at time k. Next, identifying that Hk ze = k k (k)(pk pINS ) and substituting it into (14) yields the following description of the true k observation for the system with a timing error ra k (k)(Hk zk + wk + dk ). (15)

In (15), the position error vector dk is introduced as

2 Td . (16) 2 By comparing (15) and (4) it is clear that the actual observation contains not only the position error and measurement noise but also a bias term dk that depends on the timing error Td and the dynamics of the navigation system. Substituting rk with ra in (9) and k inserting (15) into this yields the following difference equation for the error state in the closed-loop navigation system with timing errors

dk = vk Td + ak

zf d,k+1

= = +

k zf d,k Kp,k ra + Gk ek k
%

k zf d,k Kp,k (k)(Hk zk + wk + dk ) G k ek .

$

By examining the recursions in Table 1 it is clear that the Kalman lter gain K k and hence also the Kalman lter prediction gain Kp,k are zero whenever (k) is zero. Thus (17) can be rewritten into a more familiar form

pGPS k

(k) pk vk Td + ak

2 Td + wk . 2

(13)

(14)

(17)

3 C OVARIANCE

OF THE ESTIMATION ERROR

E9

zf d,k+1

(k Kp,k Hk ) zf d,k Kp,k wk + Gk ek Kp,k dk . (18)

Equation (18) has similarities with the difference equation (10) describing the estimation error in the closed-loop system without timing error except from the last additional term Kp,k dk reecting the contribution due to timing error. Next, introduce


k =

k Kp,k Hk , k ,

k = n M, n = 1, 2, ... otherwise

(19)

Then the covariance of the estimation error may be expressed as E[zf d,k+1 (zf d,k+1 )T ] k Pf d,k T + Kp,k R LT + Gk Q GT k k k Kp,k dk dT LT k E[zf,k ] dT LT k k k k Kp,k dk E[zf,k ]T T k (20)

Pf d,k+1

= = +

where the mean of the estimation error can be calculated as E[zf,k+1 ] = k E[zf,k ] Kp,k dk . (21)

The derived expressions (20) and (21) describe the dynamics of the covariance and mean of the navigation errors in a closed-loop GPS-aided integrated navigation system with timing error. The three last terms in equation (20) are the contribution to the covariance due to time delay, whereas the three former ones correspond to the case when there is no time delay. As for the case without timing error, the error dynamics are determined by the time-varying system matrix k and now also the position bias dk , both depending on the trajectory of the navigation platform. Therefore, general analytical results of the convergence of the estimation errors are difcult to derive. However, the derived expressions can be numerically evaluated to investigate how the sampling timing error will affect the accuracy of the navigation system for a certain trajectory.

3.3

Example: Single-axis GPS-aided INS

Consider the single-axis navigation problem illustrated in Fig. 3 with reference to, for example, performance measurements in drag racing type of activities [14]. The purpose is to determine the position of the vehicle along a horizontal straight track. The vehicle is equipped with a GPS-receiver that gives noisy position estimates at 10 Hz. The acceleration and tilt rate of the vehicle are measured by an accelerometer and a gyro mounted at the center of gravity of the vehicle. When the vehicle accelerates it will start to tilt and the accelerometer will not only sense the forward acceleration but also the gravity retardation force. Assume the gravity vector to have the magnitude g, the relationship between the acceleration and angular rate of the vehicle and its tilt angle, velocity, and position can be described by the following set of differential equations:

E10

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

satellite

satellite

accelerometer sensitivity axis

PSfrag replacements

(t) pINS k+1 pGPS k+1 gravity vector

acceleration a(t)

gravity vector

Figure 3: Example of a single-axis GPS-aided INS system. The acceleration and tilt rate of the vehicle are measured by an accelerometer and a gyro mounted at the center of gravity of the vehicle. When the vehicle accelerates it tilts and the accelerometer will not only sense the forward acceleration but also the gravity force. The position of the vehicle is estimated using a GPS-receiver having an inherent time delay.

p(t) v(t) (t)

= = =

v(t) s(t) cos (t)

1

(22) ((t)) g tan((t)) (23) (24)

Here p(t), v(t), s(t), (t), and (t) denote the position, velocity, specic force, tilt, and angular rate of the vehicle, respectively. These equations are referred to as the navigation equations used in the INS to calculate the position, velocity, and tilt from the measured accelerations and angular rates. By introducing the state vector

xc = and input vector

p(t)

v(t)

(t)

(25)

the continuous time navigation equations (22)(24) may be written as xc = fc (xc , uc ) (27)

uc =

s(t)

(t)

(26)

3 C OVARIANCE

OF THE ESTIMATION ERROR

E11

Assuming that there is a bias and noise ec,2 in the gyro measurements, whereas the accelerometer measurements are only disturbed by some noise e c,1 2 , the measured output from the accelerometer and gyro can be described as sc = sc + ec,1 and c = c + ec,2 (29) respectively. Using these measurements as the input to the INS results in an error in the calculated position, velocity, and tilt. A linear model for how the errors develops with time may be found by linearizing the navigation equations around the true trajectory, that is, xc = = c xa xc f c ( xc , u c ) fc (xa , ua ) c c fc (x, u)
     

(28)

fc (x, u) x
  

( ec,2 ).
u=ua c

Hence, assuming that the bias in the angular rate measurements is constant, then the errors in the single-axis INS can be modeled as

zc = where

and
0

In the error model (31)(34), both the state transition matrices c and Gc are time varying since they depend on the specic force sc and the tilt angle c . If the sample rate of the INS is high compared to the rate of change in c and Gc , then a zero-order hold sampling of the system results in the following discrete time model for the error in the navigation system.
2 Including a bias term in the description of the accelerometer measurement would result in a nonobservable state-space model of how the single-axis INS errors develops with time.

&'

Gc =
'(

0 cos1 (c ) 0 0

0 0 1 0

0)

&'

c =
'(

sc sin(c )g cos2 (c )

0 0

0 0 0 0

1 0 0 0

0 0 1 0

0)

ec =

xc

= c z e + G c ec c

ec,1

ec,2

  

xc x=xa c

fc (x, u) s

ec,1 u=ua c (30)

(31)

(32)

(33)

(34)

E12

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

4000
[m]

Position

2000 0 40
[m/s]

20

40

60

80

100

Velocity

20 0 10

20

40

60

80

100

Tilt

PSfrag replacements [ ]

0 10

20

40

Time [s]

60

80

100

Figure 4: Trajectory used in the single-axis GPS-aided INS example. The vehicle tilt is modeled as the output of a rst-order system driven by the forward acceleration.

zk+1 = k zk + Gk ek where k = I5 + Ts c (k Ts ) Ts
sc (k Ts ) sin(c (k Ts ))g cos2 (c (k Ts ))

(35)

1 0 0 Ts cos1 (c (k Ts )) 0 0
 &' '(

2 acc 0 k,l (38) 2 0 gyro Further, since the difference in the estimated position of the GPS-receiver and the INS is observed, the observation equation for the error model becomes

Qk,l = E[ek (el )T ] =

Gk = Gc (k Ts ) =

0 0 1 0

1 0 0 0

Ts 1 0 0

0 0 Ts 1
0)

(36)

(37)

4 M ODELLING THE TIMING

ERROR IN THE INTEGRATION FILTER

E13

Table 2: Setting used when evaluating the single-axis GPS-aided INS with the aid of numerical simulations.

Parameter gps acc gyro Td Tmax Tmin Ts

Value 0.5 [m] 0.01 [m/s2 ] 0.1 [ /s] 0.1 [s] 0.1 [s] 0 [s] 10 [ms]

Parameter [Pf d,0 ]1,1 [Pf d,0 ]2,2 [Pf d,0 ]3,3 [Pf d,0 ]4,4 [Pf d,0 ]5,5 [ze d,0 ]6 f M

Value 0.12 [m2 ] 0.12 [(m/s)2 ] 22 [( )2 ] 0.12 [( /s)2 ] 1/300 [s2 ] 0.05[s] 10

Notes: The covariance matrix Pf d,0 was initialized as a diagonal matrix with diagonal elements as given in the table. The initial mean error ze d,0 was set to zero, except from the timing error that was set according to the table. f This corresponds to initializing the timing error as zero in the navigation lter. Note that the variables denoted with and only exist when the timing error is and is not included in the feedback of the GPS-aided INS, respectively. All other variables are the same for both cases.

rk = H k zk + w k where [1 01,4 ] 01,5



(39)

Hk Qk,l

= =

, k = nm, n = 1, 2, ... , otherwise

(40) (41)

T 2 E[wk wl ] = GP S k,l .

Recall that m is the ratio between the sampling ratio of the IMU and the GPS-receiver. Having derived the state-space model for the error plant in the single axis INS system, which has the same structure as the error model in (2)(7), the results in (20)(21) can be applied to evaluate how the time delayed GPS measurements will affect the accuracy of the integrated navigation system given a certain vehicle trajectory. Consider, for example, the vehicle trajectory described in Fig. 4, then the expected performance in terms of the root mean square (rms) and mean error in the integrated navigation system for a time synchronization error of 100 ms is shown in Figs. 56. In the example, the system parameters are those given in Table 2. A Monte Carlo simulation of the single-axis navigation example was also conducted to illustrate the error model in (20)(21). The empirical estimated error covariances and means are indicated in black in Figs. 56 and show good agreement with those estimated by the error model.

4 Modelling the timing error in the integration lter

Assume that the integrated navigation system has been extended to also estimate the timing error Td , which then is fed back and used to adjust the sampling instants of the IMU to synchronize it with the sampling of the GPS-receiver. Alternatively, assume that the estimated timing error Td (where the denotes an estimated quantity) is fed back to control a

E14

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

4
[m]

Standard deviation of the position estimation error

2 0 1

10

20

30

40

50

60

70

80

90

100

Standard deviation of the velocity estimation error

[m/s]

0.5 0 3

10

20

30

40

50

60

70

80

90

100

Standard deviation of the tilt estimation error

PSfrag replacements [ ]

2 1 0 0 10 20 30 40 50 [s] 60 70 80 90 100

Figure 5: Standard deviation of the estimation error in the position, velocity, and tilt for a time synchronization error of 100 [ms]. (a) No timing error red line, (b) Theoretical black line, (c) Empirical blue line.

fractional delay lter synchronizing the GPS-receiver and IMU data streams (Fig. 7). Then the plants of the errors in the integrated navigation system can be described by extending the state vector and error model in equations (2)(6) as follows zext k+1 ext zext + Gext ek k k k (42)

where zext is the augmented state vector zext = [zT Td,k ]T . k k Further,

(43)

ext k Gext k

= =

Here Td,k denotes the error in the estimated time delay at time instant k, that is T d,k = a Td Td,k . The true observation can then be described as

Gk 0

k 01,

0,1 1

(44) (45)

4 M ODELLING THE TIMING

ERROR IN THE INTEGRATION FILTER

E15

0.2 0.15
[ /s]

Standard deviation of the gyro bias estimation error

0.1 0.05 0 0 20 40 60 80 100

0.15 0.1
[ /s]

Mean of the gyro bias estimation error

0.05 0 0.05 0 20 40
[s]

PSfrag replacements [ /s]

60

80

100

Figure 6: Standard deviation and mean of the error in the gyro bias estimates for a time synchronization error of 100 [ms]. (a) No timing error red line, (b) Theoretical black line, (c) Empirical black line.

ra,ext k

= = =

(k)(pGPS pINS ) k d,k

%
2 Td,k 2 a (k) p(k Ts Td ) + wk pINS d,k

(46)
%

(k) p(k Ts Td,k Td,k ) + wk pINS d,k

$

where pINS is the INS position estimates at time instant k calculated from IMU data that d,k has been delayed by Td,k seconds. By following the same procedure as in (11)(14), but expanding the Taylor series around k Ts Td,k instead, it can be shown show that the true observation in the extended state space-model can be expressed as ra,ext k
e (k) pd,k vd,k Td,k + ad,k

Here, pd,k , vd,k , and ad,k are short-hand notations for p(k Ts Td ), etc. The observation error ra,ext can then be written in terms of an extended observation matrix and the state k

wk pINS . d,k

(47)

E16

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

vector of the extended error model as ra,ext = (k) (Hext zext + wk + dext ) k k k k where and

(48) (49)

Hext = k

Hk

vd,k

2 Td,k . (50) 2 Hence, the bias in the position error that is fed back into the system with the extended error model is only a function of the accelerations of the vehicle and the timing error T d,k at time k. Next, substituting rk in (10) with ra,ext , the error states in the navigation system f d,k with the extended error model can be described as

dext = ad,k k

zext f d,k+1

(k Kp,k Hk ) zext Kp,k wk + Gk ek f d,k Kp,k dext . k (51)

To evaluate the performance of the navigation lter using the extended error model it may be tempting to use the expressions in (20)(21). However, since the bias term d ext in (43) k is a function of the last entry in zext , this cross-correlation must be considered when evalk uating the last three terms in equation (20). Under the assumption that the state estimates are Gaussian distributed the covariance of the error in the extended system with feedback becomes Pext f d,k+1 = + where ext = k Further, k = + and
$

k Pext T + Kp,k R KT + Gk Q GT f d,k k p,k k Kp,k ext KT k k KT k p,k p,k Kp,k T T k k (52)

ak aT k (3 [Pext ]2 2 [zext ]4 ) f d,k i f d,k i,i 4

(53)

[Pext ]i,i zext f,k f d,k aT k 2

%

2 [zext ]i ( [Pext ]1:n,i [zext ]i zext ) f,k f d,k f d,k f d,k

(54)

1 Kp,k ak [Pext ]i,i . (55) f d,k 2 Here i denotes the index of the error element Td,k in the state vector of the extended error model. A derivation of the above expression for the error covariance in the system with the additional state for the time synchronization error is found in Appendix A. In order to illustrate the above result, we rely on the single-axis example below. zext = (k Kp,k Hk ) zext f d,k f d,k

Td
xk+1 = f (xk xk , uk )

5 I MPLEMENTING

A VARIABLE DELAY IN THE NAVIGATION FILTER

E17

IMU

uk

Sensor error compensation

uk

Delay

ud,k

Navigation Equations

xk+1
Hk

uk

Td,k

xk
Kk

pINS k+1

GPS

pGPS k+1

Figure 7: Block diagram of a closed-loop GPS-aided INS system with feedback for the compensation of the time delay in the GPS-receiver. One possibility of implementing the variable delay in reality is to use an adjustable fractional delay lter.

4.1

Example: Single-axis GPS-aided INS, revisited

Consider again the single-axis GPS-aided INS example in Section 3.3. Here, the timedelay error has been included in the feedback loop of the integrated navigation system, as illustrated in Fig. 7. The expected performance of the navigation system can then be evaluated by using the recursions in (52)(55). The expected performance of the system when evaluated for the same trajectory and with the same setting as in the previous case (see Fig. 4 and Table 2), is given by the black (dashed) lines in Figs. 810. From the gures, it is clear that by including the timing error in the feedback loop of the integrated navigation system, the effects of the timing error Td,k will become negligibly small for large ks, given a vehicle trajectory with enough dynamics. What is then enough dynamics? It is seen that at constant velocity the extended error model is not fully observable. However, by comparing the velocity curve in Fig. 4 and the curve of the timing error in Fig. 10, it becomes clear that Td,k is observable when a(t) = 0. This means that for the trajectory to have enough dynamics it should include velocity changes. This will be more rigorously shown in Section 7 by an observability analysis of the extended state-space model.

5 Implementing a variable delay in the navigation lter

In Section 4, where the error covariance of the extended navigation lter was derived, it was assumed that the inertial measurement unit data could be delayed with an arbitrary delay (see Fig. 7). However, in reality the IMU data has been sampled at certain time instances. To have a continuously variable delay D = Td /Ts an adjustable interpolator or fractional delay (FD) lter must be used. The ideal frequency response of such fractional delay is given by the linear-phase, all-pass lter [15, 16] H(e 2 ) = ej 2 D normalized frequency (56)

where denotes the normalized frequency and is the imaginary unit. The impulse response of this system is a shifted (by a factor D) and sampled sinc function. Hence, this is a

E18

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

2
[m]

Standard deviation of the position estimation error

1 0

100

200

300

400

500

600

0.4
[m/s]

Standard deviation of the velocity estimation error

0.2 0 4

100

200

300

400

500

600

Standard deviation of the tilt estimation error

PSfrag replacements [ ]

2 0

100

200

300 [s]

400

500

600

Figure 8: Standard deviation of the estimation error in the position, velocity, and tilt for the case when the time synchronization error is included in the error model used in the navigation lter. (a) Theoretical black line, (b) Empirical blue line.

noncausal lter with innite impulse response, making it impossible to implement in realtime applications. Therefore, in practice, an approximation of the ideal fractional delay lter must be used. For a detailed overview of fractional delay lter design and their usage, see [15]. One of the most commonly used techniques for fractional delay lter design is Lagrange interpolation, in which the lter coefcients of the FD-FIR lter is calculated as
N

h(n) =

k=0 k=n

Dk nk

where N is the order of the lter and D [(N 1)/2 < (N + 1)/2] is the desired delay. If a delay larger then D = (N + 1)/2 is required, this may be solved by adding an appropriate unit-delay before the FD-lter. In Fig. 11, the magnitude and phase response of an N = 3 order Lagrange interpolator is shown. As can be seen the lter has excellent low frequency characteristics and for D = 1.5 also a linear phase (i.e., constant groupdelay). However, the magnitude response suffers from a zero at = 0.5. An adjustable version of this third-order Lagrange interpolator has been implemented using a Farrow lter structure. Refer to [16] for details on how such lter implementation can be done in a

for n = 0, 1, 2, ..., N

(57)

5 I MPLEMENTING

A VARIABLE DELAY IN THE NAVIGATION FILTER

E19

0.1 0.08
[ /s]

Standard deviation of the gyro bias estimation error

0.06 0.04 0.02 0 0 20 40 60 80 100

0.04 0.02
[ /s]

Mean of the gyro bias estimation error

PSfrag replacements [ /s]

0 0.02

20

40

[s]

60

80

100

Figure 9: Standard deviation and mean of the error in the gyro bias estimates for the case when the time synchronization error is included in the error model used in the navigation lter. (a) Theoretical black line, (b) Empirical blue line.

computationally efcient manner. The adjustable FD-lter was used as variable delay in a Monte Carlo simulation of the one-dimensional GPS-aided integrated navigation system with an extended error model described in Section 4.1. The deviation between the low-pass characteristics of the Lagrange interpolator and the ideal fractional delay lter when used as variable delay of the IMU data is of marginal importance, since the INS mechanization has a low-pass characteristics. In Figs. 810, the results from the Monte Carlo simulation of the single-axis GPS-aided integrated navigation system with the feedback for the timing error are shown together with the error covariance and mean predicted from the error model in equations (52)(55). The setting used in the simulation is again given in Table 2. Further, the timing error was modeled as uniformly distributed between 0 and 100 ms and the lter was initialized for a zero time delay. As can be seen, the empirically calculated error covariances and means agree well with those predicted by the error model.

E20

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

0.06 0.04 [s] 0.02 0

Standard deviation of the time-delay estimation error

100

200

300

400

500

600

0.06 0.04 [s] 0.02 0

Mean of the of the time-delay estimation error

PSfrag replacements [ /s] [ /s]

100

200

300 [s]

400

500

600

Figure 10: Standard deviation and mean of the error in the timing error estimates. (a) Theoretical black line, (b) Empirical blue line.

6 Time synchronization applied to a low-cost GPSaided INS

To further illustrate the usefulness of the fractional delay lter time synchronization approach, it was implemented as a part of the in-house developed Matlab software for integrating GPS and IMU data. The synchronization approach was then tested on both simulated and real-world data, into which intentional time synchronization errors were included. The software was developed targeting simulations and tests of ultra low-cost GPS-aided INS applications, such as consumer electronics In-car navigation applications. Refer to [17] for more details on the PC-based navigation platform. The integration software is designed for navigation in the tangent plane (in daily life often referred to as north, east, and down, and to be used with low-cost micro-electro-mechanical systems (MEMS) inertial sensors. Thus, parameters such as craft rates, gravity variations, the earth rotation rate, etc, can be neglected. For this type of low-cost applications where the above-stated assumptions applies, an error model as found in Appendix B is applicable [18].

6 T IME

SYNCHRONIZATION APPLIED TO A LOW- COST

GPS- AIDED INS

E21

Magnitude Response

1
Magnitude

0.8 0.6 0.4 0.2 0 0

D=1.000 D=1.167 D=1.333 D=1.500 0.1 0.2 0.3 0.4 0.5

0
Phase [rad]

Phase Response

2 4 6 8 0 D=1.667 D=1.833 D=2 0.1 0.2

PSfrag replacements

0.3

0.4

0.5

Figure 11: Magnitude and phase response of the third-order Lagrange interpolator. The magnitude curves are the same for D and N D. An adaptive version of this lter was used in the simulations of the one-dimensional GPS-aided INS to time-synchronize the IMU and GPS-receiver data.

6.1

Simulated data

In Fig. 12, the trajectory used in the Monte Carlo simulations and evaluation of the derived error covariance expression of the GPS-aided INS is shown. The results of the Monte Carlo simulation together with the values predicted by the error covariance models are shown in Figs. 13-21. In Table 3, the setting used in the simulations are summarized. For the case when the timing error is not included in the model, see Figs 1316; there is a good agreement between the covariances and means predicted by the model and the empirical covariances and means calculated from the Monte Carlo simulation. In Fig. 16, the bias in the forward acceleration due to the timing error is seen. When the timing error is included in the model (see Figs. 1721), the derived covariance expressions do not perfectly capture the transient behavior of the integration lter. Further, there is a difference when the estimated time delay is actually fed back to synchronize the two data streams and when it is only used to compensate the error in the observations. There is a better agreement between the model predicted error covariances and the results of the simulations in the case when the timing error is not fed back than when the timing error is fed back. This difference is likely due to the extra nonlinearity introduced by delaying the IMU data lter, which is not captured

E22

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

50 0 50

Simulation trajectory

North [m]
PSfrag replacements

100 150 200 250

50

East [m]

50

100

Figure 12: Trajectory used in the Monte Carlo simulation of the GPS-aided INS and the evaluation of the corresponding covariance expressions.

6 T IME

SYNCHRONIZATION APPLIED TO A LOW- COST

GPS- AIDED INS

E23

by the covariance expressions. However, as the lter converges the agreement between the model predicted covariances and the empirical covariances improves and it is clearly seen that the effects of the timing error are removed. Moreover, from Fig. 20 it is obvious that the bias in the forward acceleration is removed.

Standard deviation of the north position estimation error.

3
[m]

2 1 0 0 3 100 200 300 400

Standard deviation of the east position estimation error.

[m]
PSfrag replacements

2 1 0 0 100 200
[s]

300

400

Figure 13: Standard deviation of the estimation error in the north and east position for a time synchronization error of 100 [ms]. (a) No timing error red line, (b) Theoretical black line, (c) Empirical blue line.

6.2

Real-world data

Real-world data was collected using a 100 Hz GPS and data logger (VBOX III) together with an additional MEMS IMU, all from RacelogicTM . To capture similar dynamics, the vehicle was driven in a trajectory similar to the trajectory used in the Monte Carlo simulation (see Figure 22). The test was conducted in a parking lot, outside Stockholm, Sweden, and under the following conditions: clear sky view with no large surrounding obstacles blocking the satellite signals and with 89 satellites in view during the entire run. The recorded GPS position estimates and the IMU data were integrated using the in-house developed software. To test the time synchronization approach, the timing error of the recorded data was rst estimated and removed. Then, three data sets with the intentional GPS data delays of 50, 100, and 150 ms were constructed and used as input to the integration software. Further,

E24

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

Standard deviation of the north velocity estimation error.

1
[m/s]

0.5 0 0 1

100

200

300

400

Standard deviation of the east velocity estimation error.

[m/s]
PSfrag replacements

0.5 0 0

100

200
[s]

300

400

Figure 14: Standard deviation of the estimation error in the north and east velocity for a time synchronization error of 100 [ms]. (a) No timing error red line, (b) Theoretical black line, (c) Empirical blue line.

6 T IME

SYNCHRONIZATION APPLIED TO A LOW- COST

GPS- AIDED INS

E25

2 1 0 0 2 1 0 0 5
[ ]
PSfrag replacements

Standard deviation of the roll estimation error.

[ ]

Standard deviation of the pitch velocity estimation error.

100

200

300

400

[ ]

Standard deviation of the heading velocity estimation error.

100

200

300

400

0 0

100

200
[s]

300

400

Figure 15: Standard deviation of the estimation error in the roll, pitch and heading for a time synchronization error of 100 [ms]. (a) No timing error red line, (b) Theoretical black line, (c) Empirical blue line.

E26

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

Mean of the accelerometer bias estimation error.

[m/s2 ]

0.1 0.05 0 0.05 0 0.2 100 200 300 400

[s] Mean of the gyro bias estimation error.

[ /s]
PSfrag replacements

0 0.2 0

100

200
[s]

300

400

Figure 16: Mean of the error in the accelerometer and gyro bias estimates for a time synchronization error of 100 [ms]. Note that the mean values predicted by the error model and thus obtained from the Monte Carlo simulation are indistinguishable. X-axis (forward) red line, Y-axis (sideways) green line, Z-axis (downward) blue line.

6 T IME

SYNCHRONIZATION APPLIED TO A LOW- COST

GPS- AIDED INS

E27

Standard deviation of the north position estimation error.

2
[m]

1 0 0 2

100

200

300

400

Standard deviation of the east position estimation error.

[m]
PSfrag replacements

1 0 0

osition estimation error.

100

200

[s]

300

400

Figure 17: Standard deviation of the estimation error in the north and east position when the timing error is included in the estimation problem of the data integration. (a) Theoretical black line, (b) Empirical (fractional delay) blue line.

E28

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

Standard deviation of the north velocity estimation error.

1
[m/s]

0.5 0 0

100

200

300

400

Standard deviation of the east velocity estimation error.

0.6
[m/s]

0.4 0.2 0 0 100 200

[s]

PSfrag replacements

rd deviation of the down velocity estimation error.

300

400

Figure 18: Standard deviation of the estimation error in the north and down velocity when the timing error is included in the estimation problem of the data integration. (a) Theoretical black line, (b) Empirical (fractional delay) blue line.

6 T IME

SYNCHRONIZATION APPLIED TO A LOW- COST

GPS- AIDED INS

E29

2 deg 1 0 0 2 deg 1 0 0 4
PSfrag replacements

Standard deviation of the roll estimation error.

Standard deviation of the pitch velocity estimation error.

100

200

300

400

Standard deviation of the heading velocity estimation error.

100

200

300

400

deg

2 0 0 100 200 300 400

[degrees]

[s]

Figure 19: Standard deviation of the estimation error in the roll, pitch, and heading when the timing error is included in the estimation problem of the data integration. (a) Theoretical black line, (b) Empirical (fractional delay) blue line.

E30

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

10
[m/s2 ]

x 10

Mean of the accelerometer bias estimation error.

5 0 5 0 100 200 300 400

[s] Mean of the gyro bias estimation error.

0.1
[ /s]
PSfrag replacements

0.05 0 0.05 0 100 200

[s]
Figure 20: Mean of the error in the accelerometer and gyro bias estimates when the timing error is included in the estimation problem of the data integration. Solid lines are for when a fractional delay lter is used and the dashed lines are theoretical ones. X-axis (forward) red line, Y-axis (sideways) green line, Z-axis (downward) blue line

300

400

6 T IME

SYNCHRONIZATION APPLIED TO A LOW- COST

GPS- AIDED INS

E31

Mean of the timing estimation error.

60
[ms]

40 20 0 0 60 100 200 300 400

[s] Standard deviation of timing estimation error.

[ms]
PSfrag replacements

40 20 0 0 100 200
[s]

[ms]

300

400

Figure 21: Mean and standard deviation of the timing estimation error. (a) Theoretical black line, (b) Empirical (fractional delay) blue line.

E32

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

Table 3: Setting used when evaluating the GPS aided INS with aid of numerical simulations.

Parameter gps acc gyro Td Tmax Tmin Ts M [Pf d,0 ]1:3,1:3 [Pf d,0 ]4:6,4:6 [Pf d,0 ]7:8,7:8 [Pf d,0 ]9,9 [Pf d,0 ]10:12,10:12 [Pf d,0 ]13:15,13:15 [Pf d,0 ]16,16 [ze d,0 ]16 f

Value 3/ 3 0.02 0.15 0.1 0.1 0 10 20 (3/ 3)2 0.12 12 32 0.12 /12 12 /12 0.052 + 0.12 /12 0.05

Unit [m] [m/s2 ] [ /s] [s] [s] [s] [ms] [m2 ] [(m/s)2 ] [( )2 ] [( )2 ] [(m/s)2 ] [( /s)2 ] [s2 ] [s]

Notes: The covariance matrix Pf d,0 was initialized as diagonal matrix with diagonal elements as given in the table. The initial mean error ze d,0 was set to zero, except for the timing error that was set according to the Table. This f corresponds to initializing the timing error as zero in the navigation lter. Note that the variables denoted with other variables are the same for both cases. and only exist when the timing error is and is not included in the feedback of the GPS-aided INS, respectively. All

6 T IME

SYNCHRONIZATION APPLIED TO A LOW- COST

GPS- AIDED INS

E33

20 0 20 40

Real world trajectories Start Outage Stop Outage

North [m]
PSfrag replacements

60 80 100 120 140 160 40 20 0 20 40 East [m] 60 80 100

Figure 22: Real-world trajectory used in the test of the time synchronization approach using a fractional delay lter in the feedback loop. The height difference in the trajectory is negligible and therefore not shown.

the GPS data was downsampled to 5 Hz. The drift rate of the system during GPS outages is often used as an indicator of the MEMS sensors quality and to judge how well the sensors have been calibrated by the integration lter [19]. Therefore a GPS outage of 20 seconds was introduced to see how the timing error affects the position drift rate of the system during the outages. As can be seen in Fig. 16 from the Monte Carlo simulation, as well as shown in [2], the timing error causes a false bias in the forward acceleration. Thus, the drift rate during the GPS outages should be larger in the case of a timing error. In Table 4, the estimated timing errors and the maximum position error during the outages for the different data sets are shown. The TEM (timing error in model) column indicates if the timing error was included in the data integration lter or not. For all the three different timing errors, a time synchronization in the order of a few milliseconds was achieved. As expected, the drift rate during the GPS outages is larger when there is a timing error. By including the timing error into the error model the drift rate goes down to the same order as without timing error. Here, it should be pointed out that when evaluating the drift rate the trajectory estimated by the lter using all available data and without any intentional time synchronization errors was used as a reference trajectory.

E34

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

Table 4: Estimated time delay (Td ) for articial timing error (Td ) and maximum error (2D-horizontal plane/3D-spherical) during a 20 second long GPS outage starting after 390 seconds.

Td [ms] 0 0 50 50 100 100 150 150

TEM no yes no yes no yes no yes

Max error (2d/3d) [m] 16.5/16.8 16.3/16.6 17.1/17.3 16.3/16.7 18.2/18.4 16.5/16.8 19.7/19.8 16.5/16.9

Td [ms] 0.7 46 98 147

7 Observability of time delay error

The observability of the error states in GPS-aided INSs have been extensively studied in [2022], and [23]. Therefore, only the question regarding the observability of additional timing error state is addressed here. A system is said to be observable at time k if the state vector zk can be determined from the outputs rk , ..., rk+N for a nite N [24]. For time-varying systems such as (75)(77) the observability analysis is in general cumbersome. However, for certain trajectories the characteristics of the system model (75)(77) can be captured by a piece-wise constant model. An observability analysis of the piece-wise constant model, which is fairly straightforward, can then give valuable insight regarding the observability of the time-varying model [20]. Thus, the observability of the timing error in the extended version of the state-space model (75)(77) is examined for an approximately straight-line trajectory with piece-wise constant velocity segments, so that the model characteristics can be captured by a piece-wise constant model. Let the velocity during the rst segment k < M be denoted v1 and the velocity in the second segment k M be denoted v2 . Here, denotes the number of observations (GPS position estimates) during the rst segment of the trajectory and is taken to be an integer larger then 6. (Recall, M is the ratio between the sampling rate of the IMU and GPS-receiver). Then the error model can be captured by the piece-wise time-invariant model, where ext = ext is constant k during both segments of the trajectory, and the observation matrix is Hext = (k) k where Hj =


H1 , H2 ,

k < M k M

(58)

The observation matrix for the piece-wise constant system can written as [20] (W o is a size N 16 matrix)

vj

j = 1, 2.

(59)

8 R ESULTS AND C ONCLUSIONS

E35

For the system to be fully observable, Wo should have full rank. By utilizing the structure of ext it is straightforward to show that I3
(

where Ci , here and in the sequel, denotes a matrix of appropriate dimension and which elements are unknown and of no interest. Thus, since Hj = [I3 03,12 vj ] for j = 1, 2, then

Hj (ext )n =

I3

C3

vj

and the observability matrix will have the following form

Wo =

where is the Kronecker product. If the navigation platform moves at constant speed throughout the trajectory, that is, v1 = v2 , then clearly Wo has not got full rank since T Wo [v1 0 1]T = 0. Thus, at constant speed only the linear combination of the timing error Td and position error p is observable. This sound intuitively since at constant speed the timing error causes a constant bias in the position measurements which can not be distinguished from the true position error. However, if the velocity changes between the segments such that v1 = v2 , then the last column of Wo can no longer be expressed as a linear combination of the three rst columns. By further noting that k in (76) is independent of the velocity, so that matrices C4 and C5 in the middle columns of Wo , by construction are independent of the velocity, it is clear that the timing error is observable for trajectories including velocity changes. Summarizing, the velocity error is observable for all trajectories that includes velocity or turns (since v are taken to be in tangent-coordinate plane, turns are indirectly changing the north and east velocity components).

8 Results and Conclusions

The effects of time synchronization errors in a closed-loop GPS-aided INS with error feedback have been studied in terms of the error covariance of the system. Expressions for

1 I3 1 I3

C4 C5

1 v1 1 v2

=
&

I3 012,3 01,3

C1 C2 01,12

03,1 012,1 1

j = 1, 2

ext n

=
&

012,3 01,3

[]1:3,4:15 []4:15,4:15 01,12

03,1 012,1 1
)

'

'

Wo =
' ' ' '

0 0 0 0 0 0 0

H1 H1 (ext )M . . . ext M (1) H1 ( ) H2 (ext )M . . . ext M (N 1) H2 ( )

&' ' ' ' '(

0)

(60)

(61)

(62)

(63)

E36

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

the error covariance of the navigation system given the vehicle trajectory and a state-space model of the error plant of the integrated navigation system have been derived. Further, under the assumption of Gaussian-distributed estimates, an error covariance expression for the case when the time synchronization error is included in the feedback loop of the navigation lter is derived. Use of the two covariance expressions is illustrated by applying them to an example of a one-dimensional GPS-aided INS with an inherent time delay. The results from the example show that by extending the error model used in the navigation lter to also include a state for the timing error, the effects of the timing error become negligible as the lter converges. The correctness of the derived covariance expressions is illustrated by Monte Carlo simulations of the one-dimensional GPS-aided INS, which shows a good agreement with analytical results calculated from the derived error covariance models. Moreover, a practical approach on how to implement the variable delay for time synchronization of the IMU and GPS-receiver data is briey discussed. Further, the proposed time synchronization approach using a fractional delay lter to delay the IMU data was implemented as a part of the in-house developed software for integration of GPS and IMU data. The time synchronization was then tested on both simulated and real-world data. In the case when the timing error was not considered by the integration software, the by Monte Carlo simulations empirically calculated error covariance and those predicted by the derived covariance expression agreed well. Whereas for the case when the timing error was considered by the integration lter, the derived covariance expressions do not manage to fully capture the transient behavior of the lter. However, as the lter converges, the agreement improves and the effects of the timing error are removed. In the test with real-world data, with intentionally included timing errors, time synchronization within a few milliseconds was obtained. Further, the differences in maximum error during the simulated GPS outages indicate, that the sensor biases were better estimated when the timing error was taken into account by the integration lter (i.e., no bias in the forward acceleration). To conclude, synchronization errors in the sampling instances of the GPS-receiver and IMU sensor in a GPS-aided INS can seriously degrade the performance of the system. The derived covariance expression provides an efcient way of evaluating the systems performance for a certain timing error and vehicle trajectory. By extending the integration lter to also estimate the timing error and by feeding back the estimates to synchronize the GPS-receiver and IMU data, by means of a fractional delay lter, almost perfect time synchronization can be achieved and the effects of the timing error cancelled.

Appendix A
From the derived difference equation (51) for the error in the system with the extended state-space model it can be shown that the error covariance of the system can be written as Pext f d,k+1 = = + E{ze,ext (ze,ext )T } f d,k+1 f d,k+1 k Pext T + Kp,k R LT + G Q GT f d,k k k Kp,k ext LT k k LT k k k Kp,k T T k k (64)

A PPENDIX A

E37

where ext k = = and k = = E{ze,ext (dext )T } k f d,k E{ze,ext (Td,k )2 } f d,k aT d,k . 2 (66) E{dext (dext )T } k k ad,k aT d,k E{(Td,k )4 } 4 (65)

By assuming the state estimates to be Gaussian distributed, (65) and (66) can be rewritten in terms of Pext and ze,ext =E{ze,ext } by using the following expression for the expected f d,k f d,k f d,k value of four jointly Gaussian variables. Let Z1 , Z2 , Z3 , and Z4 be four jointly Gaussian variables, then it holds that [25] E{Z1 Z2 Z3 Z4 } = + + E{Z1 Z2 } E{Z3 Z4 } E{Z1 Z3 } E{Z2 Z4 } E{Z1 Z4 } E{Z2 Z3 } 2 E{Z1 } E{Z2 } E{Z3 } E{Z4 }.
4

(67)

By letting Z1 = Z2 = Z3 = Z4 = Td,k it can be shown that E{(Td,k ) } in (65) can be expressed as E{(Td,k )4 } = 3 E{(Td,k )2 }2 2 E{Td,k }4 . Inserting (68) into (65), ext k ext k can be written as ad,k aT d,k E{(Td,k )4 } 4 ad,k aT d,k 3 E{(Td,k )2 }2 2 E{Td,k }4 4 ad,k aT d,k 3 [Pext ]2 2 [ze,ext ]4 . f d,k i,i f d,k i 4
   

(68)

= = =

(69)

Here, i denotes the index of the Td,k element in the state vector ze,ext . By using (68) f d,k again, an expression for the expectation E{(Td,k )2 ze,ext } in (66) can be found as follows. f,k Consider the j-th element of E{ze,ext (Td,k )2 } and let Z1 = Z2 = Td,k , Z3 = [ze,ext ]j f,k f d,k 2 and Z4 = 1 (i.e Z4 is a Gaussian variable with mean 1 and with variance Z4 0) then E{(Td,k )2 [ze,ext ]j } f d,k = + E{(Td,k )2 } E{[ze,ext ]j } f d,k 2 E{Td,k } E{[ze,ext ]j Td,k } f d,k E{[Td,k ]} E{[ze,ext ]j } . f d,k
% $

(70)

E38

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

Equation (70) can be expressed in terms of the covariance matrix P ext and mean value f d,k vector ze,ext as f d,k E{(Td,k )2 [ze,ext ]j } f,k [Pext ]i,i [ze,ext ]j + 2 [ze,ext ]i f d,k f,k f,k ([Pext ]j,i [ze,ext ]i [ze,ext ]j ). f d,k f,k f,k (71) Thus, the expected value of the vector E{(Td,k )2 ze,ext } is given by f,k E{(Td,k )2 ze,ext } f,k [Pext ]i,i ze,ext + [ze,ext ]i f d,k f,k f,k ( [Pext ]1:,i [ze,ext ]i ze,ext ). f d,k f d,k f d,k (72) Recall, was the length of the state vector. Inserting this into (66), then k can be written as

where ze,ext f d,k = = (k Kp,k Hk ) ze,ext Kp,k E{dext } k f d,k 1 e,ext (k Kp,k Hk ) zf d,k Kp,k ak [Pext ]i,i . f d,k 2 (74)

Thus, under the assumption of Gaussian-distributed state estimates, the error covariance of the state vector in the navigation system with the augmented state vector, recursively, can be calculated from equations (64), (69), (73), and (74).

Appendix B
Error model used in the software for integrating GPS and IMU data. Let the error state vector be dened as

ze = c
n n

(pn )T

(vn )T

()T

(f b )T

b (ib )T

Here, p and v denotes the position and velocity error, respectively. The superscript n indicates that the variables are described in the navigation coordinate frame. Further, the vector contains the roll, pitch, and heading error. The accelerometer and gyro sensor errors p in platform coordinates are denote f p and ip , respectively. With the above-described state vector, the continuous time state transition matrix of the system becomes

( [Pext ]1:,i [ze,ext ]i ze,ext ) f d,k f d,k f d,k

[Pext ]i,i ze,ext + 2 [ze,ext ]i f d,k f,k f,k aT d,k 2 (73)

(75)

R EFERENCES

E39

and the process noise gain matrix

0 0 0

Here, S(sn ) denotes the skew-symmetric matrix representation of the specic force vector sn expressed in the navigation coordinate frame. The matrix Rn is the coordinate rotation p matrix transforming a vector in platform coordinates into navigation coordinates.

References
[1] I. Bar-Itzhack and Y. Vitek, The enigma of false bias detection in a strapdown system during transfer alignment, Journal of Guidance, Control and Dynamics, vol. 8, no. 2, pp. 175180, Mar. 1985. [2] H. Lee, J. Lee, and G. Jee, Calibration of measurement delay in global positioning system/strapdown inertial navigation system, Journal of Guidance, Control and Dynamics, vol. 25, no. 2, pp. 240247, Mar. 2002. [3] , Effect of measurement delay on SDINS, in Proc. PLANS, San Diego, CA, USA, Mar. 2000. [4] , Calibration of time synchronization error in GPS/SDINS hybrid navigation, in Proc. 15th IFAC Symposium on Automatic Control in Aerospace, Bologna/Forli, Italy, Sept. 2001. [5] B. Li, C. Rizos, H. Lee, and H. Lee, A GPS-slaved time synchronization system for hybrid navigation, GPS Solutions, vol. 10, no. 3, pp. 207217, July 2006. [6] B. Li, A cost effective synchronization system for multisensor integration, in Proc. ION GNSS Conf., Long Beach, USA, Sept. 2004. [7] W. Ding, J. Wang, P. Mumford, Y. Li, and C. Rizos, Time synchronization design for integrated positioning and georeferencing systems, in Proc. SSC 2005 Spatial Intelligence, Innovation and Praxis: The national biennial Conference of the Spatial Sciences Institute, Melbourne, Australia, Sept. 2005. [8] J. Farrell and M. Barth, The Global Positioning System and Inertial Navigation. McGraw-Hill, 1998. [9] M. Grewal, L. Weill, and A. Andrews, Global Positioning Systems, Inertial Navigation and Integration. Wiley, 2001. [10] R. Rogers, Applied Mathematics in Integrated Navigation systems. AIAA Education Series, 2003.

'

Gc =
' '(

03 Rn p 03 03 03
&'

03 03 Rn p 03 03

0)

' ' '(

c =

0 0 0

03 03 03 03 03
&'

I3 03 03 03 03

03 S(sn ) 03 03 03

03 Rn p Rn p 03 03

03 03 03 03 03

0)

(76)

(77)

E40

T IME

SYNCHRONIZATION ERRORS IN

GPS- AIDED

INERTIAL NAVIGATION SYSTEMS

[11] Chateld, Fundamentals of High Accuracy Inertial Navigation.

AIAA, 1997.

[12] I. Skog and P. H ndel, A low-cost GPS aided inertial navigation system for vehicle a applications, in Proc. EUSIPCO 2005, Antalya, Turkey, Sept. 2005. [13] R. Brown and A. Sage, Estimation using stochastic feedback with applications to integrated navigation systems, IEEE Trans. on Aerospace and Electronic Systems, vol. 7, no. 2, pp. 355, Mar. 1971. [14] G. Fox, On the physics of drag racing, American Journal of Physics, vol. 41, no. 3, pp. 311313, Mar. 1973. [15] T. Laakso, V. V lim ki, M. Karjalainen, and U. Laine, Splitting the unit delay a a [FIR/all pass lters design], IEEE Signal Processing Magazine, vol. 13, pp. 3060, Jan. 1996. [16] V. V lim ki, A new lter implementation strategy for Lagrange interpolation, in a a IEEE International Symposium on Circuits and Systems, ISCAS 95, Seattle, WA, USA, May 1995. [17] I. Skog, A. Schumacher, and P. H ndel, A versatile PC-based platform for inertial a navigation, in Proc. NORSIG 2006, Reykjavik, Iceland, June 2006. [18] S. Winkler and P. V rsmann, Multi-sensor data fusion for small autonomous uno manned aircraft, European Journal of Navigation, vol. 5, no. 2, pp. 3241, May 2007. [19] N. El-Sheimy and X. Niu, The promise of MEMS to the navigation cummunity, InsideGPS, pp. 4656, March/April 2007. [20] D. Goshen-Meskin and I. Bar-Itzhack, Observability analysis of piece-wise constant systems-part 1: Theory, IEEE Trans. on Aerospace and Electronic Systems, vol. 28, no. 4, pp. 10561067, Oct. 1992. [21] , Observability analysis of piece-wise constant systems-part 2: Application to inertial navigation in-ight alignment, IEEE Trans. on Aerospace and Electronic Systems, vol. 28, no. 4, pp. 10681075, Oct. 1992. [22] S. Hong, M. Lee, H. Chun, S. Kwon, and J. Speyer, Observability of error states in GPS/INS integration, IEEE Trans. on Veh. Techn., vol. 54, no. 2, pp. 731743, Mar. 2005. [23] I. Rhee, M. Abdel-Hafez, and J. Speyer, Observability of an integrated GPS/INS during maneuvers, IEEE Trans. on Aerospace and Electronic Systems, vol. 40, no. 2, pp. 526535, Apr. 2004. o [24] K. Astr m and B. Wittenmark, Computer Controlled Systems: Theory and Design. Prentice Hall, 1984. [25] W. Bar and F. Dittrich, Useful formula for moment computation of normal random variables with non-zero means, IEEE Trans. on Automatic Control, vol. AC-16, pp. 263265, 1971.