GNSS-aided INS for land vehicle positioning and navigation

Transcription

1 Thesis for the degree of Licentiate of Engineering GNSS-aided INS for land vehicle positioning and navigation Isaac Skog Signal Processing School of Electrical Engineering KTH (Royal Institute of Technology) Stockholm 2007

2 Skog, Isaac GNSS-aided INS for land vehicle positioning and navigation Copyright c 2007 Isaac Skog except where otherwise stated. All rights reserved. TRITA-EE 2007:066 ISSN Signal Processing School of Eletrical Engineering KTH (Royal Institute of Technology) SE Stockholm, Sweden Telephone + 46 (0)

3 Abstract This thesis begins with a survey of current state-of-the art in-car navigation systems. The pros and cons of the four commonly used information sources GNSS/RF-based positioning, vehicle motion sensors, vehicle models and map information are described. Common filters to combine the information from the various sources are discussed. Next, a GNSS-aided inertial navigation platform is presented, into which further sensors such as a camera and wheel-speed encoder can be incorporated. The construction of the hardware platform, together with an extended Kalman filter for a closed-loop integration between the GNSS receiver and the inertial navigation system (INS), is described. Results from a field test are presented. Thereafter, an approach is studied for calibrating a low-cost inertial measurement unit (IMU), requiring no mechanical platform for the accelerometer calibration and only a simple rotating table for the gyro calibration. The performance of the calibration algorithm is compared with the Cramr-Rao bound for cases where a mechanical platform is used to rotate the IMU into different precisely controlled orientations. Finally, the effects of time synchronization errors in a GNSS-aided INS are studied in terms of the increased error covariance of the state vector. Expressions for evaluating the error covariance of the navigation state vector are derived. Two different cases are studied in some detail. The first considers a navigation system in which the timing error is not taken into account by the integration filter. 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 filter, synchronizing the IMU and GNSS-receiver data. i

4

5 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 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 find - that s how it all started. I owe it all to you. To my brother, Elias, and my half-sister, Julia, I love you the most! iii

6

7 Contents Abstract Acknowledgements Contents i iii v I Introduction 1 Introduction 1 1 Contributions of the Thesis Related papers not included in the thesis II Included papers 5 A State-of-the art and future in-car navigation systems a survey A1 1 Introduction A1 2 State-of-the art systems A3 3 Global Navigation Satellite Systems and Augment Systems.... A5 4 Vehicle Motion Sensors A8 4.1 Dead reckoning and inertial navigation A13 5 Vehicle models and motions A16 6 Map information A18 7 Information Fusion A Non-linear filtering A21 8 Conclusions A22 References A23 B A low-cost GPS aided inertial navigation system for vehicle applications B1 1 Introduction B1 2 Navigation Dynamics B2 v

8 2.1 Navigation equations B2 2.2 Error equations B3 3 Discretization B5 3.1 Discrete time navigation equations B5 3.2 Discrete time error equations B5 4 Extended Kalman Filtering B6 5 Design and Conclusions B8 5.1 Hardware Design B9 5.2 Simulation results B9 References B11 C A Versatile PC-Based Platform For Inertial Navigation C1 1 Introduction C1 2 System Overview C2 3 Sensors C2 4 Software Algorithm C4 5 Results C8 6 Conclusions an Further Work C9 References C11 D Calibration of a MEMS inertial measurement unit D1 1 Introduction D1 2 Sensor Error Model D2 3 Calibration D6 4 Cramér Rao Lower Bound D8 5 Results D9 5.1 Performance Evaluation D9 5.2 Calibration of IMU D10 6 Conclusions D11 Appendix A D15 References D15 E Time synchronization errors in GPS-aided inertial navigation systems E1 1 Nomenclature E1 2 Introduction E3 3 Covariance of the estimation error E4 3.1 Closed-Loop Error E6 3.2 Timing Errors in Closed-Loop E7 3.3 Example: Single-axis GPS-aided INS E9 4 Modelling the timing error in the integration filter E Example: Single-axis GPS-aided INS, revisited E17 5 Implementing a variable delay in the navigation filter E17 6 Time synchronization applied to a low-cost GPS-aided INS.... E Simulated data E21 vi

9 6.2 Real-world data E23 7 Observability of time delay error E34 8 Results and Conclusions E35 Appendix A E36 Appendix B E38 References E39 vii

10

11 Part I Introduction

12

13 Introduction In-car navigation involves three distinguished processes: estimation of the vehicles position and velocity relative to a known reference, path planing, and route guidance. The first capability, positioning, is essential for successful path planing and route guidance capability. Nowadays, the area of high-performance positioning systems and methods is well developed. The challenge is to develop highperformance system solutions using low-cost sensor technology. This is the topic of the thesis, consisting of the following five papers. Paper A: I. Skog and P. Händel, State-of-the art and future in-car navigation systems a survey, submitted to IEEE Transactions on Intelligent Transportation Systems. Paper B: I. Skog and P. Händel, A low-cost GPS aided inertial navigation system for vehicle applications, in Proc. EUSIPCO 2005, (Antalya, Turkey), Sept Paper C: I. Skog, A. Schumacher and P. Händel, A Versatile PC-Based Platform For Inertial Navigation, in Proc. NORSIG 2006, (Reykjavik, Iceland), June Paper D: I. Skog and P. Händel, Calibration of a MEMS inertial measurement unit, in Proc. XVII IMEKO World Congress, (Rio de Janeiro, Brazil), Sept Paper E: I. Skog and P. Händel, Time synchronization errors in GPS-aided inertial navigation systems, submitted to IEEE Transactions on Intelligent Transportation Systems. 1 Contributions of the Thesis The contributions in this thesis appears in terms of five papers, devoted to different areas associated with the development of low-cost in-car navigation solutions. An introduction to land vehicle navigation is provided in paper A, written as a survey of the current state-of-the art in-car navigation technology; to mediate a

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

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

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

17 Part II Included papers

18

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

20 c 2007 IEEE The layout has been revised

21 State-of-the art and future in-car navigation systems a survey Isaac Skog and Peter Händel Abstract A survey of the information sources and information fusion technologies used in the current in-car navigation systems is presented. The pros and cons of the four commonly used information sources GNSS/RF-based positioning, vehicle motion sensors, vehicle models and map information are described. Common filters to combine the information from the various sources are discussed. A prediction of possible tracks in the further development of in-car navigation systems concludes the survey. 1 Introduction Today a large share of private cars is delivered from the factory with a GPS-based in-car navigation system. Owners of used cars can at, a reasonable cost, install one of the many third party in-car navigation systems on the market. These navigation aids are designed to support the driver by showing the vehicle s current location on a map and by giving both visual and audio information on how to efficiently 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 fire 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 fleet as efficiently as possible. To further improve the usefulness of these in-car navigation systems, for example, with information such as when, where and how to make lane changes with respect to the planned course changes, the accuracy of both the navigation systems and digital maps has to be improved [1, 2]. Increasing the accuracy and robustness of the navigation systems implies that the traffic coordinators could guide their vehicle fleets even more efficiently in terms of the traffic flow in different road lanes, etc. Refer to [3] for a discussion of robustness enhancement of

22 A2 STATE-OF-THE ART AND FUTURE IN-CAR NAVIGATION SYSTEMS A SURVEY Information sources GNSS/RF-based Positioning PSfrag replacements Vehicle Motion Sensors Road Maps Information Fusion User Information Vehicle State Guidance Vehicle Models Traffic Situation Information ADAS Camera/Radar/Laser Figure 1: Conceptional description of the available information sources and information flow for a in-car navigation system. The block with dashedlines are in general not an apart of current in-car navigation systems but will likely be a major part of next generation in-car navigation systems and advanced driver assistant systems (ADAS). a bus fleet monitoring system. Moreover, further development of advanced driver assistance systems (ADASs) and safety applications such as automatized highway systems, lane/road departure detection and warning systems, and collision avoidance requires not only navigation systems with higher accuracy but also better reliability and integrity, i.e., redundant information sources are needed [4]. With reference to Fig. 1, looking at the in-car navigation problem from an information perspective there are basically five 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 fifth type of information source - visual, radar, or laser information - is generally not used in current systems, but plays a major role in the development of ADASs, etc. Details on the incorporation of visual information into vehicle navigation systems and safety application systems are found in [6]. For designers of in-car navigation systems, the problem is to choose which of these information sources, if not all, to use and how to combine the information to meet performance

23 2 STATE-OF-THE ART SYSTEMS A3 requirements. This necessitates a balance between the cost, complexity and performance of the system. When evaluating the performance of a navigation system, it is important to remember that accuracy is only one of four performance measurements characterizing the system. The performance measurements are [7, 8]: Accuracy the degree of conformity of information concerning position, velocity, etc. provided by the navigation system relative to actual values Integrity measure of the trust that can be put in the information from the navigation system, i.e., the likelihood of undetected failures in the specified 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 system s probability of continuously providing information without non-scheduled interruptions during the intended working period. Before entering into a discussion on possible ways to achieve increased navigation performance, it is important to point out that the area of high-performance navigation is well developed. Nowadays, the challenge is to develop high-performance navigation system solutions using low-cost sensor technology [9]. The purpose of this paper is to present a survey of current in-car navigation technology: possibilities, limitations and various design approaches. Section 2 describes state-of-the art in-car navigation systems and their pitfalls. Sections 3 to 6 describe the idea of operation, together with pros and cons of the four commonly used information sources in current in-car navigation. Section 7 is devoted to the problem of combining information from the different sources. Section 8 concludes the survey with a prediction of different tracks in the further development of in-car navigation systems. 2 State-of-the art systems Generally, current commercially available in-car navigation systems match the information from a GPS receiver with that of a digital map, so called mapmatching [1, 2, 10, 11]. That is, by comparing the trajectory and position information from the GPS receiver with the roads in the digital map, the most likely position of the vehicle on the road is estimated. In urban environments, buildings may partly block satellite signals, forcing the GPS receiver to work with a poor geometric constellation of satellites and thereby reducing the accuracy of the position estimates [12 15]. Even worse, less than four satellites may become available, making position fixes impossible and interrupting the continuity of the navigation solution. Moreover, multi-path propagation of the radio signal due to reflection in

