Signal Processing School of Electrical Engineering KTH (Royal Institute of Technology) Stockholm 2007
Skog, Isaac GNSSaided INS for land vehicle positioning and navigation Copyright c 2007 Isaac Skog except where otherwise stated. All rights reserved.
Signal Processing School of Eletrical Engineering KTH (Royal Institute of Technology) SE100 44 Stockholm, Sweden Telephone + 46 (0)8790 7790
Abstract
This thesis begins with a survey of current stateofthe art incar navigation systems. The pros and cons of the four commonly used information sources GNSS/RFbased 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 GNSSaided inertial navigation platform is presented, into which further sensors such as a camera and wheelspeed encoder can be incorporated. The construction of the hardware platform, together with an extended Kalman lter for a closedloop 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 lowcost 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 CramrRao 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 GNSSaided 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 GNSSreceiver 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 halfsister, 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 Stateofthe art and future incar navigation systems a survey 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . 2 Stateofthe 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 Nonlinear ltering . . . . . . . . . . . . . . . . . . 8 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5
A1 A1 A3 A5 A8 A13 A16 A18 A20 A21 A22 A23
B A lowcost 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 . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . . . . . . .
C A Versatile PCBased 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 GPSaided inertial navigation systems E1 1 Nomenclature . . . . . . . . . . . . . . . . . . . . . . . . . . . . E1 2 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . E3 3 Covariance of the estimation error . . . . . . . . . . . . . . . . . E4 3.1 ClosedLoop Error . . . . . . . . . . . . . . . . . . . . . E6 3.2 Timing Errors in ClosedLoop . . . . . . . . . . . . . . . E7 3.3 Example: Singleaxis GPSaided INS . . . . . . . . . . . E9 4 Modelling the timing error in the integration lter . . . . . . . . . E13 4.1 Example: Singleaxis GPSaided INS, revisited . . . . . . E17 5 Implementing a variable delay in the navigation lter . . . . . . . E17 6 Time synchronization applied to a lowcost GPSaided INS . . . . E20 6.1 Simulated data . . . . . . . . . . . . . . . . . . . . . . . E21 vi
6.2 Realworld data . . . . . 7 Observability of time delay error 8 Results and Conclusions . . . . Appendix A . . . . . . . . . . . . . . Appendix B . . . . . . . . . . . . . . References . . . . . . . . . . . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
vii
Part I
Introduction
Introduction
Incar 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 highperformance positioning systems and methods is well developed. The challenge is to develop highperformance system solutions using lowcost sensor technology. This is the topic of the thesis, consisting of the following ve papers. Paper A: I. Skog and P. H ndel, Stateofthe art and future incar navigation a systems a survey, submitted to IEEE Transactions on Intelligent Transportation Systems. Paper B: I. Skog and P. H ndel, A lowcost 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 PCBased 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 GPSaided ina ertial navigation systems, submitted to IEEE Transactions on Intelligent Transportation Systems.
2I NTRODUCTION
understanding of the limitations and problems associated with the current incar 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, wheelspeed encoder etc., are easily incorporated. Calibration of lowcost IMUs. The main contribution in paper D is the proposed simplied method to calibrate lowcost IMUs, together with the derivation of the Cram rRao bound for the standard calibration method, e where a turntable 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: Stateofthe art and future incar navigation systems a survey A survey of the information sources and information fusion technologies used in the current incar navigation systems is presented. The pros and cons of the four commonly used information sources GNSS/RFbased 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 incar navigation systems concludes the survey. Paper B: A lowcost 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 continuoustime navigation and error equations for an earthcentered earthxed 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 realtime vehicle navigation. Simulation results of the system are presented.
1 C ONTRIBUTIONS
OF THE
T HESIS 3
Paper C: A Versatile PCBased Platform For Inertial Navigation A GPS aided inertial navigation platform is presented, into which further sensors such as a camera, wheelspeed 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 eldtest 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 lowcost 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 rRao 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 rRao e bound within 8 dB, and thus the proposed method may be acceptable for a wide range of lowcost applications. Paper E: Time synchronization errors in GPSaided inertial navigation systems The effects of time synchronization errors in a GPSaided 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 GPSreceiver 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 realworld 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 nonzero accelerations.
4I NTRODUCTION
Part II
Included papers
Paper A
Stateofthe art and future incar navigation systems a survey
Isaac Skog and Peter H ndel a Submitted to IEEE Transactions on Intelligent Transportation Systems
Abstract A survey of the information sources and information fusion technologies used in the current incar navigation systems is presented. The pros and cons of the four commonly used information sources GNSS/RFbased 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 incar navigation systems concludes the survey.
1 Introduction
Today a large share of private cars is delivered from the factory with a GPSbased incar navigation system. Owners of used cars can at, a reasonable cost, install one of the many third party incar 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 incar 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
NAVIGATION SYSTEMS
A SURVEY
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 incar navigation system. The block with dashedlines are in general not an apart of current incar navigation systems but will likely be a major part of next generation incar 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 incar 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 incar 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
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 nonscheduled 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 highperformance navigation is well developed. Nowadays, the challenge is to develop highperformance navigation system solutions using lowcost sensor technology [9]. The purpose of this paper is to present a survey of current incar navigation technology: possibilities, limitations and various design approaches. Section 2 describes stateofthe art incar 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 incar 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 incar navigation systems.
A4
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 incar 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 lowcost accelerometers and gyroscopes have update rates (bandwidths) of hundreds of Hertz. This means that even the highfrequency dynamics of the vehicle can be captured by the incar navigation system.
To give absolute gures on the accuracy of stateofthe art incar 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 incar navigation system based on fusion of GPSposition estimates with an odometer and gyroscope based dead reckoning system (DRS) (no mapmatching 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 GPSposition data is available, 90% or more of the long and crosstrack positioning error is due to GPSpositioning 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 lowcost dead reckoning sensors, a highaccuracy positioning aiding system is needed. Hence, the accuracy of the incar navigation system is highly dependent on available lowcost GPS receiver solutions.
A5
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
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 3dim). 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 Gaussiandistributed 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,
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 Noncommon mode Multipath 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 satellitebased 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 Multifunctional 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 lowcost 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 InternetSISNeT (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 12 m in the horizontal plane and 24 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
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 singlefrequency receiver units. Using dual frequency receivers and charierphase measurements supported by various augmentation systems, it is possible to achieve realtime 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 incar navigation systems. In [24], a discussion of the performance and cost of single and twofrequency 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 startup 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 groundbased stations acting as additional satellites, socalled 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 radiobased 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 signalbased 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.
A9
Table 2: Sensors commonly used as a complement to GNSSreceivers for enhancement of incar 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 antilock 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
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
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 nonsystematic 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
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 magnetoresistive 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 costeffective. In [18] and [40], details about compasses based on uxgate 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 incar navigation systems can be questioned [17]. However, there are other applications of magnetic sensors in incar 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 highend 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 microelectromechanicalsystem (MEMS) sensor technology it has become possible to construct inertial sensors meeting the cost and size demands needed for lowcost
A12
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 selfcontained. 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 thirdparty incar 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 nonzero output from the sensor for a zero input. Scale factor and nonlinearity errors describe the uncertainty in linear and nonlinear scaling between the input and output, respectively. Each of these error categories in general includes some or all of the following components: xed terms, turnon to turnon 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. Turnon to turnon 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 turnon to turnon and random walk error characteristics are therefore of major concern in the choice of sensors and information fusion method.
A13
In order to measure the vehicles dynamics in both the long and crosstrack 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 accelerometerbased navigation systems. In general, a sixdegreeoffreedom 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 threedimensional space, and the gyroscopes measure the rotation rates around these axes.
4.1
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 (3dim) or heading (2dim) 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
NAVIGATION SYSTEMS
A SURVEY
(x2 , y2 ) x
(x0 , y0 )
Figure 4: Deadreckoning 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 strapdown3 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 strapdown 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
dt
The integrative nature of the navigation calculations in DR and inertial navigation systems gives the systems a lowpass lter characteristic that suppresses highfrequency sensor errors but amplies lowfrequency 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 radiobased navigation systems. These properties are: They are selfcontained, 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
NAVIGATION SYSTEMS
A SURVEY
Contrary to these properties, the GNSS and other radiobased 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 fullsixdegreeoffreedom 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 threedimensional 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 lowcost MEMS inertial sensors have an error characteristic causing position errors in the range of tens of meters during 30 seconds of standalone operation [9, 14, 44, 57]. This is also illustrated in Fig. 6, where the root mean square (RMS) horizontal position error during a 30second GNSS signal outage in a GNSSaided INS is shown. In the simulation, the IMU sensors were modeled as ideal sensors, except from having measurement noises, turnon to turnon and time varying biases reect current ultra lowcost MEMS inertial sensors.
5 V EHICLE
A17
2 0
300
305
310
315 s
320
325
330
Figure 6: Empirical root mean square (RMS) horizontal position error growth during a 30second satellite signal blockage in a lowcost GPSaided INS. NOC  No constraints, NHC  Nonholonomic constraints, NHC+VA  Nonholonomic 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 abovementioned nonholonomic 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
NAVIGATION SYSTEMS
A SURVEY
Motion Acceleration xaxis (forward) yaxis (sideways) zaxis (downwards) Angular velocity xaxis (roll) yaxis (pitch) zaxis (yaw)
and environmental specics [56]. To adapt the model to different driving conditions, these parameters must be estimated in realtime. 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 preltering/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 zaxis acceleration dynamics is due to road irregularities. In [63] and [61], these bandwidths, together with a noise model, are used to develop denoising 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 highquality 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 denoising approach is given together with test results on a ightmounted GPSaided 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 incar navigation system, a process referred to as mapmatching. Traditionally, mapmatching 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
Shape point
Node (deadend)
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 mapmatching has become feasible, viewing the map information as observations in the estimation of the information fusion [6]. This type of bidirectional mapmatching 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 piecewise 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, deadend 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 roadnetwork 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 pointtopoint, pointtocurve, and curvetocurve matching to extract geometrical information. The topological information criterion determines the connectivity of the
A20
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 stateofthe art mapmatching 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]; SigmaPoint 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
Navigation Equations
Navigation errors
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
Nonlinear 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 nonGaussian 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 nonGaussian noise sources, more rened nonlinear ltering approaches such as SigmaPoint 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 highdimensional 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 Gaussiandistributed noise entries, may be solved using a Kalman lter, hence reducing the computa
A22
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 mapmatching, 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/RFbased 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 incar 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 civiluser frequencies, together with development of lowcost multifrequency receivers and carrier phase augmentation systems, will allow for decimeterlevel positioning accuracy at a cost suitable for incar navigation systems. Further developments in MEMS inertial sensor technology, especially MEMS gyroscopes, will allow for ultra lowcost micro IMUs that can bring full 3dimensional attitude information to incar navigation systems. Rened digital maps with lane information etc. will allow mapmatching 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 mapmatching to the sensor fusion. Increased processing power at reduced power consumption and cost levels will allow for usage of rened vehicle models and nonlinear ltering technologies. Further, until this stage both OEM and thirdparty incar navigation systems primarily have been developed to provide positioning and routeguidance information to the user. As the technological development of incar 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 subsystem failures jeopardizing the reliability of the advanced driver assistance systems.
References
[1] J. Du and M. Barth, Bayesian probabilistic vehicle lane matching for linklevel invehicle navigation, in Proc. of IEEE Int. Veh. Symp., Tokyo, Japan, June 2006, pp. 522526. [2] , Lanelevel positioning for invehicle 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 GPSaided INS for advanced vehicle control, IEEE Trans. Robotics and Automation, vol. 19, no. 2, pp. 269282, Apr. 2003. [5] S. Julier and H. DurrantWhyte, 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 Galileopositioning 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. 12, pp. 111122, Nov. 2006.
A24
NAVIGATION SYSTEMS
A SURVEY
[12] J. Huang and H.S. Tan, A loworder DGPSbased vehicle positioning system under urban enviroment, IEEE/ASME Trans. Mechatronics, vol. 5, no. 12, 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. McGrawHill, 1998. [17] E. Abbott and D. Powell, Landvehicle navigation using GPS, Proceedings of the IEEE, vol. 87, no. 1, pp. 145162, Jan. 1999. [18] D. Titterton and J. Weston, Strapdown Inertial Navigation Technology2nd 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. PrietoCerdeira, 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. JanturaTraveset, 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 softwareSISSIM, 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.egnospro.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. GrejnerBrzezinska, 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 stateoftheart positioning designs, IEEE Signal Processing Magazine, vol. 22, no. 4, pp. 1223, July 2005. [34] A. Sayed, A. Tarighat, and N. Khajehnouri, Networkbased 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 uxgate 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. ElSheimy 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. ElSheimy, Costefcient 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 accelerometerbased inertial navigation systems, IEEE Trans. Instrumentation and Measurement, vol. 54, no. 6, pp. 25202530, Dec. 2005.
A26
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. DurrantWhyte, 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. DurrantWhyte, The aiding of a lowcost 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. ElSheimy, A universal approach for processing any MEMS inertial sensor conguration for landvehicle 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. ToledoMoreo, M. ZamoraIzquierdo, B. beda Miarro, and A. GmezSkarmeta, Highintegrity IMMEKFbased road vehicle navigation with lowcost GPS/SBAS/INS, Proc. of IEEE Int. Transport Sys. Conf., vol. 8, no. 4, Sept. 2007. [60] S. Julier and H. DurrantWhyte, Process models for the highspeed 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. ElSheimy, Improving the positioning accuracy of DGPS/MEMS IMU integrated systems utilizing cascade denoising 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 shortterm (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. ElSheimy, 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 roadmatching 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 roadmatching 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 fuzzynetworkbased cmeasure mapmatching 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 mapmatching algorithms for transport applications: Stateofthe 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 lowcost GPS aided inertial navigation system for vehicle a applications, in Proc. EUSIPCO 2005, Antalya, Turkey, Sept. 2005. [72] E. Shin and N. ElSheimy, Unscented kalman lter and attitude errors of lowcost 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, Sigmapoint Kalman lters for nonlinear estimation and sensorfusion: 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 lowcost 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, GPSbased land vehicle navigation system assisted by a lowcost gyrofree 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 timevarying 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
NAVIGATION SYSTEMS
A SURVEY
[80] T. Sch n, F. Gustfasson, and P.J. Nordlund, Marginalized particle lters for mixed o linear/nonlinear statespace 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 lowcost GPS aided inertial navigation system for vehicle applications
Isaac Skog and Peter H ndel a Published in Proceedings of European Signal Processing Conference 2005
Abstract
In this paper an approach for integration between GPS and inertial navigation systems (INS) is described. The continuoustime navigation and error equations for an earthcentered earthxed 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 realtime 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 15100 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 lowcost 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 inhouse need for a GPS aided INS testbed 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 hostcomputer 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
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 lowcost GPS aided INS installed in a car is presented. In [4], the strapdown 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 offtheshelf 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 strapdown 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 earthxed (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
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 3dimensional 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 bodyframe, 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 skewsymmetric 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 timevarying. 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 3dimensional measured acceleration in the body frame f b and the 3dimensional 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
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)
G(t) =
(9)
The error equations (7) are timevarying, 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
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 zeroorderhold sampling of the error equation is described.
3.1
Zeroorder 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
Having a continuoustime 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 discretetime error equation becomes (17)
(t, ) G( ) uc ( )d
t0
(16)
B6
LOW COST
GPS
xk+1 = k xk + ud,k
(18)
where the state transition matrix k = ((k + 1)Ts , kTs ) is approximated as in (17) and the discretetime 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 discretetime 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.
where c(, ) denotes the dynamics, zk is the navigation system outputs: position, velocity and Euler angles dening the rotation matrix Re , that is the 9element vector b
Further, the navigation system input is the 6element vector ak which contains the inputs to navigation system, accelerations and angular rates, that is
ud,k =
(19)
(20)
(21)
k = n , n = 1, 2, 3... otherwise
(22)
zk =
re k
e vk
(24)
ak =
b fk
b ib, k
(25)
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
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
zk+1 ak+1
= k
k z k a
c(z, a) a
(29)
a=anom k
(30)
(31)
(32)
B8
LOW COST
GPS
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
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 IMUsignal 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
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
5.1
Hardware Design
A GPS aided INS platform has been developed inhouse, consisting of an offtheshelf GPS receiver and an inhouse IMU platform. The IMU platform comprises stateoftheart MEMS gyros and accelerometers, and a microcontroller to control the data acquisition. The microcontroller controls the GPSreceiver 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 12 cm/s2 respectively 510 /h. Not surprisingly, the GPS aided system clearly outperforms the GPSsystem. Our current work is focused on studying and implementing different sensor error models and calibration methods, making the testbed available for eld tests. More detailed performance evaluations and results from eld tests will be reported elsewhere.
B10
LOW COST
GPS
150
GPS reading
100
INS/GPS estimate
North [m]
50
True trajectory
PSfrag replacements
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. DurrantWhyte, 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 lowcost 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. McGrawHill, 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 PCBased Platform For Inertial Navigation
Isaac Skog, Adrian Schumacher and Peter H ndel a Published in Proceedings of IEEE Nordic Signal Processing Symposium 2006
Abstract
A GPS aided inertial navigation platform is presented, into which further sensors such as a camera, wheelspeed 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 eldtest 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 offtheshelf GPS receiver and a lowcost 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 longterm 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
During the last decade many papers about lowcost 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 lowcost 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, wheelspeed 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 inhouse constructed IMU hardware and its synchronization with the GPSreceiver. 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 errorsources 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 PentiumM 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 1115 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 microcontroller to the PC. The microcontroller 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 GPSreceiver. However more sensors can be incorporated into the system through the USBports 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 sensorfusion [7], improving the system performance in the absence of GPSdata. 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
PSfrag replacements
IMU/GPS
IMU C GPS
Signal processing
External sensors
USB port
PC
Power supply
Battery DCDC converter
Communication
TouchWLAN GSM/GPRS screen
220V to 12V
Figure 2: Blockdiagram of the navigation platform. The platform is constructed around ve modules.
With reference to Figure 3, the IMU has been constructed inhouse around MicroElectroMechanical System (MEMS) accelerometers and gyros from AnalogDevices. 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 3dimensional 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 degreeoffreedom 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 10bit analog to digital converter. Further, the microcontroller also communicates with the GPSreceiver 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 GPSreceiver, 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
PSfrag replacements
60 m
60 mm
Gyroscope zaxis 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
PSfrag replacements
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 lowfrequency 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 lowfrequency INS component and the predominantly highfrequency 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 lowpass 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 EarthCenteredEarthFixed (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 coordinateframe 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 ECEFcoordinates. Note that the attitude can be calculated from Re and vice versa. p
4 S OFTWARE A LGORITHM
C7
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
Navigation Attitude
p ie
Gyros
p ip
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 crossproduct operator. The rotational rate bep tween the ECEF and platformcoordinates, 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 specicforce measured in the s platformframe into a specicforce in the ECEFframe. 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
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
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 GPSreceiver position estimates (red crosses) from the eldtest. 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 GPSreceiver 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.
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.
C10
1 0.5 0
Accelerometer biases
xaxis yaxis zaxis
Bias [m/s2]
0.5 1
0
3
20
40
60
80
5
PSfrag replacements
x 10
100
120
140
160
180
200
Bias [rad/s]
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 lowcost strapdown IMU by means GPS, Ortung und Navigation, no. 1, pp. 5165, June 1999. [6] J. Farrell, T. Givargis, and M. Barth, Realtime differential carrier phase GPSaided INS, IEEE Trans. on Control Systems Technology, vol. 8, no. 4, pp. 709721, July 2000. [7] G. Dissanayake, S. Sukkarieh, E. Nebot, and H. DurrantWhyte, The aiding of a lowcost 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 PCbased platform for inertial a navigation, in Proc. NORSIG 2006, Reykjavik, Iceland, June 2006. [11] I. Skog and P. H ndel, A lowcost 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
Abstract
An approach for calibrating a lowcost 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 rRao 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 rRao bound within 8 dB, e and thus the proposed method may be acceptable for a wide range of lowcost applications.
1 Introduction
The development in microelectromechanical 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
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 inhouse 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 inhouse IMU is calibrated, and the estimated model parameters are examined. The conclusions are drawn in Section 6.
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 ith accelerometer sensitivity axis around the jth 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
D3
PSfrag replacements
60 m
60 mm
Gyroscope zaxis Micro controller
Figure 1: 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.
D4
C ALIBRATION
OF A
MEMS
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
Figure 2: The accelerometer sensitivity axes {xa , y a , z a } are mounted to span a 3dim 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 ith gyro sensitivity axis around the jth 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)
D5
V (f ) [V olt]
V f
PSfrag replacements
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 nonzero 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 ith 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
(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 gyrocluster 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 gyrocluster, 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
vk yk Sensor output
T1
Figure 4: Sensor model including misalignments T1 , scale factors K, biases b, and measurement noise vk .
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
where um and ym are the input and sample mean at the mth orientation and rotation, respectively. This reduced cost function may be minimized offline 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
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
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
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
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 inhouse 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
Axis yz zy zx
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 scalefactors, 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 lowcost applications.
5.2
Calibration of IMU
The accelerometer cluster of the inhouse 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 yaxis in the IMU, which should be close to zero. This is due to the fact that the x and yaccelerometers are mounted into the same MEMS sensor case. The remaining two misalignments angles correspond to the misalignment between the x and zaxis respectively yand zaxis, which due to the impreciseness when the IMU was constructed, should be much
6 C ONCLUSIONS
D11
6 Conclusions
A MEMS sensor based inertial measurement unit has been constructed inhouse, intended to be used in a lowcost 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 rRao 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 lowcost 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
90
Scale factors
85
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 scalefactors 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
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
75
Bias
70
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 PCbased platform for inertial a navigation, in Proc. NORSIG 2006, Reykjavik, Iceland, June 2006. [2] N. ElSheimy, S. Nassar, and A. Noureldin, Wavelet denoising 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
[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. 4th 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 GPSaided inertial navigation systems
Isaac Skog and Peter H ndel a Submitted to IEEE Transactions on Intelligent Transportation Systems
Abstract
The effects of time synchronization errors in a GPSaided 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 GPSreceiver 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 realworld 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 nonzero 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
[]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 GPSreceiver. 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. Specicforce 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)
Angularrate 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 GPSaided inertial navigation systems (INS) based on commercial offtheshelf components (COTS) is the problem of time synchronization in the sampling of the GPSreceiver 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 adhoc 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 strapdown INS. In [2, 3] and [4], the effects of using timedelayed GPS measurements in a GPSaided strapdown INS were studied by examining the steadystate 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 closedloop GPSaided INS is studied in terms of the increased error covariance of the system. In Section 3, by modeling the timing error as a trajectorydependent bias in the GPSreceivers position estimates and by studying the behavior of navigation errors in the closedloop GPSaided INS,
E4
T IME
SYNCHRONIZATION ERRORS IN
GPS AIDED
PSfrag replacements
t = k Ts
Navigation t = l T s Td , l = k M Filter
GPS
Figure 1: Effects of the timing error Td in the synchronization between the sampling of the GPSreceiver and IMU are studied in terms of the navigation systems increased error covariance. Here, the ratio between the sampling rate of the IMU and GPSreceiver 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 closedloop 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 singleaxis GPSaided 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 inhouse developed software for simulations and tests for lowcost GPSaided INSs. In Section 6, the integration software and the time synchronization are tested with both simulated and realworld 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 C OVARIANCE
E5
+1
= f (xk xk , uk )
IMU
uk
uk
Navigation Equations
xk+1
Hk
uk
xk
Kk
pINS k+1
GPS
pGPS k+1
Figure 2: Block diagram of a closedloop GPSaided INS model. The EKF recursions used to fuse the GPSreceiver and the IMU data may be interpreted as a way of choosing the gain matrices Kk in the feedback.
tem, the statespace 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 statespace 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 textbooks [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 downsampling function
#
(k) =
1, 0,
Here, M denotes the ratio between the sampling rate of the IMU and the GPSreceiver, 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 statespace 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
Table 1: The indirect extended Kalman lter algorithm for integration between GPSreceiver 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 GPSreceiver 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)
For an error model of the form (2)(7) the corresponding extended Kalman lter recursions, used to fuse the IMU and GPSreceiver data in a closedloop system, are given in Table 1, as derived in [12]. This set of equations may graphically be interpreted as the closedloop system in Fig. 2, where the feedback gain is chosen as the Kalman lter gain.
3.1
ClosedLoop Error
To nd a difference equation for the error in the closedloop 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 GPSreceiver are correlated both in time and space with a covariance structure depending on the positioning algorithm of the GPSreceiver [8].
3 C OVARIANCE
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 closedloop system z f,k is found to be
zf,k+1
= =
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 onestepahead 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 onestepahead 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 onestepahead 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 GPSaided INS. The dynamics of the errors in the system are determined by the time varying closedloop 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 closedloop system is given by the onestepahead 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
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 GPSreceiver. Further, let the function p(t) describe the true trajectory of the navigation platform. Then the position estimates from the GPSreceiver at time k can be described as
E8
T IME
SYNCHRONIZATION ERRORS IN
GPS AIDED
pGPS = (k)(p(k Ts Td ) + wk ) k
(11)
where wk is the estimation error of the GPSreceiver. Assume that a rough synchronization of the GPSreceiver 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 secondorder 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 closedloop 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)
dk = vk Td + ak
zf d,k+1
= = +
k zf d,k Kp,k ra + Gk ek k
%
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
E9
zf d,k+1
Equation (18) has similarities with the difference equation (10) describing the estimation error in the closedloop 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 closedloop GPSaided 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 timevarying 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
Consider the singleaxis 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 GPSreceiver 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
satellite
satellite
acceleration a(t)
gravity vector
Figure 3: Example of a singleaxis GPSaided 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 GPSreceiver having an inherent time delay.
= = =
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
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
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 singleaxis 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 zeroorder 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 statespace model of how the singleaxis 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
(31)
(32)
(33)
(34)
E12
T IME
SYNCHRONIZATION ERRORS IN
GPS AIDED
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 singleaxis GPSaided INS example. The vehicle tilt is modeled as the output of a rstorder 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 GPSreceiver and the INS is observed, the observation equation for the error model becomes
Gk = Gc (k Ts ) =
0 0 1 0
1 0 0 0
Ts 1 0 0
0 0 Ts 1
0)
(36)
(37)
E13
Table 2: Setting used when evaluating the singleaxis GPSaided INS with the aid of numerical simulations.
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 GPSaided INS, respectively. All other variables are the same for both cases.
(39)
Hk Qk,l
= =
(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 GPSreceiver. Having derived the statespace 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 singleaxis 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.
E14
T IME
SYNCHRONIZATION ERRORS IN
GPS AIDED
4
[m]
2 0 1
10
20
30
40
50
60
70
80
90
100
[m/s]
0.5 0 3
10
20
30
40
50
60
70
80
90
100
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 GPSreceiver 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)
E15
0.2 0.15
[ /s]
0.15 0.1
[ /s]
0.05 0 0.05 0 20 40
[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
= = =
(46)
%
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 spacemodel 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 shorthand 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
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
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 crosscorrelation 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)
(53)
(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 singleaxis example below. zext = (k Kp,k Hk ) zext f d,k f d,k
Td
xk+1 = f (xk xk , uk )
5 I MPLEMENTING
E17
IMU
uk
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 closedloop GPSaided INS system with feedback for the compensation of the time delay in the GPSreceiver. One possibility of implementing the variable delay in reality is to use an adjustable fractional delay lter.
4.1
Consider again the singleaxis GPSaided 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 statespace model.
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
2
[m]
1 0
100
200
300
400
500
600
0.4
[m/s]
0.2 0 4
100
200
300
400
500
600
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 FDFIR 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 unitdelay before the FDlter. 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 thirdorder 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
E19
0.1 0.08
[ /s]
0.04 0.02
[ /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 FDlter was used as variable delay in a Monte Carlo simulation of the onedimensional GPSaided integrated navigation system with an extended error model described in Section 4.1. The deviation between the lowpass 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 lowpass characteristics. In Figs. 810, the results from the Monte Carlo simulation of the singleaxis GPSaided 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
100
200
300
400
500
600
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 T IME
E21
Magnitude Response
1
Magnitude
0
Phase [rad]
Phase Response
PSfrag replacements
0.3
0.4
0.5
Figure 11: Magnitude and phase response of the thirdorder 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 onedimensional GPSaided INS to timesynchronize the IMU and GPSreceiver 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 GPSaided INS is shown. The results of the Monte Carlo simulation together with the values predicted by the error covariance models are shown in Figs. 1321. 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
50 0 50
Simulation trajectory
North [m]
PSfrag replacements
50
East [m]
50
100
Figure 12: Trajectory used in the Monte Carlo simulation of the GPSaided INS and the evaluation of the corresponding covariance expressions.
6 T IME
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.
3
[m]
[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
Realworld data
Realworld 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 inhouse 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
1
[m/s]
0.5 0 0 1
100
200
300
400
[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
E25
2 1 0 0 2 1 0 0 5
[ ]
PSfrag replacements
[ ]
100
200
300
400
[ ]
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
[m/s2 ]
[ /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. Xaxis (forward) red line, Yaxis (sideways) green line, Zaxis (downward) blue line.
6 T IME
E27
2
[m]
1 0 0 2
100
200
300
400
[m]
PSfrag replacements
1 0 0
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
1
[m/s]
0.5 0 0
100
200
300
400
0.6
[m/s]
PSfrag replacements
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
E29
2 deg 1 0 0 2 deg 1 0 0 4
PSfrag replacements
100
200
300
400
100
200
300
400
deg
[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
10
[m/s2 ]
x 10
0.1
[ /s]
PSfrag replacements
300
400
6 T IME
E31
60
[ms]
[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
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 GPSaided INS, respectively. All
6 T IME
E33
20 0 20 40
North [m]
PSfrag replacements
Figure 22: Realworld 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
Table 4: Estimated time delay (Td ) for articial timing error (Td ) and maximum error (2Dhorizontal plane/3Dspherical) during a 20 second long GPS outage starting after 390 seconds.
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
H1 , H2 ,
k < M k M
(58)
The observation matrix for the piecewise constant system can written as [20] (W o is a size N 16 matrix)
vj
j = 1, 2.
(59)
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
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 tangentcoordinate plane, turns are indirectly changing the north and east velocity components).
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
03,1 012,1 1
)
'
'
Wo =
' ' ' '
0 0 0 0 0 0 0
0)
(60)
(61)
(62)
(63)
E36
T IME
SYNCHRONIZATION ERRORS IN
GPS AIDED
the error covariance of the navigation system given the vehicle trajectory and a statespace model of the error plant of the integrated navigation system have been derived. Further, under the assumption of Gaussiandistributed 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 onedimensional GPSaided 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 onedimensional GPSaided 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 GPSreceiver 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 inhouse developed software for integration of GPS and IMU data. The time synchronization was then tested on both simulated and realworld 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 realworld 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 GPSreceiver and IMU sensor in a GPSaided 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 GPSreceiver 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 statespace 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 jth 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
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 Gaussiandistributed 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 abovedescribed state vector, the continuous time state transition matrix of the system becomes
(75)
R EFERENCES
E39
Here, S(sn ) denotes the skewsymmetric 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. BarItzhack 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 GPSslaved 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. McGrawHill, 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
AIAA, 1997.
[12] I. Skog and P. H ndel, A lowcost 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 PCbased platform for inertial a navigation, in Proc. NORSIG 2006, Reykjavik, Iceland, June 2006. [18] S. Winkler and P. V rsmann, Multisensor data fusion for small autonomous uno manned aircraft, European Journal of Navigation, vol. 5, no. 2, pp. 3241, May 2007. [19] N. ElSheimy and X. Niu, The promise of MEMS to the navigation cummunity, InsideGPS, pp. 4656, March/April 2007. [20] D. GoshenMeskin and I. BarItzhack, Observability analysis of piecewise constant systemspart 1: Theory, IEEE Trans. on Aerospace and Electronic Systems, vol. 28, no. 4, pp. 10561067, Oct. 1992. [21] , Observability analysis of piecewise constant systemspart 2: Application to inertial navigation inight 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. AbdelHafez, 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 nonzero means, IEEE Trans. on Automatic Control, vol. AC16, pp. 263265, 1971.