24 A4 STATE-OF-THE ART AND FUTURE IN-CAR NAVIGATION SYSTEMS A SURVEY surrounding objects may lead to decreased position accuracy without notification by the GPS receiver, thereby reducing the integrity of the navigation solution [15]. Therefore, to counteract navigation solution degradation in situations with poor satellite constellation geometry, shadowing and multipath propagation of the satellite signals, advanced in-car navigation systems use information from additional sensors such as accelerometers, gyroscopes and odometers. To give an example, Siemens car navigation system uses a gyroscope and odometer to perform dead reckoning (DR). The trajectory estimated from dead reckoning is then projected onto the digital map. If the estimated position is between several roads, several projections are done and the likelihood of each projection is estimated based on the information from the GPS receiver and the development of the trajectory over time [10, 11]. Including additional sensors is not merely a question of giving the navigation system higher accuracy, better integrity or providing a more continuous navigation solution. It also allows the update rate of the system to be increased and provides more information such as acceleration, roll and pitch, depending on which types of sensors are used. The typical update rate for a GPS receiver is less than 20 times per second [16], whereas modern low-cost accelerometers and gyroscopes have update rates (bandwidths) of hundreds of Hertz. This means that even the high-frequency dynamics of the vehicle can be captured by the in-car navigation system. To give absolute figures on the accuracy of state-of-the art in-car navigation systems and navigation systems in general is difficult, since the performance of the systems depends not only on the characteristics of the sensors, GPS receiver, vehicle model and map information but also on the trajectory dynamics and surrounding environment. However, an indication of the achievable performance that can be expected from an in-car navigation system based on fusion of GPS-position estimates with an odometer and gyroscope based dead reckoning system (DRS) (no map-matching or vehicle model) can be found in an excellent paper [17]. The authors evaluate how much the error in each individual sensor contributes to the total error in the position estimates of a land vehicle traveling at constant speed along a straight road. The sensitivity analysis shows that when GPS-position data is available, 90% or more of the long- and cross-track positioning error is due to GPS-positioning errors. Further, performance during GPS outages is mainly determined by the drift characteristics and accuracy with which the DR sensors were calibrated before the outage. The implication of this finding is that in order to design a robust navigation system from low-cost dead reckoning sensors, a high-accuracy positioning aiding system is needed. Hence, the accuracy of the in-car navigation system is highly dependent on available low-cost GPS receiver solutions.

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

26 A6 STATE-OF-THE ART AND FUTURE IN-CAR NAVIGATION SYSTEMS A SURVEY Satellit Satellit Pseudo Range Uncertainty Region Receiver Satellit True Range PSfrag replacements Figure 2: Conceptional description of the positioning of a GNSS receiver. Under ideal circumstances, the propagation times of the satellite signals calculated by the GNSS receiver correspond to the true ranges between the receiver and the satellites, and the position of the receiver is given by the interception of the circles (spheres in 3-dim). Due to errors in the range estimates, there is no single interception point, but rather an interception region reflecting the possible positions of the receiver. worth noting that (1) is based on several assumptions such as uncorrelated range estimates and circular Gaussian-distributed position estimation errors, which more or less hold true [21]. Therefore (1) should only be used as a rough indication of position error. Since common mode errors are the same for all GNSS receivers in a restricted local area, they can be compensated by having a stationary GNSS receiver at a known location that estimates common mode errors and transmits correction information to rover GNSS receivers. This technology is commonly referred to as differential GNSS (DGNSS). The correlation of the common mode error decreases with the distance between the reference station and the rover unit. This will also be the case with the system performance [22]. The problem can be solved by a network of reference stations over the intended coverage area. The errors observed by these stations are constantly sent to a central processing station, where a map of the ionospheric delay,

27 3 GLOBAL NAVIGATION SATELLITE SYSTEMS AND AUGMENT SYSTEMS A7 Table 1: Standard deviations of errors in the range measurements in a singlefrequency GPS receiver [16]. Error Source Standard deviation [m] Common mode Ionospheric 7.0 Clock and ephemeris 3.6 Tropospheric 0.7 Non-common mode Multi-path Receiver noise Total (UERE) CEP with a horizontal dilution of precision, HDOP=1.2 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 specific location [8, 23]. There are several satellite-based augmentation systems (SBASs) that, through geostationary satellites, regionally provide correction information free of charge for the GPS and GLONASS systems. In North America, there is the Wide Area Augmentation System (WAAS), in Europe the European Geostationary Navigation Overlay Service (EGNOS) and in Japan the Multi-functional Satellite Augmentation System (MSAS). Further, the GAGAN system for India and SNAS system for China are under development [23 25]. 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 ( MHz) frequency of the GPS system, and therefore only the software for GPS receivers has to be modified to receive correction data. Many low-cost GPS receivers are able to use correction data from the SBASs [24]. In areas where obstruction prevents the reception of the EGNOS signal from any of the geostationary satellites, the information may be obtained from the EGNOS data access system, broadcasting the information via Internet-SISNeT (Signals in Space through the Internet) [26, 27]. Test results, based on correction data from the WAAS and EGNOS systems, demonstrate position accuracy in the range of 1-2 m in the horizontal plane and 2-4 m in the vertical plane at a 95% confidence 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

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

29 4 VEHICLE MOTION SENSORS A9 Table 2: Sensors commonly used as a complement to GNSS-receivers for enhancement of in-car navigation systems. Sensor Steering encoder Odometer Velocity encoders Electronic compass Accelerometer Gyroscope Measurement Front wheel direction Travelled distance Wheel velocities (Indirectly, heading) Heading relative magnetic north Acceleration Angular velocity A steering encoder measures the angle of the steering wheel. Hence, it provides a measure of the angle of the front wheels relative to the forward direction of the vehicle platform. Together with information on the wheel speeds of the front wheel pair, the steering angle can be used to calculate the heading rate of the vehicle. An odometer provides information on the traveled curvilinear distance of a vehicle by measuring the number of full and fractional rotations of the vehicle s 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 vehicle s velocity by observing the rotation rates of the wheels. If separate encoders are used for the left and right wheel of either the rear or front wheel pair, an estimate of the heading change of the vehicle can be found through the difference in wheel speeds. Information on the speed of the different wheels is often available through the sensors used in the anti-lock breaking system (ABS). See [36 38] for details. For a kinematic vehicle model as illustrated in Fig. 3, the left and right rear wheel velocities v lr and v rr, respectively, together with the track width, tw, can be mapped to a heading rate estimate as: ψ = v rr v lr. (2) tw By measuring the velocity of the left and right front wheels, v lf and v rf, respectively and observing the steering angle δ, the yaw rate can be estimated as: ψ = v rf v lf tw cos(δ). (3) The dependency of the steering angle δ is due to the variation in efficient 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

30 A10 STATE-OF-THE ART AND FUTURE IN-CAR NAVIGATION SYSTEMS A SURVEY v lf - Velocity left front wheel PSfrag replacements v lr - Velocity left rear wheel δ tw - Track width Center of gravity ψ - Yaw rate v -Velocity vector β - Direction of velocity Steering angle v rf - Velocity right front wheel v rr - 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 efficient wheelbase (track width), and limited resolution and sample rate of the wheel encoders. The first three error sources are terrain dependent and occur in a non-systematic way. This makes it difficult 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

31 4 VEHICLE MOTION SENSORS A11 wheel diameter, unequal wheel diameter and uncertainties in efficient 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 earth s magnetic north by observing the direction of the earth s local magnetic field [17]. To convert the compass heading into an actual north heading, the declination angle (i.e., the angle between the geographic and magnetic north) is needed, which is position dependent. Thus, knowledge of the compass position is necessary to calculate the heading relative to geographic north. Generally, the compass is constructed around three magneto-resistive or fluxgate 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 earth s magnetic field 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 sufficient and cost-effective. In [18] and [40], details about compasses based on flux-gate magnetometers can be found. A review of magnetic sensors is found in [41]. Power lines, metal structures such as bridges and buildings, along the trajectory of the vehicle cause variations in the local magnetic field, resulting in large and unpredictable errors in the heading estimates of the compass. Therefore, the usefulness of magnetic compasses in in-car navigation systems can be questioned [17]. However, there are other applications of magnetic sensors in in-car navigation systems. See [4], where magnetic sensors are used to detect the vehicle s 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 object s 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 vehicle s acceleration and rotation is obtained and can be mapped into estimates of the vehicle s attitude, velocity and position. There are many different ways to construct inertial sensors. In [18], a description of common technologies and their typical performance parameters can be found. A description of the trends in inertial sensor technology is offered in [42]. Historically, inertial sensors have mostly been used in high-end navigation systems for missile, aircraft and marine applications, due to the high cost, size and power consumption of the sensors. However, with the progress in microelectromechanical-system (MEMS) sensor technology it has become possible to construct inertial sensors meeting the cost and size demands needed for low-cost

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

33 4 VEHICLE MOTION SENSORS A13 In order to measure the vehicle s dynamics in both the long- and cross-track direction, a cluster of inertial sensors is needed, referred to as an inertial sensor assembly (ISA). Depending on the construction of the navigation system, the ISA may consist of solely accelerometers, but more frequently a combination of accelerometers and gyroscopes is used. See [49] and [50] for examples of all accelerometer-based navigation systems. In general, a six-degree-of-freedom ISA, i.e., an inertial measurement unit (IMU) designed for unconstrained navigation in three dimensions, consists of three accelerometers and three gyroscopes, where the sensitivity axes of the accelerometers are mounted to be orthogonal and span a three-dimensional space, and the gyroscopes measure the rotation rates around these axes. 4.1 Dead reckoning and inertial navigation Velocity encoders, accelerometers and gyroscopes all provide information on the first 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 configuration, if the navigation is done in two or three dimensions and the constraints on the movements of the vehicle. Basically, they all include three steps: 1. The estimation of attitude (3-dim) or heading (2-dim) of the vehicle relative the navigation coordinate system. 2. The translation of the traveled distance, velocity and acceleration into navigation coordinates using the attitude or heading information. 3. The integration of traveled distance, velocity and acceleration over time to obtain position and velocity estimates in the navigation coordinate frame. In Fig. 4, the method of DR in two dimensions is illustrated in terms of vector addition [10]. The position (x i, y i ) of the vehicle at time i is calculated based on information on the heading ψ i and the traveled distance l i from the last known location (x i 1, y i 1 ). 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

34 A14 STATE-OF-THE ART AND FUTURE IN-CAR NAVIGATION SYSTEMS A SURVEY y PSfrag replacements ψ 1 (x 1, y 1 ) ψ 2 l 2 l 1 (x 2, y 2 ) (x 0, y 0 ) x Figure 4: Dead-reckoning in terms of vector addition. The position (x i, y i) at time i is calculated based upon information about the heading ψ i and the travelled distance l i from the last known location (x i 1, y i 1). magnetometer (electronic compass), a gyroscope or a combination of these methods and sensors. Refer to [10, 11, 36, 37] for details on how dead reckoning is done in vehicle navigation systems. In Fig. 5, a block diagram of a strap-down 3 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 specific force observed in platform coordinates into the navigation frame, where the gravity acceleration is subtracted from the observed specific 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, 51 53] for a thorough treatment of the subject of inertial navigation. In [54], a survey of inertial systems terminology can be found. 3 The term strap-down referees to that the gyroscopes and accelerometers are rigidly attached to the navigation platform. In a gimballed INS the sensors are mounted on a platform isolated from the rotations of the vehicle [20].

35 PSfrag replacements 4 VEHICLE MOTION SENSORS A15 INS Navigation Equations Gravity Model Accelerometers Coordinate Rotation + + dt dt Position dt IMU Gyroscopes Attitude Determination Velocity Attitude Figure 5: Conceptional sketch of a strap-down INS. The integrative nature of the navigation calculations in DR and inertial navigation systems gives the systems a low-pass filter characteristic that suppresses highfrequency sensor errors but amplifies low-frequency sensor errors. This results in a position error that grows without bound as a function of the operation time or traveled distance, and where the error growth depends on the error characteristics of the sensors. In general, it holds that for an INS a bias in the accelerometer measurements causes error growth proportional to the square of the operation time, and a bias in the gyroscopes causes error growth proportional to the cube of the operation time [44, 49, 55, 56]. The detrimental effect of the gyroscope errors on the navigation solution is due to the direct reflections 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 vehicle s accelerations are significantly smaller than the gravity acceleration, small errors in attitude may cause large errors in estimated accelerations. These errors are then accumulated in the velocity and position calculations. Hence, the error characteristics of the gyroscopes used in the IMU are of major concern when designing an INS. To summarize, the properties of DRSs and INSs are complimentary to those of the GNSSs and other radio-based navigation systems. These properties are: They are self-contained, i.e., they do not rely on any external source of information that can be disturbed or blocked. The update rate and dynamic bandwidth of the systems are mainly set by the system s 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.

36 A16 STATE-OF-THE ART AND FUTURE IN-CAR NAVIGATION SYSTEMS A SURVEY Contrary to these properties, the GNSS and other radio-based navigation systems give position and velocity estimates with a bounded error but at relatively low rate and depend on information from an external source that may be disturbed. The complimentary features of the two types of systems make their integration favorable and if properly done results in navigation systems with higher update rates, accuracy, integrity and ability to provide a more continuous navigation solution under various conditions and environments. Odometers and velocity and steering encoders have proven to be very reliable DR sensors. For movements in a planar environment, they can provide reliable navigation solutions during several minutes of GNSS outages. However, in environments that significant violate the assumption of a planar environment, accuracy is drastically reduced [56]. An INS constructed around a full-six-degreeof-freedom IMU does not include any assumption of the motion of the navigation system and therefore is independent of the terrain in which vehicle is traveling. Moreover, it provides three-dimensional position, velocity and attitude information, and if situated in the package of the GNSS receiver reduces the need for vehicle fixed sensors. In combination with decreasing cost, power consumption and size of the MEMS inertial sensors, this makes vehicle navigation systems incorporating MEMS IMUs attractive. However, current ultra low-cost MEMS inertial sensors have an error characteristic causing position errors in the range of tens of meters during 30 seconds of stand-alone operation [9, 14, 44, 57]. This is also illustrated in Fig. 6, where the root mean square (RMS) horizontal position error during a 30-second GNSS signal outage in a GNSS-aided INS is shown. In the simulation, the IMU sensors were modeled as ideal sensors, except from having measurement noises, turn-on to turn-on and time varying biases reflect current ultra low-cost MEMS inertial sensors. 5 Vehicle models and motions Under ideal conditions, a vehicle moving in a planar environment experiences no wheel slip and no motions in the direction perpendicular to the road surface. Thus, in vehicle coordinates, the downward and sideways velocity components should be close to zero. In [14, 44, 56], this type of non-holonomic constraint has been applied to the navigation solution of the vehicle-mounted GNSS-aided INSs. The results show a great reduction in position error growth during GNSS outages and increased attitude accuracy when imposing non-holonomic constraints on the navigation solution. In Fig. 6, the reduction in error growth using non-holonomic constraints in a GPS-aided INS using a MEMS IMU is illustrated. The case when observing the forward velocity from a simulated velocity encoder is also shown. In the case of both non-holonomic constraints and forward velocity aiding, error growth during the outage is negligible. From an estimation-theoretical perspective, sensors and vehicle-model information play an equivalent role in the estimation of the vehicle state [5]. If there

37 5 VEHICLE MODELS AND MOTIONS A17 RMS horizontal position error 10 8 NOC NHC NHC+VA m 6 4 PSfrag replacements s Figure 6: Empirical root mean square (RMS) horizontal position error growth during a 30-second satellite signal blockage in a low-cost GPSaided INS. NOC - No constraints, NHC - Non-holonomic constraints, NHC+VA - Non-holonomic constraints and velocity aiding. were a perfect vehicle model, such that the vehicle state could be perfectly predicted from control inputs, sensor information would be superfluous. Contrarily, if there were such things as perfect sensors, the vehicle model would provide no additional information. Neither of these extremes exists. It is clear, however, that navigation system performance can be enhanced by utilizing vehicle models. Moreover, the incorporation of a vehicle model in the navigation system may allow the use of less costly sensors without degradation in navigation performance. There are numerous vehicle model and motion constraints, ranging from the above-mentioned non-holonomic constraints to more advanced models incorporating wheel slip, tire stiffness, etc. Different vehicle models and constraints of varying complexity can be found in [5, 12, 44, 56, 58 60]. 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 refined vehicle models, especially in the accuracy of the orientation estimate. However, it is difficult to find good vehicle models, independent of the driving situation [59]. More advanced models require knowledge about several parameters such as vehicle type, tires,

38 A18 STATE-OF-THE ART AND FUTURE IN-CAR NAVIGATION SYSTEMS A SURVEY Table 3: Bandwidth of the true motion dynamics of a land-vehicle as estimated by [61]. Motion Bandwidth [Hz] Acceleration x-axis (forward) < 2 y-axis (sideways) < 2 z-axis (downwards) < 8 Angular velocity x-axis (roll) < 8 y-axis (pitch) < 8 z-axis (yaw) < 2 and environmental specifics [56]. To adapt the model to different driving conditions, these parameters must be estimated in real-time. Alternatively, the driving conditions must be detected and used to switch between different vehicle models. An example of this, using an interactive multimodel extended Kalman filter, can be found in [59]. Another way to incorporate knowledge about the vehicle dynamics into the navigation system is through pre-filtering/denoising of the sensor s measurements using the efficient bandwidth of the vehicle s motion dynamics and characteristics of the sensor noises [61 63]. In Table 3, the bandwidth of the actual motion dynamics of a land vehicle as estimated by [61] is shown. The wider bandwidth of the pitch and roll angular velocity and z-axis acceleration dynamics is due to road irregularities. In [63] and [61], these bandwidths, together with a noise model, are used to develop de-noising algorithms that are tested on three IMUs of different quality. The results show a 56% reduction in attitude errors during GNSS outages in the case of the MEMS IMU, and even more with high-quality IMUs. Since the attitude error of an INS is directly related to position error growth, a reduction in attitude error also implies a reduced position error. In [62], a deeper description of the idea behind the de-noising approach is given together with test results on a flight-mounted GPS-aided INS. The results are similar to those in [61, 63]. 6 Map information Under normal conditions, the location and trajectory of a car is restricted by the road network. Hence, a digital map of the road network can be used to impose constraints on the navigation solution of the in-car navigation system, a process referred to as map-matching. Traditionally, map-matching has been a unidirectional process, where the position and trajectory estimated by the GNSS receiver, vehicle motion sensors and vehicle model information have been used as input to produce a position and trajectory consistent with the road network of the digital

39 6 MAP INFORMATION A19 arc candidates region PSfrag replacements Node (Intersection) Position Estimate Shape point Node (dead-end) Figure 7: Road network described by a planar model. The street system is represented by a set of arcs (i.e., curves in R 2 ). Generally, a set of candidate arcs/segments close to the position estimate are selected first, 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 flow in the map-matching has become feasible, viewing the map information as observations in the estimation of the information fusion [6]. This type of bidirectional map-matching is found in [64 66]. Commonly, the road network is represented by a planar model in the digital maps, where the street system is represented by a set of arcs (i.e., curves in R 2 ) [67, 68]. Each arc represents a road in the network and is assumed to be piece-wise linear, such that it can be described by a finite set of points (see Fig. 7). The first and last points in the set are referred to as nodes and the rest as shape points. The nodes describe the beginning and termination of the arc, indicating a start, dead-end or an intersection (i.e., a point where it is possible to go from one arc to another) in the street system. Matching the output of the navigation system to the road-network of the digital map generally involves three steps. First, a set of candidate arcs or segments are selected. Second, the likelihood of the candidate arcs/segments is evaluated using geometrical and topological information. Finally, the vehicle location on the most likely road segment is determined. The geometrical information includes measures like closeness between the position estimate and nearest road in the map; the difference in heading as indicated by the navigation system and road segments of concern; and the difference in the shape of the road segments with respect to estimated trajectory. Refer to [67] for a discussion and description of common measures such as point-topoint, point-to-curve, and curve-to-curve matching to extract geometrical information. The topological information criterion determines the connectivity of the

40 A20 STATE-OF-THE ART AND FUTURE IN-CAR NAVIGATION SYSTEMS A SURVEY candidate roads (arcs), e.g., the vehicle cannot suddenly move from one road segment to another if there is no intersection point in between the segments. The likelihood of the road segment candidates is found by assigning different weights to the geometrical and topological information measures and combining them. Refer to [10, 64, 65, 68, 69] for various weighting and combining approaches such as belief theory, fuzzy network and state machines. In [70] a survey of the current state-of-the art map-matching algorithms is found, together with ideas on further research directions. 7 Information Fusion Numerous filters can be used to fuse the information from the different information sources into an estimate of the vehicle state: various versions of extended Kalman filters (EKFs) are used in [4, 36, 59, 71]; Sigma-Point filters are used in [72 74]; particle filters 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 filters can be used in basically two ways, a direct integration or a complimentary filtering approach. In the direct method, information from all sources is used as observations for a filter housing a vehicle model, relating the observation to an estimate of the vehicle s state. The dynamics of most vehicles include highly deterministic components, which are difficult to model in the stochastic framework of many filters [16]. This is avoided in the complimentary filtering approach. In the complimentary filtering, 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 filter 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 filter 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 filter, results not only in a model that better fits into the statistical framework but also in a smaller bandwidth of the filter, since it estimates the slowly changing errors and not the full navigation stage. Hence, the noise sensitivity of the filter is reduced. In [77], a deeper description of the concept of complimentary filtering, together with an example of information fusion in an underwater vehicle, is found.

41 PSfrag replacements 7 INFORMATION FUSION A21 Vehicle Dynamics Sensors Sensor errors Navigation Equations Navigation errors Navigation Solution Mapping and Down sampling Filter + + Aiding Information Figure 8: Information fusion using a complimentary filtering approach with feedback. The vehicle motion sensors, together with the navigation equations, provide the major navigation solution, and the other information sources aid the DR/INS system through estimations and corrections of errors in the calculated navigation solution. 7.1 Non-linear filtering The most widely used nonlinear filtering 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 filtering 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 filter [78]. For non-gaussian distributed noise sources, the Kalman filter provides the linear MMSE solution to the filtering problem. Unfortunately, linearization in the EKF means that the original problem is transformed into an approximated problem which is solved optimally, rather than approximating the solution to the correct problem. This can seriously affect the accuracy of the obtained solution or lead to divergence of the system. Therefore, in systems of a highly nonlinear nature and non-gaussian noise sources, more refined nonlinear filtering approaches such as Sigma-Point filters (Unscented Kalman filters), particle filters (sequential Monte Carlo methods) and exact recursive nonlinear filters, which keep the nonlinear structure of the problem, may significantly improve system performance [73, 79]. The inherent weakness of these nonlinear filtering approaches is the curse of dimensionality. That is, in general the computational complexity of the filter grows exponentially with the dimension of the state vector to be estimated [79]. Therefore, even with today s computational capacity, nonlinear filters can be unfeasible for navigation systems with high-dimensional state vectors. However, since the navigation equations in many navigation systems are only partial nonlinear, the filtering problem can be divided into a linear and nonlinear part, where the linear part, under the assumption of Gaussian-distributed noise entries, may be solved using a Kalman filter, hence reducing the computa-

42 A22 STATE-OF-THE ART AND FUTURE IN-CAR NAVIGATION SYSTEMS A SURVEY tional complexity of the system [80, 81]. A short introduction to nonlinear filtering and the advantages and disadvantages of various algorithms are given in [79]. In [66], a framework for positioning, navigation and tracking using particle filters is developed, and its usage is illustrated through examples of car positioning by map-matching, terrain navigation of aircraft, etc. 8 Conclusions A survey of information sources and information fusion technologies used in incar navigation systems has been presented. The pros and cons of the four commonly used information sources - GNSS/RF-based positioning, vehicle motion sensors, vehicle models and map information - have been examined. Common filter techniques to combine the information from different sources have been briefly discussed. Summarizing the survey, the following items are likely to improve in nextgeneration in-car navigation systems: In a scenario where all GNSSs have reached their full constellations, i.e., 30 Galileo satellites, 24 GLONASS satellites, and 35 GPS satellites, hybrid system GNSS receivers will have more than 25 satellites in view at any one time. Thus, the risk of blockages and poor satellite constellations is highly reduced. Three separate systems will also contribute to a higher integrity level. The modernization of GNSSs with multiple civil-user frequencies, together with development of low-cost multi-frequency receivers and carrier phase augmentation systems, will allow for decimeter-level positioning accuracy at a cost suitable for in-car navigation systems. Further developments in MEMS inertial sensor technology, especially MEMS gyroscopes, will allow for ultra low-cost micro IMUs that can bring full 3-dimensional attitude information to in-car navigation systems. Refined digital maps with lane information etc. will allow map-matching procedures with a bidirectional information flow, not only producing a position and trajectory consistent with the road network but also feeding back information from the map-matching to the sensor fusion. Increased processing power at reduced power consumption and cost levels will allow for usage of refined vehicle models and non-linear filtering technologies. Further, until this stage both OEM and third-party in-car navigation systems primarily have been developed to provide positioning and route-guidance information to the user. As the technological development of in-car navigation systems

43 REFERENCES A23 progresses, two tracks can be distinguished with different target applications and constraints on the navigation solution. One track is small handheld personal navigation devices, e.g., incorporated into mobile phones, used for positioning route guidance for both pedestrian and vehicles. The second track is OEM navigation systems, designed not only for route guidance but also to provide highly reliable inputs to advanced driver assistance and safety systems, such as automatized highway system, lane keeping and collision avoidance These systems not only require navigation systems with higher accuracy, update rates and availability but also good integrity for fast detection of sensor and sub-system failures jeopardizing the reliability of the advanced driver assistance systems. References [1] J. Du and M. Barth, Bayesian probabilistic vehicle lane matching for link-level invehicle navigation, in Proc. of IEEE Int. Veh. Symp., Tokyo, Japan, June 2006, pp [2], Lane-level positioning for in-vehicle navigation and automated vehicle location (AVL) systems, in Proc. of IEEE Int. Transport Sys. Conf., Washington, D.C, USA, Oct. 2004, pp [3] M. Tsakiri, M. Stewart, T. Forward, D. Sandison, and J. Walker, Urban fleet monitoring with GPS and GLONASS, Navigation, J. of The Institute of Navigation, vol. 51, no. 3, pp , Sept [4] Y. Yang and J. Farrell, Magnetometer and differential carrier phase GPS-aided INS for advanced vehicle control, IEEE Trans. Robotics and Automation, vol. 19, no. 2, pp , Apr [5] S. Julier and H. Durrant-Whyte, On the role of process models in autonomous land vehichle navigation systems, IEEE Trans. Robotics and Automation, vol. 19, no. 1, pp. 1 14, Feb [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 , Sept [7] G. Hein, From GPS and GLONASS via EGNOS to Galileo-positioning and navigation in the third millennium, GPS Solutions, vol. 3, no. 4, pp , Apr [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 , Aug [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 [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 , Jan [11], Fusion of map and sensor data in a modern car navigation system, Journel of VLSI Signal Processing, vol. 45, no. 1-2, pp , Nov

44 A24 STATE-OF-THE ART AND FUTURE IN-CAR NAVIGATION SYSTEMS A SURVEY [12] J. Huang and H.-S. Tan, A low-order DGPS-based vehicle positioning system under urban enviroment, IEEE/ASME Trans. Mechatronics, vol. 5, no. 1-2, pp , Oct [13] B. Boberg, Robust navigation, Swedish Journal of Military Technology, no. 3, pp , [14] S. Godha and M. Cannon, GPS/MEMS INS integrated system for navigation in urban areas, GPS Solutions, vol. 11, no. 3, pp , July [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 , Sept [16] J. Farrell and M. Barth, The Global Positioning System and Inertial Navigation. McGraw-Hill, [17] E. Abbott and D. Powell, Land-vehicle navigation using GPS, Proceedings of the IEEE, vol. 87, no. 1, pp , Jan [18] D. Titterton and J. Weston, Strapdown Inertial Navigation Technology-2nd Edition. IEE, [19] ESA, Sept [20] M. Grewal, L. Weill, and A. Andrews, Global Positioning Systems, Inertial Navigation and Integration. Wiley, [21] F. Diggelen, GNSS Accuracy: Lies, Damm Lies, and Statistics, GPS World, no. 1, pp , Jan [22] D. Lapucha, R. Barker, and H. Zwaan, Wide area carrier phase positioning, European Journal of Navigation, vol. 3, no. 1, pp , Feb [23] R. Prieto-Cerdeira, J. Samson, J. Traveset, and B. Rastburg, Ionospheric information broadcasting methods for standardization in SBAS L5, in Proc. IEEE/ION Position, Location, And Navigation Symposium, SAN DIEGO, CA, USA, Apr [24] K. Dixon, Satellite positioning systems: Efficiencies, Performance and Trends, European Journal of Navigation, vol. 3, no. 1, pp , Feb [25] A. Gorski and G. Gerten, Do we need augmentation systems, European Journal of Navigation, vol. 5, no. 4, pp. 6 12, Sept [26] F. Torán, J. Jantura-Traveset, R. Luca, C. Echazarreta, C. Seynat, and et al, The EGNOS data acces system (EDAS), European Journal of Navigation, vol. 5, no. 3, pp , July [27] M. Opitz and R. Weber, Development of a SISNeT simulation software-sissim, European Journal of Navigation, vol. 4, no. 2, pp , May [28] R. Chen and X. Li, Virtual differential GPS based on SBAS signal, GPS Solutions, vol. 8, no. 4, pp , Sept [29] ESA, Sept [30] P. Enge and P. Misra, Special issue on global positioning system, Proceedings of the IEEE, vol. 87, no. 1, pp. 3 15, Jan [31] P. Misra, B. Burke, and M. Pratt, GPS performance in navigation, Proceedings of the IEEE, vol. 87, no. 1, pp , Jan

45 REFERENCES A25 [32] H. Lee, J. Wang, C. Rizos, D. Grejner-Brzezinska, and C. Toth, GPS/Pseudolite/INS integration: concept and firts test, GPS Solutions, vol. 6, no. 1 2, pp , Oct [33] G. Sun, J. Chen, W. Guo, and K. Ray Liu, Signal processing techniques in networkaided positioning: a survey of state-of-the-art positioning designs, IEEE Signal Processing Magazine, vol. 22, no. 4, pp , July [34] A. Sayed, A. Tarighat, and N. Khajehnouri, Network-based wireless location: challenges faced in developing techniques for accurate wireless location information, IEEE Signal Processing Magazine, vol. 22, no. 4, pp , July [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 , July [36] R. Carlson, J. Gerdes, and J. Powell, Practical position and yaw rate estimation with GPS and differential wheelspeeds, in Proc. AVEC th Int. Symp. Advanced Vehicle Control, Hiroshima, Japan, Sept [37], Error sources when land vehicle dead reckoning with differential wheelspeeds, Navigation, the J. of The Institute of Navigation, vol. 51, no. 1, pp , [38] C. Hay, Turn, turn, turn, GPS world, pp , Oct [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 , Dec [40] T. Peters, Automobile navigation using a magnetic flux-gate compass, IEEE Trans. on Veh. Techn., vol. 35, no. 2, pp , May [41] J. Lenz, A review of magnetic sensors, Proceedings of the IEEE, vol. 78, no. 6, pp , June [42] N. Barbour and G. Schmidt, Inertial sensor technology trends, IEEE Sensors Journal, vol. 1, no. 4, pp , Dec [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. 8 28, March April [44] N. El-Sheimy and X. Niu, The promise of MEMS to the navigation cummunity, InsideGPS, pp , March/April [45] Chatfield, Fundamentals of High Accuracy Inertial Navigation. AIAA, [46] R. Rogers, Applied Mathematics in Integrated Navigation systems. AIAA Education Series, [47] P. Aggarwal, Z. Syed, X. Niu, and N. El-Sheimy, Cost-efficient testing and calibration of low cost MEMS sensors for integrated positioning, navigation and mapping systems, in Proc. XXIII FIG Congress, Munich, Germany, Oct [48] I. Skog and P. Händel, Calibration of a MEMS inertial measurement unit, in Proc. XVII IMEKO WORLD CONGRESS, Rio de Janeiro, Brazil, Sept [49] C. Tan and S. Park, Design of accelerometer-based inertial navigation systems, IEEE Trans. Instrumentation and Measurement, vol. 54, no. 6, pp , Dec

46 A26 STATE-OF-THE ART AND FUTURE IN-CAR NAVIGATION SYSTEMS A SURVEY [50] J. Chen, S. Lee, and D. DeBra, Gyroscope free strapdown inertial measurement unit by six linear accelerometers, Journal of Guidance, Control and Dynamics, vol. 17, no. 2, pp , [51] P. Savage, Strapdown inertial navigation integration algorithm design, part 1: attitude algorithms, Journal of Guidance, Control and Dynamics, vol. 21, no. 1, pp , Jan [52], Strapdown inertial navigation integration algorithm design, part 2: velocity and position algorithms, Journal of Guidance, Control and Dynamics, vol. 21, no. 2, pp , Mar [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 , Oct [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, PLANS 2004, Apr [55] S. Sukkarieh, E. Nebo, and H. Durrant-Whyte, A high integrity IMU/GPS navigation loop for autonomous land vehicle applications, IEEE Trans. Robotics and Automation, vol. 15, no. 3, pp , June [56] G. Dissanayake, S. Sukkarieh, E. Nebot, and H. Durrant-Whyte, The aiding of a low-cost strapdown inertial measurement unit using vehicle model constraints for land vehicle applications, IEEE Trans. Robotics and Automation, vol. 17, no. 5, pp , Oct [57] X. Niu, S. Nasser, C. Goodall, and N. El-Sheimy, A universal approach for processing any MEMS inertial sensor configuration for land-vehicle navigation, Navigation, J. of The Institute of Navigation, vol. 60, no. 2, pp , May [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 , Dec [59] R. Toledo-Moreo, M. Zamora-Izquierdo, B. beda Miarro, and A. Gmez- Skarmeta, High-integrity IMM-EKF-based road vehicle navigation with low-cost GPS/SBAS/INS, Proc. of IEEE Int. Transport Sys. Conf., vol. 8, no. 4, Sept [60] S. Julier and H. Durrant-Whyte, Process models for the high-speed navigation of road vehicles, in Proc. IEEE International Conference on Robotics and Automation, Nagoya, Japan, May [61] K. Chiang, H. Hou, X. Niu, and N. El-Sheimy, Improving the positioning accuracy of DGPS/MEMS IMU integrated systems utilizing cascade de-noising algorithm, in Proc.17th International Technical Meeting of the Satellite Division of the Institute of Navigation ION GNSS 2004, Long Beach, CA, USA, Sept [62] J. Skaloud, A. Bruton, and K. Schwarz, Detection and filtering of short-term (1/f) noise in inertial sensors, Navigation, the J. of The Institute of Navigation, vol. 46, no. 2, pp , Summer [63] K. Chiang, C. Goodall, and N. El-Sheimy, Improving the attitude accuracy of INS/GPS integrated systems, European Journal of Navigation, vol. 4, no. 2, pp , May 2006.

47 REFERENCES A27 [64] M. El Najjar and P. Bonnifait, A road-matching method for precise vehicle localization using belief theory and kalman filtering, Auton. Robots, vol. 19, no. 2, pp , [65], Road selection using multicriteria fusion for the road-matching problem, Proc. of IEEE Int. Transport Sys. Conf., vol. 8, no. 2, pp , June [66] F. Gustafsson, F. Gunnarsson, N. Bergman, U. Forssell, J. Jansson, R. Karlsson, and P.-J. Nordlund, Particle filters for positioning, navigation, and tracking, IEEE Trans. on Signal Processing, vol. 50, no. 2, pp , Feb [67] D. Bernstein and A. Kornhauser, An Introduction to Map Matching for personal navigation assistants. New Jersy TIDE 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 , Dec [69] S. Kim and J. Kim, Adaptive fuzzy-network-based c-measure map-matching algorithm forcar navigation system, Trans. on Industrial Electronics, vol. 48, no. 2, pp , Apr [70] M. Quddus, W. Ochieng, and R. Noland, Current map-matching algorithms for transport applications: State-of-the art and future research directions, ELSEVIER, Transportation Research Part C: Emerging Technologies, vol. 15, no. 5, pp , Oct [71] I. Skog and P. Händel, A low-cost GPS aided inertial navigation system for vehicle applications, in Proc. EUSIPCO 2005, Antalya, Turkey, Sept [72] E. Shin and N. El-Sheimy, Unscented kalman filter and attitude errors of low-cost inertial navigation systems, Navigation, J. of The Institute of Navigation, vol. 54, no. 1, pp. 1 9, Spring [73] R. Merwe, E. Wan, and S. Julier, Sigma-point Kalman filters for nonlinear estimation and sensor-fusion: Applications to integrated navigation, in AIAA Guidance, Navigation, and Control Conference and Exhibit, Rhode Island, USA, Aug [74] S. Cho and W. Choi, Robust positioning technique in low-cost DR/GPS for land navigation, IEEE Trans. on Veh. Techn., vol. 55, no. 4, pp , Aug [75] N. Yang, W. Tian, Z. Jin, and C. Zhang, Particle filter for sensor fusion in a land vehicle navigation system, Meas. Sci. Technol., vol. 16, no. 3, pp , Mar [76] J. Wang and Y. Gao, GPS-based land vehicle navigation system assisted by a low-cost gyro-free INS using neural network, Navigation, J. of The Institute of Navigation, vol. 57, no. 3, pp , Oct [77] A. Pascoal, I. Kaminer, and P. Oliveira, Navigation system design using time-varying complementary filters, IEEE Trans. on Aerospace and Electronic Systems, vol. 36, no. 4, pp , Oct [78] T. Kailath, A. Sayed, and B. Hassibi, Linear Estimation. Prentice Hall, [79] F. Daum, Nonlinear filters: Beyond the Kalman filter, IEEE Aerospace and Electronic Systems Magazine, vol. 20, no. 8, pp , Aug

48 A28 STATE-OF-THE ART AND FUTURE IN-CAR NAVIGATION SYSTEMS A SURVEY [80] T. Schön, F. Gustfasson, and P.-J. Nordlund, Marginalized particle filters for mixed linear/nonlinear state-space models, IEEE Trans. on Signal Processing, vol. 53, no. 7, pp , July [81] R. Karlsson, T. Schön, and F. Gustafsson, Complexity analysis of the marginalized particle filter, IEEE Trans. on Signal Processing, vol. 53, no. 11, pp , Nov

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

50 c 2005 EURASIP The layout has been revised

51 A low-cost GPS aided inertial navigation system for vehicle applications Isaac Skog and Peter Händel Abstract In this paper an approach for integration between GPS and inertial navigation systems (INS) is described. The continuous-time navigation and error equations for an earthcentered earth-fixed INS system are presented. Using zero order hold sampling, the set of equations is discretized. An extended Kalman filter for closed loop integration between the GPS and INS is derived. The filter 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 briefly discussed. Using a battery operated PC the system is fully mobile and suitable for real-time vehicle navigation. Simulation results of the system are presented. 1 Introduction Today many vehicles are equipped with global positioning system (GPS) receivers that constantly can provide the driver with information about the vehicles position with an accuracy in the order of meters [1]. However, the GPS receiver has two major weaknesses. The slow update rate, only once a second for most receivers and the sensitivity to blocking of the satellite signals. An inertial navigation system (INS) is an alternative tool for positioning and navigation. A classical reference on low-cost INS for mobile robot applications is [2]. In the opposite of the GPS receiver an INS is self contained and can provide position, velocity and attitude estimates at a high rate, typically 100 times per second [3]. However, due to the integrative nature of the INS, low frequency noise and sensor biases are amplified. The unaided INS may therefore have unbounded position and velocity errors [2]. These complementary properties make an integration of the two systems suitable, which is the topic of this paper. The primary motivation for the reported work is the in-house need for a GPS aided INS test-bed for education and research in the area of vehicle navigation and performance analysis. A goal is to develop a hardware platform and additional software, which together with a standard host-computer will work as a basic GPS aided INS. From the software skeleton the student/researcher can then build an application meeting their demands. Related work

52 B2 A LOW-COST GPS AIDED INERTIAL NAVIGATION SYSTEM FOR VEHICLE APPLICATIONS INS input INS Navigation output H KF - + GPS input Figure 1: Loosely coupled position aided closed loop implementation of a GPS aided INS system. KF denotes the extended Kalman filter, and H the map between navigation output and GPS data. do exist, for example in [4] a field evaluation of a low-cost GPS aided INS installed in a car is presented. In [4], the strap-down INS is integrated with two different GPS solutions (pseudo range and carrier phase differential GPS, respectively) using a Kalman filter. In this paper a loosely coupled position aided method is proposed which allows the designer to keep the costs low by using an off-the-shelf GPS receiver that provides position estimates employing NMEA (National Marine Electronics Association) data transmission protocol and sentence format. In Section 2, the INS equations and the corresponding error models are introduced. Next the navigation dynamics are discretized in Section 3. Section 4 presents an indirect extended Kalman filter (EKF) for the integration between the GPS and INS data. The INS provides the reference trajectory and output. The EKF then estimates the errors, which are fed back to the INS for correction of its internal states, resulting in a closed loop integration, see Figure 1. Implementation aspects, simulations results and conclusions are presented in Section 5. 2 Navigation Dynamics A strap-down INS comprises two distinguished parts. The inertial measurement unit (IMU) housing the accelerometers and gyros. The computational part, consisting of several differential equations, translates the measurements into position, velocity and attitude estimates. The calculations are performed in two steps. From the gyro measurements the directional cosine matrix relating the body coordinate frame to the used navigation frame is propagated. The coordinate rotation matrix is then used when solving the differential equations relating accelerations in the body frame to the navigation coordinate system. An earthcentered earth-fixed (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 simplifies the integration between the two systems [1]. 2.1 Navigation equations The continuous-time navigation equations in the ECEF frame are [5]

53 2 NAVIGATION DYNAMICS B3 ṙ e = v e v e = R e b f b 2 Ω e ie v e + g e (1) Ṙ e b = R e b Ω b eb where r e and v e denote the position and velocity in 3-dimensional ECEF coordinates, respectively. The superscripts e, b and i (that will be used below) are used to denote in which coordinate frame a variable is resolved in, that is the ECEF, body or inertial frame. Further, f b is the measured acceleration in the body-frame, g e is the position dependent, but known earth acceleration in ECEF coordinates, that may be compensated for. The rotation matrix, R e b transforms a vector in the body frame to the ECEF frame. Although R e b has nine elements, 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 eb and Ω e ie are the skew-symmetric matrix representations of the angular rates ωeb b and ωie, e defined such that a ωeb b = Ω b eb a and a ωie e = Ω e ie a, where a is a 3 1 dimensional vector. The matrix Ω b eb reads 0 ωeb b z ωeb b y Ω b eb = ωeb b z 0 ωeb b x ωeb b y ωeb b x 0 where the elements ω b eb x, ω b eb y and ω b eb z are the angular rates of the body (vehicle) frame relative to the ECEF frame, resolved in the body navigation frame. The matrix, Ω e ie has the structure of (2) with the components of ω b eb replaced by the corresponding components of the earth rotational rate ω e ie. Since variations in the earth rotational rate ω e ie are neglectable, Ω e ie is assumed constant and known, while Ω b eb depends on the body to ECEF angular rates, ω b eb = [ω b eb x ω b eb y ω b eb z ] and thus is time-varying. Here ( ) denotes the transpose operation. The body to ECEF angular rates are obtained by subtracting the angular rate of the earth, resolved in body coordinates from the gyro outputs ω b ib, that is (2) ω e eb = ω b ib R b e ω e ie (3) where the rotation matrix from the ECEF to body coordinates is R b e = (R e b) 1 = (R e b) (4) The last equality is a result of the fact that the directional cosine matrix is an orthonormal matrix (that is, R e b R e b = I). With reference to Figure 1, the INS inputs are the 3-dimensional measured acceleration in the body frame f b and the 3-dimensional angular rates of the body frame with respect to the inertial frame of reference ωib. b 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

54 B4 A LOW-COST GPS AIDED INERTIAL NAVIGATION SYSTEM FOR VEHICLE APPLICATIONS measurement noise, respectively. Here the IMU sensors are assumed to be the only noise sources in the system. Hence, the noise enters the system equations only through the attitude and velocity state, that is the two last equations in (1). Defining the error state vector δx(t) and the measurement noise vector u c(t) as δx(t) = δr e δv e ɛ δf b δω b ib u c(t) = u acc(t) u gyro(t) where δr e 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 u gyro(t) the gyro noise, respectively. Then, if neglecting gravity errors, the navigation error equations can be written as [5] where F(t) is the matrix F(t) = and G(t) is of size 15 6 δẋ(t) = F(t) δx(t) + G(t) u c(t) (7) 0 3 I Ω e ie F e R e b Ω e ie 0 3 R e b G(t) = R e b R e b (5) (6) (8) (9) The error equations (7) are time-varying, since R e b depends on the attitude and F e (as defined below) on the acceleration of the vehicle. In (8), I 3 (0 3) denotes the unity (zero) matrix of order 3 and F e is the skew symmetric matrix, defined as 0 f3 e f2 e F e = f3 e 0 f1 e f2 e f1 e 0 where f e l denotes the acceleration along the l: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. Let σ 2 acc and σ 2 gyro denote the variance of the accelerometer and the gyro noise, respectively. Then the covariance matrix, Q c(t) of the Gaussian measurement noise u c(t) in (6) is given by (10) E{u c(t + τ) u c(t)}= σ 2 acc I σ 2 gyro I 3 δ(τ) =Q c(τ) (11) where δ(τ) is the Kronecker delta.

55 3 DISCRETIZATION B5 3 Discretization The implementation of a GPS aided INS system requires that the navigation and error equations are discretized. First the navigation equations are discretized, where special care is taken to preserve the properties of the rotation matrix. Next the zero-order-hold sampling of the error equation is described. 3.1 Discrete time navigation equations Zero-order sampling of the position and velocity equations in (1) results in r e k+1 = r e k + T s v e k (12) v e k+1 = v e k + T s (R e b,k f b k 2Ω e ie v e k + g e ) (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 T s denote the sampling interval and assume that Ω b eb is constant. Then the matrix taking the solution of the attitude differential equation from time instant k T s to (k+1) T s is exp(ω b eb T s). Hence, the attitude equations can be approximated by R e b,k+1 = R e b,k exp(ω b eb T s) (14) By expanding the matrix exponential into an (n, n) Padè approximation the orthogonality constraints of the rotation matrix are preserved [1]. Using a (2, 2) Padè approximation the discrete attitude equation becomes R e b,k+1 = R e b,k(2i 3 + Ω b eb T s)(2i 3 Ω b eb T s) 1 (15) 3.2 Discrete time error equations Having a continuous-time equation as in (7) with a known solution at time t 0, the solution at a time t > t 0 can be represented as [8, 9]. t δx(t) = Φ(t, t 0) δx(t 0) + Φ(t, τ) G(τ) u c(τ)dτ (16) t 0 where the state transition matrix Φ(t, t 0) is defined 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 t 0)) [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 definition of the matrix exponential, the state transition matrix between time instants kt s and (k + 1)T s can be approximated as Φ((k + 1)T s, kt s) I 15 + F(kT s)t s (17) Hence, the discrete-time error equation becomes

56 B6 A LOW-COST GPS AIDED INERTIAL NAVIGATION SYSTEM FOR VEHICLE APPLICATIONS δx k+1 = Ψ k δx k + u d,k (18) where the state transition matrix Ψ k = Φ((k + 1)T s, kt s) is approximated as in (17) and the discrete-time process noise, u k is (k+1)t u d,k = s Φ((k + 1)T s, s)g(s)u c(s)ds (19) kt s Since u d,k is a linear combination of Gaussian noise, it is Gaussian distributed and described by its first and second order moments. The mean of u d,k is zero, since u c(t) is assumed zero mean. Applying the definition of covariance and assuming T s small, the covariance of the discrete-time noise Q d,k can be approximated as [1] Q d,k G(kT s) Q c(kt s) G (kt s) T s = diag(0 3, σ 2 acc I 3, σ 2 gyro I 3, 0 6) (20) where diag( ) denotes a block diagonal matrix. The last equality is a result of the orthonormality property of the rotation matrix R e b, and that Q c(t) is a diagonal matrix according to (11). The definition 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 w d,k the error in the GPS position estimates. Then the observation equation can be written as δy k = H k δx k + w d,k (21) with the state observation matrix H k of size 3 15, defined as H k = [I ], k = n l, n = 1, 2, , otherwise (22) where l denotes the ratio between the INS and GPS sampling frequency. 4 Extended Kalman Filtering The discrete non-linear navigation equations (12), (13) and (15) can be written as z k+1 = c(z k, a k ) + u k (23) where c(, ) denotes the dynamics, z k is the navigation system outputs: position, velocity and Euler angles defining the rotation matrix R e b, that is the 9-element vector z k = r e k v e k θ k (24) Further, the navigation system input is the 6-element vector a k which contains the inputs to navigation system, accelerations and angular rates, that is a k = f b k ω b ib, k (25)

57 4 EXTENDED KALMAN FILTERING B7 The vector u k is the measurement noise of the navigation inputs. Linearization of the navigation equations (23) are first 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 filter is applied. Then substituting the nominal trajectory with that of the INS estimated trajectory results in an extended Kalman filter. Consider the true state vector z k and the measured input ã k to the system written as z k = z nom k + δz k (26) ã k = a nom k + δa k (27) where z nom k and a nom k are the nominal trajectory and input, respectively. The quantity δz k is the perturbation away from the true trajectory and δa k the bias of the measurements. Assuming that δz k and δa k are small and applying a first order Taylor series expansion of c(z, a), equation (23) can be approximated as where z nom k+1 + δz k+1 c(z nom k, a nom k ) + C 1,k δz k + C 2,k δa k + u k (28) c(z, a) C 1,k = z z=z nom k C 2,k = c(z, a) a a=a nom k The Jacobians of c(z, a) are updated with nominal trajectory and input for each sample. Choosing z nom k and a nom k to fulfill the deterministic difference equation z nom k+1 = c(z nom k (29), a nom k ) (30) and substituting (30) into (28) results in a linear model for the error δz k, that is δz k+1 = C 1,k δz k + C 2,k δa k + u k (31) Note that δx k = δz k δa k, and thus it becomes clear that C1,k and C 2,k correspond 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 δa k develop with time. Since this is a linear model the standard Kalman filter equations can be applied to estimate δx k [9]. The Kalman filter equations read δẑ k+1 δâ k+1 δẑ k δẑ δâ k = k z δâk +K f,k y k H k nom k = Ψ k δẑ k δâ k (32) a nom k H k δẑ k δâ k (33) K f,k = P k H k (H k P k H k + R d,k ) 1 (34) P k = (I K f,k H k ) P k (35) P k+1 = Ψ k P k Ψ k + Q d,k (36) Here δâ k denotes the estimated biases in the measurements, et cetera. Variables with a minus sign, ( ) are predicted values, and those with superscript nom the one obtained from (30). The matrix R d,k is the covariance matrix of the error w d,k in the GPS position

58 B8 A LOW-COST GPS AIDED INERTIAL NAVIGATION SYSTEM FOR VEHICLE APPLICATIONS No GPS data available. (k 100, 200,...) â k = ã k + δâ k ẑ k+1 = c(ẑ k, â k ) δâ k+1 = [Ψ k] 10:15,10:15 δâ k P k+1 = Ψ k P k Ψ k + Q d,k GPS data available. (k = 100, 200,...) K f,k = P k H k (H k P k H k + R k) 1 [ ] [ ] [ ]) δẑk 09 1 ẑ = δâ k δâ + K f,k (y k H k k k ẑ k = ẑ k + δẑ k â k = ã k + δâ k P k = (I K f,k H k ) P k ẑ k+1 = c(ẑ k, â k ) δâ k+1 = [Ψ k] 10:15,10:15 δâ k P k+1 = Ψ k P k Ψ k + Q d,k Table 1: The algorithm for integration between GPS and INS data, with a ratio between the sample rates equal to 100 times. estimates y k. Now adding z nom k to both sides of equation (32) and substituting z nom k with the current estimate in all equations result in an extended Kalman filter, where the time and filter update for the estimates are given below ẑ k+1 = c(ẑ k, â k ) (37) δâ k+1 = [Ψ k ] 10:15, 10:15 δâ k (38) δẑ k δâ k = δẑ k δâ k + K f,k y k H k ẑ k (39) The solution to (37) is provided by the INS, since this corresponds to the navigation equations. The vector â k is the estimate of the true IMU-signal obtained by subtracting the estimated bias from the measured IMU signal. The only obstacle is the time update of navigation state errors δẑ k. If the estimated navigation error states are fed back to the INS for correction of the INS internal states, the corresponding error states can be set to zero [6]. Hence, δẑ k = The final algorithm for the integration is given in Table 1. 5 Design and Conclusions Discrete navigation equations for a direct ECEF INS implementation and the corresponding error model have been derived. Further, an indirect extended Kalman filter algorithm for integration between the position estimates from an off-the-shelf GPS receiver and the INS has been presented.

59 5 DESIGN AND CONCLUSIONS B9 rag replacements IMU GPS RS232 µc RS232 Host PC Figure 2: Block diagram of the hardware. 5.1 Hardware Design A GPS aided INS platform has been developed in-house, consisting of an off-the-shelf GPS receiver and an in-house IMU platform. The IMU platform comprises state-of-the-art MEMS gyros and accelerometers, and a micro-controller to control the data acquisition. The micro-controller controls the GPS-receiver via an RS232 serial interface. The GPS and INS data are synchronized and sent over a second RS232 serial interface to the host PC, see Figure 2. Using a battery operated PC the system is fully mobile and able to perform realtime signal processing [7]. However, at current state procedures for calculating the different calibration parameters are yet to be implemented and therefor no field tests are available. Below follows a short evaluation of the system for simulated IMU data, corresponding to a typical driving scenario. 5.2 Simulation results The superiority of the GPS aided system over traditional GPS is illustrated in Figure 3. In Figure 3 the dashed trajectory is the position estimates generated by simulated data as input to the GPS aided INS system. The shown specks are the GPS position and the solid line is the true trajectory. A ratio of 100 times was used between the INS and GPS sample ratio and the GPS position estimates had a standard deviation of ten meters. The biases of the accelerometers and gyros were in the order of 1-2 cm/s 2 respectively 5-10 /h. Not surprisingly, the GPS aided system clearly outperforms the GPS-system. Our current work is focused on studying and implementing different sensor error models and calibration methods, making the test-bed available for field tests. More detailed performance evaluations and results from field tests will be reported elsewhere.

60 B10 A LOW-COST GPS AIDED INERTIAL NAVIGATION SYSTEM FOR VEHICLE APPLICATIONS 150 GPS reading 100 INS/GPS estimate North [m] 50 True trajectory 0 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.

61 REFERENCES B11 References [1] Q. Honghui and J. Moore, Direct kalman filtering approach for GPS/INS integration, IEEE Trans. on Aerospace and Electronic Systems, vol. 38, no. 2, pp , Apr [2] B. Barshan and H. Durrant-Whyte, Inertial navigation systems for mobile robots, IEEE Trans. Robotics and Automation, vol. 11, no. 3, pp , June [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 [4] R. Dorobantu and B. Zebhauser, Field evaluation of a low-cost strapdown IMU by means GPS, Ortung und Navigation, no. 1, pp , June [5] Chatfield, Fundamentals of High Accuracy Inertial Navigation. AIAA, [6] J. Farrell and M. Barth, The Global Positioning System and Inertial Navigation. McGraw-Hill, [7] I. Skog, Development of a low cost gps aided ins for vehicles, Master s thesis, Dept. of Signals, Sensors and Systems, Royal Institut of Technology (KTH), Sweden, [8] K. Åström and B. Wittenmark, Computer Controlled Systems: Theory and Design. Prentice Hall, [9] T. Kailath, A. Sayed, and B. Hassibi, Linear Estimation. Prentice Hall, 1999.

62

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

64 c 2006 IEEE The layout has been revised

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

66 C2 A VERSATILE PC-BASED PLATFORM FOR INERTIAL NAVIGATION During the last decade many papers about low-cost GPS aided systems for land vehicles have been written, see for example [5, 6] and the references therein. Most of the described work focus on the implemented algorithm used in the sensor fusion, and few give an overview of the total system and its construction and features. This paper gives an overview and presents preliminarily results from the construction of a portable low-cost GPS aided INS platform, intended to be used for example in car racing and driver training. The platform is constructed with the aim of creating a GPS aided INS application skeleton into which further sensors such as a camera, wheel-speed encoder etc., can easily be incorporated. The outline of the paper is as follows: first, an overview of the platform is given in Section 2, describing its major building blocks and their features. Second, in Section 3 the sensors used in the navigation system are described. Focus is on the in-house constructed IMU hardware and its synchronization with the GPS-receiver. Next, in Section 4 the software algorithm used to fuse the sensor data is briefly described. In Section 5, preliminarily results from a first field test are presented, indicating which error-sources that need to be more accurately modelled. Finally, in Section 6 conclusions and areas for further work are discussed. 2 System Overview The navigation platform in Figure 1 is designed with the aim of being both portable and robust, but still inexpensive. It is constructed around five blocks: signal processing, power supply, communication, IMU/GPS and the external sensors, see Figure 2. The heart of the navigation platform is a Pentium-M 1.7 GHz PC, 512 MB RAM, 60 GB HD equipped with a 7 TFT touch screen. The PC handles all signal processing and interaction with the user. The PC and screen together with the IMU, power supply and wireless communication modules are built into an aluminium case, resulting in a robust system. The power supply module allows the platform to use three different power sources: the mains outlet (230 V AC), an external V DC source, or internal batteries. The internal batteries allow for 2 hours stand alone operation of the navigation platform. The wireless communication module consists of a WLAN and GSM/GPRS connection, for realtime transmission of navigation data to remote applications as well as input from wireless sensors. Furthermore, an external GPS receiver is connected via a micro-controller to the PC. The micro-controller synchronizes the sampling of the GPS and IMU, as described in Section 3. The described hardware platform provides a skeleton for the development of a easy to use navigation platform. 3 Sensors The navigation platform is designed around the IMU and a GPS-receiver. However more sensors can be incorporated into the system through the USB-ports on the front of the navigation platform. Here, two auxiliary sensors are under special consideration, a vehicle speed encoder and a camera. The vehicle speed encoder gives the possibility to include a vehicle model in the sensor-fusion [7], improving the system performance in the absence of GPS-data. The camera provides visual information about the drivers reaction in different situations.

67 3 SENSORS 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.

68 C4 A VERSATILE PC-BASED PLATFORM FOR INERTIAL NAVIGATION PSfrag replacements IMU/GPS Signal processing External sensors IMU USB port GPS µc PC Power supply Battery Communication WLAN GSM/GPRS Touchscreen DC-DC converter 220V to 12V The platform is con- Figure 2: Block-diagram of the navigation platform. structed around five modules. With reference to Figure 3, the IMU has been constructed in-house around Micro- Electro-Mechanical System (MEMS) accelerometers and gyros from Analog-Devices. The double and single axled accelerometers ADXL 203 and ADXL 103, respectively have been mounted so that their sensitivity axes are nearly orthogonal and span a 3-dimensional space. Due to impreciseness in the construction, the sensitivity axes will not be perfectly orthogonal. The three ADXRS 150 gyros are mounted to measure the rotational velocity around the accelerometer s sensitivity axes. This gives a six degree-of-freedom IMU capable of measuring accelerations and angular rates between ±1.5[g] and ±150[ /s], respectively. The sensors are controlled and sampled at a rate of 100 Hz by an AVR, ATmega 8 microcontroller, housing a built in 10-bit analog to digital converter. Further, the micro-controller also communicates with the GPS-receiver via a RS232 serial connection, synchronizing the sampling of the IMU and GPS. However, this synchronization does not take into account the processing delay inside the GPS-receiver, which may cause erroneous convergence of the integration algorithm [8]. The IMU and GPS data is enumerated by the controller and sent to the PC via a second RS232 serial connection. Due to impreciseness in the mounting and construction of the MEMS sensors the IMU has some statical errors such as misalignment between the sensitivity axes, scale factors and biases [9]. These errors may be identified 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-

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

70 C6 A VERSATILE PC-BASED PLATFORM FOR INERTIAL NAVIGATION PSfrag replacements EKF IMU INS ω p f ip p navigationequations ˆr e ˆv e ˆΦ Output estimated errors KF ˆΦ ˆf e ˆr e H - r e + 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 filter, estimating the navigation errors. The errors are fed back to the INS for correction of internal states. tial navigation system provides the main navigation solution. Since the INS is an integrative process the output of the INS is the actual position and a predominantly low-frequency error. When a GPS position estimate is available the difference between the position estimate of the GPS and the INS is calculated. This error signal contains two noise components: the predominantly low-frequency INS component and the predominantly high-frequency GPS component [6]. The error signal is fed to a Kalman filter (KF), designed to attenuate the GPS measurement error and provide an estimate of the INS errors. Hence, the KF will mainly have a low-pass characteristic. The estimated INS error state is the feedback to the INS for correction of its internal states, see Figure 4. The INS and KF together forms an indirect extended Kalman filter (EKF), where the navigation equations are linearized around the current navigation output. The continuous Earth-Centered-Earth-Fixed (ECEF) navigation equations used in the INS system to calculate position r e, velocity v e and attitude Φ from the accelerations f p and angular rates ω p ip are ṙ e = v e (1) v e = R e p f p 2 Ω e ie v e + g e (2) Ṙ e p = R e p Ω p ep. (3) Here and in the sequel, the superscripts e, p, and i denote in which coordinate-frame a quantity is expressed, the ECEF, platform and inertial frame, respectively. Further, R e p = R e p(φ) denotes the directional cosine matrix transforming a vector from platformto ECEF-coordinates. Note that the attitude Φ can be calculated from R e p and vice versa.

71 4 SOFTWARE ALGORITHM C7 INS Navigation Equations g e Gravity Acc f p R e p f e + r e t+ t t ( )dt ṙ e t+ t t ( )dt r e Navigation Position λ latitude ϕ longitude h height IMU Gyros ω p ip t+ t t ( )dt ω p ep ω p ie 2 Ω e iev e R p e Coriolis force ωie, e earth rotation Navigation Attitude r e ωie, e earth rotation φ roll θ pitch ψ yaw Figure 5: The block diagram corresponds to ECEF navigation equations used in the INS. The matrices Ω p ep and Ω e ie are the skew symmetric matrix representations of (ω p ep ) and (ω e ie ), respectively. Here ( ) denotes the cross-product operator. The rotational rate between the ECEF- and platform-coordinates, expressed in platform coordinates ω p ep may be calculated from the gyro measurements as ω p ep = ω p ip Re pω e 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. solving the difference equation in (3), transforming the specific-force measured in the platform-frame into a specific-force in the ECEF-frame. Next, the gravity g e and coriolis force 2Ω e iev e is subtracted. Remaining is then only the acceleration of the platform. Integrating 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 order terms a linear model relating the sensor errors δf p and δω p ip to the errors δre, δv e and δφ in the output of the INS may be found. Defining the errors as the perturbation between the calculated value and true value, respectively measured value and true value, then the error equations reads δv e = δṙ e (4) δ v e = F e δψ + R e p δf p + δg e 2 Ω e ie δv e (5) δ Ψ = R e p δω p ip Ωe ie Ψ. (6) Here F e is the skew symmetric matrix representation of (f e ) and δg e is the error in the gravity vector. Worth noting is that the error equations depend on the current navigation

72 C8 A VERSATILE PC-BASED PLATFORM FOR INERTIAL NAVIGATION state, i.e. R e p and f e. In [11] the navigation error equations are discretized and complimented with a model for how the sensor errors develop with time. A standard Kalman filter algorithm is applied to the error model, resulting in an indirect extended Kalman filter where the navigation equations are linearized around the output of the INS. 5 Results 0m 1000m Figure 6: Estimated trajectory (blue solid line) and GPS-receiver position estimates (red crosses) from the field-test. The vehicle was kept stationary (see top) for the first 30 seconds and then drove along the highway southward. In Figure 6, the estimated trajectory (blue solid line) and GPS-receiver position estimates (red crosses) from a field 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.