Low-Cost Navigation Systems a Study of Four Problems

Low-Cost Navigation Systems A Study of Four Problems

ISAAC SKOG

Doctoral Thesis in Signal Processing Stockholm, Sweden 2009 KTH, School of Electrical Engineering ACCESS Linnaeus Center TRITA-EE 2009:057 Signal Processing Laboratory ISSN 1653-5146 SE-100 44 Stockholm ISBN 978-91-7415-528-0 SWEDEN

Akademisk avhandling som med tillstånd av Kungl Tekniska högskolan framlägges till offentlig granskning för avläggande av teknologie doktorsex- amen i signalbehandling fredagen den 22 jan 2010 klockan 10.00 i hörsal F3, Lindstedtsvägen 26, Stockholm.

Tryck: Universitetsservice US AB iii

For the pleasure of ﬁnding things out!

Abstract

Today the area of high-cost and high-performance navigation for vehicles is a well-developed field. The challenge now is to develop high-performance navigation systems using low-cost sensor technology. This development involves problems spanning from signal processing of the dirty measurements produced by low-cost sensors via fusion and synchronization of information produced by a large set of diverse sensors, to reducing the size and energy consumption of the systems. This thesis examines and proposes solutions to four of these problems. The first problem examined is the time synchronizing of the sensor data in a global positioning system aided inertial navigation system in which no hardware clock synchronization is possible. A poor time synchronization results in an increased mean square error of the navigation solution and expressions for calculating this mean square error are presented. A method to solve the time synchronization issue in the data integration software is proposed. The potential of the method is illustrated with tests on real- world data that are subjected to timing errors. The second problem examined is the achievable clock synchronization accuracy in a sensor network employing a two-way message exchange model. The Cramér-Rao bound for the estimation of the clock parameters is derived and transformed into a lower bound on the mean square error of the clock offset. Further, an approximate maximum likelihood estimator for the clock parameters is proposed. The estimator is shown to be of low complexity and to have a mean square error in the vicinity of the Cramér- Rao bound. The third problem examined is the detection of the time epochs when zero-velocity updates can be applied in a foot-mounted pedestrian navigation system. Four general likelihood ratio tests for detecting when the navigation system is stationary based on the inertial measurement data are studied. The performance of the four detectors is evaluated using levelled ground, forward-gait data. The results show that the signals from the gyroscopes hold the most reliable information for the zero-velocity detection. The fourth problem examined is the calibration of a low-cost inertial measurement unit. A calibration procedure that relaxes the accuracy re-

v vi ABSTRACT quirements of the orientation angles the inertial measurement unit must be placed in during the calibration is studied. The proposed calibration method is compared with the Cramér-Rao bound for the case when the inertial measurement unit is rotated into precisely controlled orientations. Simulation results show that the mean square error of the estimated sensor model parameters reaches the Cramér-Rao bound within few decibels. Thus, the proposed method may be acceptable for a wide range of low-cost applications. Acknowledgements

Five years of work on my thesis are coming to an end, and I would like to take this opportunity to express my gratitude to all those who have supported me. First and foremost, I would like to thank my supervisor, Professor Peter Händel, who, for ﬁve years, has shared his knowledge in a most inspiring way. Thank you for your guidance and support. This journey has been a pure pleasure! Professor Magnus Jansson, thank you for being my co- supervisor. You have given me many valuable inputs and ideas. And Professor Mats Bengtsson, thank you for all your help in solving Latex problems. To my present and former fellow doctoral students and all my colleauges at the Signal Processing Lab, thank you for the interesting and stimulating discussions and for all the fun during the social events. I would also like to thank Annika Augustsson for taking care of a variety of administrative issues and for creating an enjoyable work environment: You spoil us! I would also like to thank Professor Naser El-Sheimy and the rest of the Mobile Multi-Sensor System Research group at the University of Calgary for the opportunity to be a part of your research team for six months. I learned a great deal. I am grateful to Professor Fredrik Gustafsson for acting as the opponent to this thesis, and Professor Danica Kragic, Professor Karl-Johan Åström, and Peter Hessling for participating in the grading committee. Last but not the least, I would like to express my heartfelt thanks to my family and all my friends for supporting me. You bring joy to my life.

Isaac Skog Stockholm, December 2009

vii Contents

Abstract v

Acknowledgements vii

Contents viii

1 Low-Cost Navigation Systems 1 1.1 Navigation...... 1 1.2 ProblemsinLow-CostNavigationSystems...... 1 1.3 ThesisOutlineandContributes ...... 3 1.4 RelatedResearch...... 6 1.5 FurtherResearch ...... 7

2 In-Car Navigation Technologies 9 2.1 Introduction...... 9 2.2 StateoftheArtSystems...... 11 2.3 GNSS and Augmentation Systems ...... 13 2.4 MotionSensorsandCoordinateSystems ...... 19 2.5 InformationProcessing...... 25 2.6 MapInformation ...... 30 2.7 InformationFusion ...... 33 2.8 Conclusions ...... 39

3 Time Synchronization in GPS-aided INS 43 3.1 Introduction...... 43 3.2 MSEoftheStateEstimate ...... 45 3.3 Estimating and Removing the Timing Error ...... 55 3.4 Time Synchronization in GPS-aided INS ...... 60 3.5 ObservabilityAnalysis ...... 67 3.6 Conclusions ...... 71 3.A Extended State Space Model Error Covariance ...... 73 3.B INSErrorModel ...... 74 3.C ObservabilityMatrices ...... 76

viii Contents ix

4 Synchronization by Message Exchanges 77 4.1 Introduction...... 77 4.2 ProblemDescription ...... 79 4.3 TheTwo-WayMessageExchangeModel ...... 81 4.4 Cramér-RaoBound...... 84 4.5 Approximative Maximum Likelihood Estimator ...... 93 4.6 SimulationStudy...... 97 4.7 Conclusions ...... 101 4.A FIMElements...... 103 4.B Polynomial Functions in the CRB Expressions ...... 103 4.C ProofsoftheInequalities ...... 104

5 Zero-Velocity Detection 107 5.1 Introduction...... 107 5.2 ProblemDeﬁnition ...... 108 5.3 SignalandSensorModel...... 110 5.4 GeneralizedLikelihoodRatioTest ...... 112 5.5 PerformanceEvaluation ...... 119 5.6 Conclusions ...... 124

6 CalibrationofaMEMSIMU 127 6.1 Introduction...... 127 6.2 SensorErrorModel...... 129 6.3 CalibrationAlgorithm ...... 132 6.4 Cramér-RaoBound...... 134 6.5 Results...... 136 6.6 Conclusions ...... 139 6.A FIMElements...... 141

Bibliography 143

Chapter 1

Low-Cost Navigation Systems

1.1 Navigation

”Navigation – the science of getting ships, aircraft, or spacecraft from place to place; especially the method of determining position, course, and distance travelled.” [Mer09]. This is the deﬁnition of the word navigation given in [Mer09]. Within this thesis the words navigation, and navigation system, are used with respect to the latter part, (i.e., the method of determining position and course, and the system performing this task). The thesis will investigate four problems encountered in the development of low-cost vehicle and pedestrian navigation systems, and propose possible solutions.

1.2 Problems in Low-Cost Navigation Systems

Today the area of high-cost and high-performance navigation for ground, sea, underwater, or air vehicles is a well developed ﬁeld. The challenge now is to develop high-performance navigation systems using low-cost sensor technology or pedestrian indoor navigation systems. This development involves problems spanning from signal processing of the dirty measurements produced by low-cost sensors via fusion and synchronization of information produced by a large set of diverse sensors, to reducing the size and energy consumption of the systems. To get a picture of the problems encounter going from high to low-cost navigation, the navigation problem may be studied with start from the information classes available and the quality of the sensors employed to extract the information. With reference to Figure 1.1, looking at the navigation problem from an information perspective, there are basically four diﬀerent classes of information sources available: (1) active external infrastructure sources such as various global navigation satellite systems and

1 2 CHAPTER 1. LOW-COST NAVIGATION SYSTEMS

Active External Sources Passive External Sources

Information Fusion

Self Contained Motion Sensors Motion Constraints

Figure 1.1: The four basic classes of navigation information. other radio frequency based navigation systems; (2) passive external infrastructure sources such as land marks or radio-frequency identiﬁcation tags that may be read by onboard sensors; (3) self-contained motion sensors located onboard the navigation platform, such as accelerometers, velocity encoders, and gyroscopes; (4) motion constraints information sources such as road or building maps, and vehicle or human motion models. The designer of a navigation system must choose which of these types of information sources, if not all, to use, and how to combine them to meet the performance requirements. This necessitates a balance between the cost, complexity, and performance of the system. In the design of a high-cost vehicle navigation system the designer may get away by relying on a few high quality sensors extracting information from one or two of the information sources. However, in the design of a low-cost vehicle navigation system, the information quality provided by cheaper sensors, may force the designer to work with a large variety of sensors, employing information from all of the four information classes, to fulﬁll the performance requirements. This means that complexity of the system increases, since:

• The information fusion ﬁlter must be able to manage many diﬀerent information sources with varying characteristics.

• The information from many sources, possibly widely separated within the system must be communicated, synchronized, and spatially related.

• The dirty sensor measurements must be compensated for by using reﬁned error models, calibration schemes and motion models. 1.3. THESIS OUTLINE AND CONTRIBUTES 3

Designing a robust pedestrian navigation system, especially to provide seamless navigation in indoor and harsh radio environments without prein- stalled dedicated infrastructure, is an even more challenging problem. The additional complexity is owing to several facts. In an indoor or in a harsh radio environment the backbone of most of today navigation systems, the global positioning system, works poorly. The complex motion of the human body in comparison to the ridge chassis of a vehicle makes it hard to construct motion models, and spatially relate measurements from sensors located on diﬀerent body parts. Finally, the size and power consumption of the systems must be further reduced. A more in-depth introduction and description of the problems encountered in the development of low-cost navigation systems is presented in Chapter 2.

1.3 Thesis Outline and Contributes

The outline of the thesis is as follows. Chapter 2, presents a survey on in-car navigation and serves as an introduction to the remaining chapters by describing the major navigation information sources, their characteristics, and common methods used to combine them. Chapters 3–6 are then devoted to detailed investigations of four problems related to the development of low-cost vehicle and pedestrian navigation systems. The four problems treated are the time synchronization in a global positioning system aided inertial navigation system, the time synchronization of wireless sensor nodes by two-way message exchanges, the zero-velocity detection in a foot-mounted pedestrian navigation system, and the calibration of inertial measurement unit. A summary of each chapter is given next.

Chapter 2 This chapter presents a survey of the information sources and information fusion technologies used in current in-car navigation systems. The navigation system performance measurements of accuracy, integrity, availability, and continuity of service, are introduced. General principles of operation and, pros and cons of the four commonly used information sources – receivers for radio based positioning using satellites, vehicle motion sensors, vehicle models, and digital map information – are described. Common ﬁl- ters and system architectures to combine the information from the various sources are discussed. The chapter is based upon:

• I. Skog and P. Händel. In-Car Positioning and Navigation Technolo- gies – A Survey. IEEE Trans. on Intelligent Transportation Systems, 10(1):4-21, March 2009. 4 CHAPTER 1. LOW-COST NAVIGATION SYSTEMS

Chapter 3 This chapter presents a study of the effects of a time synchronization error in a loosely coupled global positioning system aided inertial navigation system. It is shown that a time synchronization error increases the mean square error of the estimated navigation solution. Expressions for evaluating the mean square error of the navigation solution, given the vehicle trajectory and the model of the inertial navigation system 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 mean square error and a bias in the estimated forward acceleration. In the second case, a parametrization 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 and the global positioning system 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 acceleration changes. The chapter is based upon:

• I. Skog and P. Händel, Time synchronization errors in GPS-aided inertial navigation systems. IEEE Trans. on Intelligent Transportation Systems, In revision, 2009.

A shorter version is available as:

• I. Skog and P. Händel, Eﬀects of time synchronization errors in GNSS- aided INS. In Proc. of PLANS 2008, Monterey, CA, USA, April 2008.

The material on the observability analysis is excluded from the preceding listed publications, and is under consideration for a future publication.

Chapter 4 This chapter presents a study of the achievable clock synchronization accuracy in a wireless sensor network employing a two-way message exchange model. The Cramér-Rao bound for the estimation of the clock parameters is derived for four different parameterizations (i.e., different nuisance parameters), reflecting different levels of prior knowledge concerning the system parameters. The results on the Cramér-Rao bound are transformed into a lower bound on the mean square error of the clock offset, a figure of 1.3. THESIS OUTLINE AND CONTRIBUTES 5 merit often more relevant, characterizing the system performance. Further, by introducing a set of artificial observations through a linear combination of the observations originally obtained in the two-way message exchange, an approximate maximum likelihood estimator for the clock parameters is proposed. The estimator is shown to be of low complexity and it obeys near-optimal performance, that is, a mean square error in the vicinity of the Cramér-Rao bound. The applicability of the derived results is shown through a simulation study of an offshore engineering scenario, where a remotely operated underwater vehicle is used for operations at the seabed. The position of the vehicle is tracked using a wireless sensor network. The chapter is based upon:

• I. Skog and P. Händel. Synchronization by Two-Way Message Ex- changes: Cramér-Rao Bounds, Approximate Maximum Likelihood, and Oﬀshore Submarine Positioning. IEEE Trans. on Signal Pro- cessing, Accepted for publication with mandatory minor revisions, May 2009.

Chapter 5 This chapter presents a study of the problem of detecting the time epochs when zero-velocity updates can be applied in a foot mounted pedestrian navigation system. Three, in the literature, commonly used detectors are examined: the acceleration moving variance detector, the acceleration magnitude detector, and the angular rate energy detector. It is shown that all detectors can be derived within the same general likelihood ratio test framework – given diﬀerent prior knowledge about the sensor signals. Further, by combining all prior knowledge, a new likelihood ratio test detector is derived. The performance of the detectors is then evaluated using slow (approx. 3 km/h) and fast (approx. 5 km/h), levelled ground, forward- gait data. As a reference system, indicating when the inertial measurement unit is stationary, force sensitive resistors mounted beneath the foot of the user are used. The test results are presented in terms of the detection versus false alarm probability. The results show that the new detector performs marginally better than the angular rate energy detector, which out performs both the acceleration moving variance detector and acceleration magnitude detector. The conclusion is therefore that, under the conditions stated, the signals from the gyroscope assembly hold the most reliable information for zero-velocity detection. The chapter is based upon:

• I. Skog and P. Händel. Zero-Velocity Detection in Pedestrian Naviga- tion Systems – An Algorithm Evaluation. IEEE Trans. on Biomedi- cal Engineering, Submitted, Nov. 2009. 6 CHAPTER 1. LOW-COST NAVIGATION SYSTEMS

Chapter 6 This chapter presents an approach for calibrating a low-cost inertial measurement unit, 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 is equal to the magnitude of applied force and rotational velocity, respectively. This fact, together with a 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 inertial measurement unit into diﬀerent 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. The chapter is based upon: • I. Skog and P. Händel. Calibration of a MEMS Inertial Measurement Unit. In Proc. of IMEKO 2006, Rio de Janeiro, Brazil, September 2006. Note that there is no cross-referencing between the chapters in the thesis; instead, references are made to the preceding listed published material. Further, the mathematical notation employed in the thesis is adapted to the problem under investigation and thus varies between the chapters. The notation employed is deﬁned within each chapter.

1.4 Related Research

Results from three other research projects in the ﬁeld of vehicle and pedestrian navigation, which I have been involved in, have been omitted from the thesis. The ﬁrst of these projects was concerned with the development of a test and demonstrator platform for low-cost global positioning system aided inertial navigation. The results of the project are presented in: • I. Skog and P. Händel. A low-cost GPS aided inertial navigation system for vehicle applications. In Proc. of EUSIPC 2005, Antalaya, Turkey, September 2005. • I. Skog and A. Schumacher and P. Händel. A Versatile PC-Based Platform for Inertial Navigation. In Proc. of NORSIG 2006, Reyk- javik, Iceland, June 2006. The second project investigates existing techniques for positioning of emergency personal. The results of the project are presented in: 1.5. FURTHER RESEARCH 7

• J. Rantakokko, P. Händel, F. Eklöf, B. Boberg, M. Junered, D. Akos, I. Skog, H. Bohlin, F. Neregård, F. Hoﬀmann, D. Andersson, M. Jans- son, and P. Stenumgaard. Positioning of emergency personnel in res- cue operations - possibilities and vulnerabilities with existing techniques and identiﬁcation of needs for future R&D. Technical report TRITA-EE 2007:037, Royal Institute of Technology, 2007, Stock- holm, Sweden.

• J. Rantakokko, J. Rydell, K. Fors, S. Linder, S. L. Wirkander, I. Skog, and P. Händel. Technologies for ﬁrst responder indoor localization. In Precision Indoor Personnel Location and Tracking for Emergency Re- sponders Technology Workshop, Worcester, MA, USA, August 2009.

The third project is on the development, construction, and test of an ultra wide band radio aided inertial navigation system for indoor positioning. Initial results are found in:

• J. O. Nilsson, A. De Angelis, I. Skog, P. Carbone, and P. Händel. Signal processing issues in indoor positioning by ultra wide band radio aided inertial navigation. In Proc. of EUSIPCO 2009, Glasgow, Scotland, September 2009.

• A. De Angelis, J. O. Nilsson, I. Skog, P. Händel, and P. Carbone. Indoor Positioning by Ultra Wide Band Radio Aided Inertial Navi- gation. In Proc. of IMEKO 2009, Lisbon, Portugal, September 2009.

Another research project, not in the field of navigation but in the closely related field of advanced driver assistant systems, in which I have been involved, is the moose early warning system project. The project evaluated the feasibility of using an off-the-shelf consumer electronics infrared camera to construct a system that at an early stage warns the driver when there is a moose on or close to the road. The results of the project are presented in:

• P. Händel, Y. Yao, N. Unkuri, and I. Skog. Far infrared camera platform and experiments for moose early warning systems. JSAE Trans., 40(4):1095–1099, July 2009.

1.5 Further Research

During the research that bred to the results presented in the thesis, several questions and research ideas have emerged. A selection of these is outlined next. 8 CHAPTER 1. LOW-COST NAVIGATION SYSTEMS

Time Synchronization In Chapter 3, a method to estimate and compensate for a constant time synchronization error in the data sampling in a loosely coupled global positioning system aided inertial navigation system is proposed. A natural development of this work is to investigate the possibility of extending the method to work with a tightly coupled system architecture and a time- varying synchronization error. Moreover, it would be interesting to develop a framework for time synchronization in sensor networks consisting of a set of slave nodes that can transmit but not receive data, and one master node responsible for the data fusion. This could possibly be completed by combining the clock skew estimation method in Chapter 4, and the constant time synchronization error (i.e., the initial clock oﬀset) estimation method in Chapter 3.

Zero-Velocity Detection for Pedestrian Navigation In Chapter 5, the detection versus false alarm probability performance of four zero-velocity detectors for pedestrian navigation systems is evaluated. An extension of this work would be to investigate how these performance ﬁgures relates to the accuracy of the of the navigation solution in a pedestrian navigation system employing the diﬀerent detectors.

Inertial Measurement Unit Calibration In Chapter 6, a calibration method that simpliﬁes the inertial sensor calibration by not requiring the inertial measurement unit to be rotated into precisely controlled orientations, is studied. The accuracy of the calibration method is compared to the Cramér-Rao bound for the calibration method where the inertial measurement unit is rotated into precisely controlled orientations. The calibration method has later been reﬁned and further evaluated in [FON08]. To better access the theoretical achievable accuracy of the calibration method a tighter bound could be calculated by treating the unknown orientations as nuisance parameters in the derivation of the Cramér-Rao bound. Chapter 2

In-Car Navigation Technologies

2.1 Introduction

Today a large share of passenger cars is delivered from the factory with a GPS-based in-car navigation system. Owners of used cars can at, a rea- sonable cost, install one of the many third party in-car navigation systems on the market. Indeed, in Western Europe around 14.4 million portable satellite navigation systems were sold during 2007 [GfK08]. 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. Oper- ators at the center can use this information to direct the vehicle feet as effciently 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 [DB06, DB04, LVB00, Row01]. Increasing the accuracy and robustness of navigation systems implies that traffic coordinators could guide their vehicle fleets more effciently based on the traffic flow on different road lanes, etc. Refer to [TSF+98] and [HSH+08] for discussions on robustness enhancement of a bus fleet monitoring system and the use of GPS positioning in bus priority control at traffic lights, respectively. Moreover, further development of intelligent transport system (ITS) applications such as advanced driver assistance systems (ADASs), traffic control, automatic positioning of accidents, electronic fee collection, goods tracking, etc. requires not only navigation systems with higher accuracy but also better

9 10 CHAPTER 2. IN-CAR NAVIGATION TECHNOLOGIES

Information sources

GNSS/RF-based Positioning

Man-machine interface Vehicle motion Sensors Information Vehicle state Fusion Road maps Guidance

Vehicle models Traﬃc situation information ADAS

Camera/Radar/Laser

Figure 2.1: Conceptional description of the available information sources and information ﬂow for an in-car navigation system. The block with dashed lines is generally not part 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).

reliability and integrity [SBBD08], i.e., redundant information sources are needed [YF03]. With reference to Figure 2.1, looking at the in-car navigation problem from an information perspective there are basically four diﬀerent sources of information available: various Global Navigation Satellite Systems (GNSSs) and other RF-based navigation systems, sensors observing vehicle dynamics, road maps and vehicle models. The GNSS receiver and vehicle motion sensors provide observations to estimate the vehicle’s 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 current observation information [JDW03]. Further, a ﬁfth information source is indicated in Figure 2.1 – visual, radar, or laser information. This kind of information is generally not used in current systems but plays a major role in the development of ADASs, etc. Presently one of the major bottlenecks for incorporating this type of information into safety systems is the price/performance ratio of the sensors [KPE+06]. Refer to e.g., [WSM+05, MT06, HYUS09] for details on the incorporation of visual information into vehicle navigation systems and safety application systems. 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 in- 2.2. STATE OF THE ART SYSTEMS 11 formation to meet performance 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 [Hei00, EWP+96, OSW+03]:

• 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 speciﬁed 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 [BL04].

2.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 map-matching [DB06, DB04, OLS07, OLS06]. 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 [HT06, Bob05, GC07, MBH05]. 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 surrounding objects may lead to decreased position accuracy without notification by the GPS receiver, thereby reducing the integrity of the navigation solution [MBH05]. Therefore, to counteract navigation solution degradation in situations with 12 CHAPTER 2. IN-CAR NAVIGATION TECHNOLOGIES poor satellite constellation geometry, shadowing and multipath propagation of satellite signals, advanced in-car navigation systems commonly use complimentary navigation methods, relying upon information from 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 [OLS07, OLS06]. Pioneer is another manufacture that has presented advanced consumer in-car navigation systems [SANN05, NAY02]. Once the vehicle’s location has been matched to a location on the map, the navigation system can access related attributes in the map database. Depending on the attributes available, the information can be used to aid the ADAS to perform adaptive cruise controlling, adaptive light controlling, preview curve information, intelligent speed adaptation (speed limit alerting), etc. [Row01]. Assuming sufficient map quality, the results of the map-matching can be fed back into the system to correct sensor errors and enhance system accuracy. Indeed, in e.g. [GGB+02] a vehicle navigation system solely based on wheel speed sensors and a probabilistic map-matching approach is described. The constraints introduced by the map are used as virtual observations to bound navigation and sensor errors. Adding more sensors to the GNSS receiver is not merely a question of giving the navigation system higher accuracy, better integrity or providing a more continuous navigation solution. It also increases the update rate of the system and provides more information such as acceleration, roll and pitch, depending on which types of sensors are used. The typical update rate for a GNSS receiver is less than 20 times per second [FB98], 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 is generally 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 the excellent paper [AP99]. The authors evaluate how 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 2.3. GNSS AND AUGMENTATION SYSTEMS 13 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. Summarizing the section, the accuracy of the in-car navigation system is highly dependent on available low-cost GPS receiver solutions as well as the quality of the map used in the map-matching.

2.3 Global Navigation Satellite Systems and Augmentation Systems

Currently there are two global navigation satellite systems available: the Russian GLONASS1 and the American Global Positioning System (GPS) [TW04]. Further, the European satellite navigation system Galileo is under construction and is scheduled to be fully operational by 2013. 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 diﬀerent receiver structure. Further, the diﬀerence in the orbit plans of the systems’ satellite constellations provides good coverage across regions. The GPS system provides good coverage at mid latitudes, whereas the GLONASS system gives better coverage at higher latitudes [TW04].

2.3.1 Idea of Operation and Major Error Sources The basic operational idea of the GNSS is that receivers measure the time- of-arrival 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 trilateration. This is illustrated in Figure 2.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 trilateration [EWP+96, MBH05]. Errors in range estimates can be grouped together, depending on their spatial correlation, as common mode and non-common mode errors [FB98, GWA07]. Common mode errors are highly correlated between GNSS receivers in a local area (50-200 km) and are due to ionospheric radio signal

1Globalnaya Navigatsionnaya Sputnikovaya Sistema. 14 CHAPTER 2. IN-CAR NAVIGATION TECHNOLOGIES

Satellit Satellit Pseudo range

Uncertainty region Receiver

Satellit True range

Figure 2.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 reﬂecting the possible positions of the receiver. propagation delays, satellite clock and ephemeris2 errors, and tropospheric radio signal propagation delays. Non-common 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 2.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 [FB98]. 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 [FB98]:

2The ephemeris errors are due to the deviations in the satellite orbits, resulting in a diﬀerence between the actual and theoretically calculated satellite locations. 2.3. GNSS AND AUGMENTATION SYSTEMS 15

Table 2.1: Standard deviations of errors in the range measurements in a single-frequency GPS receiver [FB98]. Error Source Standard deviation [m] Common mode Ionospheric 7.0 Clock and ephemeris 3.6 Tropospheric 0.7 Non-common mode Multi-path 0.1–3.0 Receiver noise 0.1–0.7 Total (UERE) 7.9–8.5 CEP with a horizontal dilution of pre- 6.6–7.1 cision, HDOP=1.2

CEP = √ln 2 HDOP UERE. (2.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, reﬂecting the geometry of the satellite constellation. Refer to [Mil08, OJCL06, ORL02] for details on typical dilution of precision ﬁgures for the GPS and Galileo system. It is worth noting that (2.1) is based on several assumptions such as uncorrelated range estimates and circular Gaussian-distributed position estimation errors, which more or less hold true [Dig07]. Therefore (2.1) should only be used as a rough indication of position error.

2.3.2 Augmentation Systems 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 trans- mits 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 [LBZ05]. 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, 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 16 CHAPTER 2. IN-CAR NAVIGATION TECHNOLOGIES correction data for their specific location [EWP+96, PCSTR06]. 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 Euro- pean 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 [PCSTR06, Dix05, GG07]. 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 [GG07], an illustrative example of the enhancement of the HDOP for a GPS receiver in an alpine canyon environment using EGNOS data is given. All SBASs are designed to be interoperable. The geostationary satellites of the augmentation systems transmit correction data using the L1 (1575.42 MHz) frequency of the GPS system, and therefore only the software for GPS receivers has to be modified to receive correction data. Many low- cost GPS receivers are able to use correction data from the SBASs [Dix05]. 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 [CL04]. A more thorough description on how the SBASs operate and correction data is calculated can be found in [EWP+96]. 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 [Hei00, LBZ05, Dix05, EM99, MBP99]. However, the required receiver units are currently far too costly for use in commercial in-car navigation systems. In [Dix05], a discussion of the performance and cost of single- and two-frequency GPS receivers and various augment systems is presented. In [MBH05], 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 [HW06]. Still, there will be areas such as tunnels where reliable GNSS receiver navigation solu- 2.3. GNSS AND AUGMENTATION SYSTEMS 17 tions 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 [GWA07, LWR+02]. How- ever, 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.

2.3.3 Integrity Monitoring The inherent weakness of all radio signal-based navigation methods is their reliance on information that may become erroneous, disturbed, or blocked while transferred from the external sources to the receiver. More precise, due to malfunctions in the software or hardware of the external sources, the transmitted information may become erroneous without any notification by the receiver. In, e.g. [BO07, OSW+03] possible GPS failure modes are listed. Further, intentionally or not, other electronic devices may ra- diate RF signals in the frequency spectrum used for the GNSS signals, causing interferences. Moreover, the environment surrounding the receiver may cause multi-path propagation, distortion and attenuation of the radio signal, thus complicating proper signal acquisition and distorting the information accessible by the receiver. Therefore, to create a reliable and robust navigation system, the system should incorporate integrity monitoring as well as be combined with information from redundant sources. There are three key components in integrity monitoring: fault detection, fault isolation and removable of faulty measurements sources from the navigation solution [HLW04]. The availability of the navigation system to perform these tasks is directly related to its integrity and continuity of service [Lee07]. Fault detection, i.e., checking for outliers and other anomalies in the information from the sensors or subsystems, is used to timely alert if the calculated navigation solution is unreliable and may exceed predefined protection levels. Hence, it indicates if the specified minimum accuracy in the navigation solution, required for subsequently applications, cannot be maintained. The ability to isolate and remove the faulty information before contaminating the navigation solution means that the system can continue working using the remaining information sources. Thus, the continuity of service of the navigation system is enhanced. A necessity to be able to perform integrity monitoring is the availability of redundant information in the system [Stu89]. For a civilian GNSS receiver this redundant information can be obtained from an augmentation system such the EGNOS or WAAS system [GWA07, EWP+96], by observing more satellite signals than the minimum number necessary to compute a position estimate [Lee07, HLW04, HW06], or by a complimentary navigation system, such as dead-reckoning or inertial naviga- 18 CHAPTER 2. IN-CAR NAVIGATION TECHNOLOGIES tion [BOF07a, BOF07b, BO07, MZIMGS07]. The inherent weakness of integrity monitoring via an augmentation system, such as an SBAS, is that its signals are also vulnerable to jamming, in- terference and blockage [BOF07a]. Therefore, integrity critical applications or systems with limited access to the signals from the augmentation system, such as electronic fee collection of land-vehicles moving in dense urban arias, may necessitate a navigation platform that self-evaluates the integrity of the navigation solution. For GNSS receivers, self-evaluated integrity monitoring through observation of more satellite signals than necessary to compute a position estimate is referred to as receiver autonomous integrity monitoring (RAIM). There are several different RAIM methods, e.g., the range comparison method, the least-square residual method, and the parity method [GWA07]. They all require a minimum of five satellites signals to detect a failure in one of the signals and signal measurements from a minimum of six satellites to isolate the failure [HLW04, Stu89, MZIMGS07]. The reliability of the detection and identification of the different RAIM algorithms is highly dependent on the amount of redundant information available and the geometry of the satellite constellation. Therefore, with the renewal of the GLONASS system and the upcoming Galileo system RAIM algorithms using combinations of these systems have been developed, see [Lee07, HLW04, HW06]. In the simulation study of [HW06], the results theoretically indicate that for any point on the globe a RAIM algorithm utilizing the signals from the GPS, GLONASS and Galileo systems together can detect an outlier of 20 m with a 80% probability at a 0.5% significance level and then isolate the fault with a 90% probability. The corresponding outlier value for the GPS alone is approximately 435 m. Despite improvements in availability and integrity monitoring using the combination of GNSSs, the likelihood of having access to sufficient satellites signals to produce a navigation solution and perform RAIM is low in dense urban environments [MZIMGS07]. Therefore, it is necessary to use additional navigation means and aides, such as dead-reckoning, inertial navigation or maps-matching, to produce a reliable and robust in-car navigation system. The information from these systems may then be used to access the integrity of the GNSS-receiver navigation solution. However, since the sensors, etc. of the other system components are also subjected to possible failures – see e.g. [BO07] and [QON06] for possible failure modes in INS and map-matching, respectively – integrity monitoring should include the full system. Integrity monitoring on a full system level is further discussed in Section 2.7 on information fusion and filter structures. 2.4. MOTION SENSORS AND COORDINATE SYSTEMS 19

Table 2.2: Sensors commonly used as a complement to GNSS-receivers for enhancement of in-car navigation systems. Sensor Measurement Steering encoder Front wheel direction Odometer Traveled distance Velocity encoders Wheel velocities (indirectly. heading) Electronic compass Heading relative magnetic north Accelerometer Acceleration Gyroscope (rate-gyro) Angular rotation (velocity)

2.4 Motion Sensors and Coordinate Systems

In this section, a variety of useful sensors for vehicle positioning and navigation is considered. To fuse the available information from the vehicle based sensors to reliable estimates on position, velocity and attitude, a review of the most commonly used coordinate systems is required, which is another topic of this section. Information processing, e.g., dead reckoning and inertial navigation, is then discussed in Section 2.5.

2.4.1 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 system. In Table 2.2, the most commonly used sensors, together with the information they provide, are summarized. 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 [AP99]. 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 by multiplying by 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, or if separate encoders are used for the wheels on one side of the vehicle, an 20 CHAPTER 2. IN-CAR NAVIGATION TECHNOLOGIES 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 [CGP02, CGP04, Hay05] for details. Though this information is provided at no additional cost in terms of extra sensors, the generally low resolution of the ABS sensors can seriously affect the reliability of the calculated heading estimate. Therefore, the usage of additional wheel encoders may be necessary to form a reliable estimate of the heading changes from the information about the wheel speeds. These notions of how to estimate traveled distance, velocity and heading of the vehicle are all based on the assumption that wheel revolutions can be translated into linear displacements relative to the ground. However, there are several sources of inaccuracy in the translation of wheel encoder readings to traveled distance, velocity and heading change of the vehicle. They are [AP99, CGP04, BF96]:

• 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 diﬀerent wheels • Uncertainties in eﬃcient wheelbase (track width) • 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 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 [AP99]. 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. 2.4. MOTION SENSORS AND COORDINATE SYSTEMS 21

Generally, the compass is constructed around three magneto-resistive or flux-gate magnetometers, together with pitch and roll sensors [TW04]. 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 [TW04] and [Pet86], details about compasses based on flux-gate magnetometers can be found. A review of magnetic sensors is found in [Len90]. Power lines and 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 [AP99]. However, there are other applications of magnetic sensors in in-car navigation systems. See [YF03], where magnetic sensors are used to detect the vehicle’s location with the help of 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 produces an output proportional to the specific force exerted on the instrument,3 projected onto the coordinate frame mechanized by the accelerometer [Bri71]. Information about an object’s orientation and rotation may be obtained using a gyroscope (rate-gyro), which measures the angular rotation (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 [TW04], 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 [BS01]. 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 micro-electromechanical-system (MEMS) sensor technology it has become possible to construct inertial sensors meeting the cost and size demands needed for low-cost commercial electronics, such as vehi-

3According to the principle of equivalence, it is impossible to instantaneously distin- guish between gravitational and inertial forces. Hence, the output of an accelerometer contains both forces, referred to as the specific force. 22 CHAPTER 2. IN-CAR NAVIGATION TECHNOLOGIES cle 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 is offered in [BRB+06]. In chapter 7 of [TW04], an introduction to MEMS inertial sensor technology can be found. In [ESN07], 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. However there are several error sources associated with inertial sensors which must be considered. Some of the most significant inertial sensor errors can be categorized as [TW04, GWA07]:

• Biases • Scale factors • Nonlinearities • 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 • Temperature varying terms

The ﬁxed terms, and to a large extent the temperature varying terms, can be estimated and compensated by calibration of the sensors; refer to [Cha97, Rog03, ASNES06, SH06a, FON08, SAG+07] for several calibration approaches. Turn-on to turn-on terms diﬀer 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. Besides the error components discussed above, there are also error components due to the inevitable imprecision in the mounting of the sensors as well as motion dependent error components, which maybe necessary to consider in 2.4. MOTION SENSORS AND COORDINATE SYSTEMS 23 the choice of sensors and information fusion algorithms. Refer to [Far07] for details on inertial instrumentation error characterization. In order to measure the vehicle’s dynamics in both long- and cross- track directions, 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 fre- quently a combination of accelerometers and gyroscopes is used. See [TP05] and [CLD94] 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 rotations around these axes.

2.4.2 Coordinate Systems Before continuing with a discussion on how the information supplied by vehicle mounted sensors is processed into an estimate of the vehicle’s position, velocity, and attitude, or is exchanged with the interfacing information sources in the system, it is essential to introduce a few coordinate systems: the vehicle coordinate system, the earth centered inertial (ECI) coordinate system, the earth centered earth fixed (ECEF) coordinate system and the geographic coordinate system. The vehicle coordinate system, sometimes referred to as the body coordinates, is the coordinate system associated with the vehicle. Commonly, but not necessarily, it has its origin at the center of gravity of the vehicle, and the coordinate axes are aligned with the forward, sideways (to the right) and down directions associated with the vehicle. The information provided by vehicle mounted sensors and the motion and dynamic constraints imposed by the vehicle model are generally expressed with reference to this coordinate system. The earth centered inertial coordinate system is the favored inertial coordinate system for navigation in a near-earth environment [GWA07]. The origin of the ECI coordinate system is located at the center of gravity of the Earth (viz. a geocentric coordinate system). Its z-axis is aligned with the spin axis of the Earth, the x-axis points towards the vernal equinox, and the y-axis completes the right hand orthogonal coordinate system. This is illustrated in Figure 2.3. The accelerations and angular velocities observed by the inertial sensors are measured relative to this coordinate system. Closely related to the ECI coordinate system is the geocentric earth centered earth fixed coordinate system, which also has its origin at the center of gravity of the Earth but rotates with the Earth. Its first coordinate axis (x- axis) lies in the intersection between the primer meridian and the equator 24 CHAPTER 2. IN-CAR NAVIGATION TECHNOLOGIES

zi ze ≡ Earth rotation North

East

Geographic frame Equator Down

xi Latitude

Longitude Vernal equinox Focus

Reference ye xe meridian

Figure 2.3: Illustration of the earth centered earth ﬁxed frame (axes denoted by the superscript e), the geographic frame (axes denoted North, East and Down), and the earth centered inertial coordinate frame (axes denoted by the superscript i).

plane, the z-axis is parallel to the spin axis of the Earth, and the y-axis completes the right hand orthogonal coordinate system. Refer to Figure 2.3 for an illustration. Nearly related to the geocentric ECEF coordinate system is the geodetic coordinate system defined by the World Geodetic System (WGS) 84 datum, commonly referred to as the geodetic ECEF coordinate system. Details on the coordinate transformation between these two systems are found in [FB98]. A geodetic coordinate system representation is based on an approximation of the Earth geoid (globally or locally) by an ellipsoid that rotates around its minor axis [Zha97]. A location in the coordinate system is described in terms of the longitude and latitude angles measured with respect to the equatorial and meridional plan associated with the reference ellipsoid. The parameters of the reference ellipsoid, such 2.5. INFORMATION PROCESSING 25 as shape, size, orientation, etc., define the datum of the coordinate system. Refer to [Zha97, DR98] for details on reference coordinate systems and different datums. The coordinate system defined by WGS 84 is the coordinate system used in the GPS. The geographic coordinate system is a local coordinate frame whose origin is the projection of the vehicle coordinate system origin onto the Earth’s geoid. The x-axis points towards true north, the y-axis to the east and z-axis completes the right hand orthogonal coordinate system pointing towards the interior of the Earth perpendicular to the reference ellipsoid [Bri71]. The coordinate system is illustrated in Figure 2.3. (Note that the z-axis does not point towards the center of Earth but rather along the ellipsoid normally towards one of its foci.) The geographic coordinate system is generally used as a reference when expressing the velocity components of the vehicle’s motion and the attitude of the vehicle platform. The vehicle attitude is commonly described by the three Euler angles – roll, pitch and yaw – relating the vehicle and geographic coordinate systems to each other. Detailed descriptions of the various coordinate systems used in navigation, together with common coordinate transformations, are found in the standard textbooks on inertial navigation [Bri71, FB98, TW04, GWA07, Cha97, Rog03].

2.5 Information Processing

Processing of information from vehicle based sensors to estimate position, velocity and attitude is discussed in this section.

2.5.1 Dead Reckoning Velocity encoders, accelerometers and gyroscopes all provide information on the ﬁrst or second order derivative of the position and attitude of the vehicle. Further, the odometer gives information on the traveled distance of the navigation system. Hence, except for the magnetometer, all the measurements of the sensors in Table 2.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 that the initial state of the vehicle is known, and for which measurement errors will accumulate with time or, for the odometer, with the traveled distance. Moreover, the information provided by the vehicle mounted sensor is, except from possible ﬁxed rotations, represented in the vehicle coordinate system. Therefore, before the sensor measurements are processed into a position, velocity and attitude estimate, they must be transformed into a coordinate system where they are more easily interpreted, preferably the 26 CHAPTER 2. IN-CAR NAVIGATION TECHNOLOGIES

ECEF or the geographic coordinate system. Moreover, if the sensor measurements are to be used in combination with information provided by other information sources, they must be expressed in a common coordinate system. The process of transforming the measurements from the vehicle mounted sensor into an estimate of the vehicles position and attitude is generally referred to as DR, or if only involving inertial sensors inertial navigation. The process of DR (and inertial navigation) can brieﬂy be described as:

1. The gyroscope, compass or diﬀerences in wheel speed measurements are used to determine the attitude (3-dim) or heading (2-dim) of the vehicle.

2. Attitude (or heading) information is then used to project the in- vehicle coordinates measured acceleration, velocity or traveled distance onto the coordinate axes of the preferred navigation coordinates system, e.g., the ECEF or geographic coordinate frame.

3. Traveled distance, velocity or acceleration is then integrated over time to obtain position and velocity estimates in the navigation coordinate frame.

Refer to [OLS07, OLS06, CGP02, CGP04] for details on how dead reckoning is done in vehicle navigation systems.

2.5.2 Inertial Navigation

In Figure 2.4, a block diagram of a strap-down4 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, preferably the ECI coordinate system. The angular rotations (rates) observed by the gyroscopes are used to track the relation between vehicle coordinate system and the navigation coordinate frame of choice, commonly the ECEF or geographic coordinate frame. This information is then used to transform the speciﬁc force observed in vehicle coordinates into the navigation frame, where the gravity acceleration is subtracted from the observed speciﬁc force. What remains are the accelerations in the navigation coordinates. To obtain the position of the vehicle, the accelerations are integrated twice with respect to time; refer to [Bri71, FB98, TW04, GWA07, Cha97, Rog03, Sav98a, Sav98b, KGG+83] 2.5. INFORMATION PROCESSING 27

INS Navigation equations Gravity model

Coordinate + Accelero- + − dt dt meters rotation Position R R IMU Velocity Gyro- Attitude scopes determination Attitude Sensor error compensation

Figure 2.4: Conceptional sketch of a strap-down INS. The dash arrows indicate possible points for insertion of calibration (aiding) data. for a thorough treatment of the subject of inertial navigation. In [Std01, CATB04], details on inertial sensors and systems terminology can be found. The integrative nature of the navigation calculations in DR and inertial navigation systems gives the systems a low-pass filter characteristic that suppresses high-frequency 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 [ESN07, TP05, SNDW99, DSNDW01]. The detrimental effect of 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 force 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

4The term strap-down refers to the fact 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 [GWA07]. 28 CHAPTER 2. IN-CAR NAVIGATION TECHNOLOGIES

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.

Contrary to these properties, the GNSS and other radio-based navigation systems give position and velocity estimates with a bounded error but at a 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 signiﬁcantly violate the assumption of a planar environment, accuracy is drastically reduced [DSNDW01]. An INS constructed around a full-six-degree-of-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. In combination with decreasing cost, power consumption and size of the MEMS inertial sensors, this makes vehicle navigation systems incorporating MEMS IMUs at- tractive. 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 [BL04, GC07, ESN07, NNGES07]. This is also illustrated in Figure 2.5 (legend NOC), 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 that measurement noises, turn-on to turn-on and time varying biases reﬂect current ultra low-cost MEMS inertial sensors were added.

2.5.3 Vehicle Models and Motions Under ideal conditions, a vehicle moving in a planar environment experi- ences 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 [GC07, ESN07, DSNDW01], 2.5. INFORMATION PROCESSING 29

RMS horizontal position error

10 NOC NHC 8 NHC+VA ] m

[ 6 Error 4

0 300 305 310 315 320 325 330 Time [s] Figure 2.5: Empirical root mean square (RMS) horizontal position error growth during a 30-second satellite signal blockage in a low-cost GPS-aided INS. NOC - No constraints, NHC - Non-holonomic constraints, NHC+VA - Non-holonomic constraints and velocity aiding.

this type of non-holonomic constraint has been applied to the navigation solution of the vehicle-mounted GNSS-aided INSs. The results show a significant reduction in position error growth during GNSS outages and increased attitude accuracy when imposing non-holonomic constraints on the navigation solution. In Figure 2.5 (legend NHC), 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 (legend NHC+VA), error growth during the outage is negligible. The non-holonomic constraints were in the simulation imposed by introducing pseudo observations for the sideways and downward velocities of the vehicle. From an estimation-theoretical perspective, sensors and vehicle-model information play an equivalent role in the estimation of the vehicle state [JDW03]. If there were a perfect vehicle model, such that the vehicle state could be perfectly predicted from control inputs, sensor information would 30 CHAPTER 2. IN-CAR NAVIGATION TECHNOLOGIES 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 [JDW03, HT06, ESN07, DSNDW01, BRG06, MZIMGS07, JDW95, RS07]. In [JDW03], 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 [MZIMGS07]. More advanced models require knowledge about several parameters such as vehicle type, tires, and environmental specifics [DSNDW01]. 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 [MZIMGS07].

2.6 Map Information

Position information in terms of pure coordinates is often diﬃcult to interpret for a driver. To assist the driver in relating the position information to a physical location, the in-car navigation system commonly displays the vehicle’s position on a map. Moreover, 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 [QON06].

2.6.1 Digital Maps and Spatial Road Work Representation The digital maps used in in-car navigation systems diﬀer quite substantially from ”classical” printed or digitalized maps in the way they are represented. These maps can be seen as an image built up of a set of pixels, which is easy for the human to interpret. For a computer trying to match the vehicle location on the street network, and to compute a recommended path, the image contains little information. The digital maps used in- 2.6. MAP INFORMATION 31

Arc candidates region

Node (intersection) Position Shape point estimate

Node (dead-end)

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

car navigation systems and other ITS applications are therefore built up as databases of topological (connectivity properties of the features in the map) and metrical (coordinates) information, together with attributes such as road class, street name, expected driving speed, turn restrictions, etc. This makes the digital maps easier and faster to interpret by a computer. Refer to [Whi93, Zha97, DR98] for an in-depth description of digital maps in in-car navigation systems and other ITS applications. In [Row01], a review of map attributes necessary to the development of new ADAS applications is found. Simplified, the road network is generally represented by a planar model on digital maps, where the street system is represented by a set of arcs (i.e., curves in R2) [BK96, QOZN03, Zha97, DR98]. Each arc represents the centerline of a road segment in the network and is commonly assumed to be piece-wise linear (another means of representing the arcs is with splines), such that it can be described by a finite set of points (see Figure 2.6). 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. 32 CHAPTER 2. IN-CAR NAVIGATION TECHNOLOGIES

2.6.2 Map-Matching

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 as well as the correlation between the trajectory history of the vehicle and the candidate paths in the map. Finally, the vehicle location on the most likely road segment is determined. The geometrical information includes measures such as the closeness between the position estimate and nearest road on 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 [BK96] for a discussion and description of common measures such as point-to-point, point-to-curve and curve-to- curve matching to extract geometrical information. The topological information criterion determines the connectivity of the candidate roads (arcs), e.g., the vehicle cannot suddenly move from one road segment to another if there is no intersection point 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 [OLS07, ENB05, ENB07, QOZN03, KK01, BJD07, Zha97, ZWW07] for various weighting and combining approaches such as belief theory, fuzzy network and state machines. In [QON07], a survey of state-of-the art map- matching algorithms is found, together with ideas on further research directions. 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 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 [WSM+05]. That is, the heading, length, etc. of the road segment identified by the map- matching algorithm can be compared to the navigation solution and used to calibrate the sensors in the system [Zha97]. This type of bidirectional map-matching is found in [SC97, ENB05, ENB07, GGB+02]. In [LTK05], a scheme for map-matching is examined, where the pseudo ranges, and not the position estimates, from the GNSS receiver are used together with a terrain map as the input for the mapping algorithm. The result is not only a higher accuracy of the position estimates but also the possibility to obtain a navigation solution when less than four satellites are available. A series of decisions, such as map scale, map projection and datum, are involved in the creation of a map and influence its quality and accuracy. 2.7. INFORMATION FUSION 33

Further, the digitalization and identification process in the construction of the spatial road network in the digital map causes additional errors. These errors can be categorized as topological or geometrical, where topological errors originate from the fact that features such as roundabouts, junctions, medians, curves, etc. of the real world have been omitted, missed or simplified in the creation of the digital map [QON06, QON07]. Geometrical errors arise from displacements of map features, such as road centerlines, junctions, etc. from their actual location. The cumulative effect of these errors influences the accuracy of the map and causes an inherent uncertainty level in the map-matching process. To establish a high level of confidence (integrity) in the system, it is essential to constantly quantify reliability and look for anomalies in the result of the map-matching. In [QON06], an empirical approach to evaluate the integrity of the map-matching is therefore developed. In [QON07, CLYC05, ZWW03], the effects of sensor errors and map quality on the map-matching are analyzed. Refer to [Gru08] for a few critical comments regarding the integrity of map-matching in road-use charging systems.

2.7 Information Fusion

The objective of information fusion is to obtain more information than is present in any individual information source by combining information from diﬀerent sources [YLZ07]. In practice, this means that by utilizing the complimentary properties of the diﬀerent information sources, the information fusion tries to reduce ambiguities in the measured information and thereby expand the spatial and temporal coverage in which the system works and enhance the reliability of the system.

2.7.1 Filter Structures The key component in information fusion is in the concept of navigation, to produce estimates of the vehicle’s position, velocity, etc. from the sensor measurements [YLZ07]. The state estimation can be realized basically in two ways, the centralized filtering mode and the decentralized filtering mode [Jek00], where the choice of mode is related to the system requirements regarding estimation accuracy, computational complexity, possibility of fault detection and fault removal. In the centralized filter mode, the raw measurements from all information sources are processed and combined in one filter to produce a navigation solution. The centralized filtering structure is illustrated in Figure 2.7. The centralized filtering mode has the benefit of minimal information loss, so that all the information is directly available to the filter, which if properly designed and assigned with correct prior information theoretically should produce an optimal state estimate [SD04]. However, the computational 34 CHAPTER 2. IN-CAR NAVIGATION TECHNOLOGIES

Sensor/Information source #1

Sensor/Information source #2 Navigation Filter solution

Sensor/Information source #N

Figure 2.7: Centralized (direct) ﬁltering.

complexity of the centralized filter mode, resulting from the ability of the filter to handle all the types of raw data in the system, is often unfeasible. Also, there is the issue of robustness against spurious information from any of the information sources and how to perform fault detection and fault isolation [SD04, GKA93]. Information fusion using decentralized filtering is characterized by its sequential data processing, where the information from the individual subsystems first is filtered through a set of local filters working in parallel, producing a set of (partial) navigation solutions which subsequently on a periodical basis are blended, with various degrees of optimality, by the master filter [Jek00]. See Figure 2.8 for an illustration. The end result is that the computational load in general can be significantly reduced, but at the cost of reduced estimation accuracy [GKA93]. Theoretically, as long as the correct statistics of the estimation error are propagated from the local filters to the master filter, the decentralized and centralized filter architecture should yield the same performance. However, error statistics are often more rigorously modeled and propagated in one filter; therefore the centralized filter structure is often considered to offer better accuracy. One of the pros of decentralized filtering is the simplified possibility of fault detection and fault isolation and exclusion by comparing the (partial) navigation solutions of the local filters and rejecting those found erroneous. Regardless of whether a centralized or decentralized filter structure is used, the designer of the state estimator(s) is faced with a two-fold process: process and measurement dynamics modeling and the choice of filtering algorithm. The modeling part is concerned with the development of a 2.7. INFORMATION FUSION 35

Sensor/Information source #1 Local ﬁlter

Sensor/Information source #2 Local ﬁlter Navigation Master ﬁlter solution Raw data

Sensor/Information source #N Local ﬁlter Partial navigation solutions

Figure 2.8: Decentralized filtering. model that consistently and in a framework suitable for the state estimator (filter) describes the process and measurement dynamics of the navigation system, together with a proper description of the process and measurement noise statistics. The model must be complete enough to give an adequate description of the system and at the same time be sufficiently simple for the filtering algorithm to become computationally feasible [GS69]. The choice of filter algorithm is then a balance between computational complexity, robustness against modeling errors and accuracy of the algorithm. Since nonlinear process and measurement models generally are used to describe information observations and vehicle dynamics, the proceeding discussion on filter algorithms will focus on nonlinear filtering methods.

2.7.2 Filter Algorithms The most widely used nonlinear filtering approach, due to its simplicity, is the extended Kalman filter (EKF) in its various varieties. The idea behind the 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 [KSH99]. For non-Gaussian distributed noise sources, the Kalman filter provides the linear MMSE solution to the filtering problem. Unfortunately, the 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 na- 36 CHAPTER 2. IN-CAR NAVIGATION TECHNOLOGIES ture 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 [MWJ04, Dau05]. The inherent weakness of these nonlinear filtering approaches is the curse of dimensionality. That is, the computational complexity of the filter usually grows exponentially with the dimension of the state vector being estimated [Dau05]. 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 computational complexity of the system [SGN05, KSG05]. A short introduction to nonlinear filtering and the advantages and disadvantages of various algorithms are given in [Dau05]. Originating from the field of nonlinear systems and control theory, the idea of constructing a filter with a nonlinear observer is sometimes used to blend the information from different subsystems and sensors. Refer to [BMR06, ZS05, YLP04] for details. Compared to EKF algorithms for which there is no guaranty of convergence, the framework in which the nonlinear observer is derived often provides strong results on the region of convergence. All filters discussed so far rely on an adequate system model together with proper prior knowledge of the noise statistics, which can be difficult and time consuming to find. If there is a lack of accurate noise statistics, adaptive filters which try to optimize the filter parameters reflecting the noise properties are sometimes used; refer to [MS99, HLES07, GW07] for details. Another approach, avoiding the need for the designer to specify a system model at all, is the usage of a neural network to fuse the information in the system. A neural network consists of several layers of interconnected nodes (neurons). Through a learning process where layers and intercon- nections are rearranged, the neural network is trained to develop an adequate system model, later used to fuse the information from the different sensors/subsystems [Zha97]. Refer to [WG04, SN06, SNOES05, NOES04, HC08, STTN07, SN07, KEE07, PION08, CH08] for more information. In Table 2.3, a list of filter algorithms, together with comments on estimation accuracy, computational complexity and reference to different papers using the filters in navigation and tracking contexts, is presented. To quantify the efficiency and robustness of the chosen process model and filter algorithm two major analytical tools are available, the Posterior Cramér Rao Bound (PCRB) for the filtering problem and a sensitivity analysis. The PCRB provides a lower bound on the mean square error 2.7. INFORMATION FUSION 37 for the filtering problem, against which the filter algorithms are evaluated. Refer to [TMN98, Ker89, Ber99] for more details. However, the mean square error lower bound provided by the PCRB is only valid when the input data to the filter is generated according to the system model used in the PCRB. To quantify the effect of the choice of system model as well as the filtering algorithm, a sensitivity analysis may be performed. Refer to [GS69, BS71, GS68, JDW03, AP99] for details on sensitivity analyzes of Kalman filter and Extended Kalman filter algorithms.

2.7.3 Information Fusion in Systems Including a GNSS Receiver Without doubt the GNSS receiver is and will remain a major component in most vehicle navigation systems, often in combination with various vehicle dynamic sensors. The information fusion between GNSS receivers and other vehicle dynamic sensors, especially inertial sensors, has therefore received a considerable amount of interest and deserves special attention. Depending on information flows and at what level the information is exchanged and blended between the GNSS receiver and other components in the integrated system, the system architectures are commonly referred to as loosely coupled, tightly coupled, ultra tightly coupled or deeply integrated. However, no distinct bounds between the different systems exist and the classification of integration architectures in the literature is not consistent [BO07]. In a loosely coupled system architecture, the information exchange between the GNSS receiver and other system components is unidirectional and the navigation solution (position, velocity, etc.) of the two sub systems is blended. Generally, the information provided by the GNSS receiver is used to aid the other system components, even though the reverse is also possible. This system architecture benefits from its simplicity but has some inherent drawbacks. Due to the filter inside the GNSS receiver, the position and velocity estimates from the GNSS receiver will be correlated in space and time and with a structure most likely unknown to the information fusion algorithm designer [FB98]. Thus, the (correct) statistics on the estimation error are not available for the master fusion filter, leading to suboptimal performance. Further, no information is gained from the GNSS receiver in situations when there are not enough satellite signals for the GNSS receiver to produce a navigation solution on its own. In a tightly coupled system architecture, the information flow between the GNSS receiver and the other system components is still unidirectional, but instead of using the navigation solution from the GNSS receiver as input for the fusion algorithm, basic GNSS observations (pseudo range, carrier phase or Doppler shift estimates) are used. This has several benefits. The basic GNSS observations are not as correlated as the position and velocity 38 CHAPTER 2. IN-CAR NAVIGATION TECHNOLOGIES solutions calculated in the GNSS receiver, and with better known statistics, leading to higher accuracy of the integrated system [AAR03]. Further, by accessing the signals from individual satellites, information is gained from the GNSS receiver even in situations when a navigation solution solely based on GNSS observations is not possible. The integrity and continuity of service of the system also benefit from the access to individual satellite signals, since fault detection at the signal level means that the individual satellite signals can be rejected instead of the full navigation solution of the GNSS receiver [BO07]. This is especially important for systems using low cost vehicle sensors, which may work alone only during a shorter duration of time without the aid of the GNSS receiver. A thorough treatment of integrity with respect to system architectures in GNSS-aided INSs is found in [BO07, BOF07a, BOF07b]. By allowing a bidirectional information flow, so that the navigation solution of the fusion filter is fed back to aid the GNSS receiver’s signal acquisition, a type of ultra tightly coupled system architecture is obtained, also referred to as a deeply integrated system [BW05, AAR03]. The information feedback to the GNSS receiver is used to predict the Doppler shift in the satellite signals due to the vehicle dynamics, so that the bandwidth of the carrier tracking loops can be reduced. The benefits of this are increased accuracy of the pseudo range, carrier phase and Doppler estimate of the GNSS receiver and the possibility of signal acquisition at lower signal to noise ratios. More refined systems can at the cost of even higher computational complexity instead of the basic GNSS observations use the in phase and quadrate phase component of the received GNSS signal as input to the fusion filter. Refer to [AAR03, BW05, SGVG08, GERE+05] for details on ultra-tightly coupled systems.

2.7.4 Sensor Pre-Filtering As should be clear from previous discussions, the quality of the information provided by the sensors in the navigation system has a direct influence on the overall system’s performance. Therefore, vehicle motion sensor pre- filtering/de-noising using the efficient bandwidth of the vehicle’s motion dynamics and sensor noise characteristics is sometimes applied to enhance performance [SBS99, CGES06, CHNES04], as a way of establishing information fusion between sensor and vehicle dynamic information using a decentralized filtering structure. In Table 2.4, the bandwidth of the actual motion dynamics of a land vehicle as estimated by [CHNES04] is shown. The wider bandwidth of the pitch and roll angular velocity and z-axis acceleration dynamics is due to road irregularities. In [CGES06] and [CHNES04], 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 2.8. CONCLUSIONS 39 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 [SBS99], 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 [CHNES04, CGES06].

2.8 Conclusions

A survey of information sources and information fusion technologies used in in-car 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 next-generation 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.

• Reﬁned digital maps with lane information, etc. will allow map- matching procedures with a bidirectional information ﬂow, not only producing a position and trajectory consistent with the road network but also feeding back information from the map-matching to the sensor fusion.

• Increased processing power at reduced power consumption and cost levels will allow for usage of reﬁned vehicle models and non-linear ﬁltering technologies. 40 CHAPTER 2. IN-CAR NAVIGATION TECHNOLOGIES

Until this stage both OEM and third-party in-car navigation systems have been developed primarily to provide positioning and route-guidance information to the user. As the technological development of in-car navigation systems 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 input for advanced driver assistance and safety systems, such as automatized highway systems, 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. In-car positioning and navigation has been a killer application for GPS receivers and a variety of consumer electronics launched on a large scale. Positioning technologies based on a single GPS receiver are vulnerable, and thus have to be supported by additional sensor information. Moore’s law implies a continued reduction in the cost of contemporary and future sensors based on, for example, MEMS technology. Low cost sensors often produce dirty measurements, implying the need for digital signal processing as an enabler for post processing of the raw sensor data including integrity monitoring. An increasing amount of sensor information requires advanced information fusion that has to fulfil requirements on memory size, processing speed and energy consumption. There is a need for future inter- and cross-disciplinary research and development in this emergent field in order to obtain accurate positioning estimates with high integrity and full availability and continuity of service. 2.8. CONCLUSIONS 41 02, YTJZ05, + [YF03,MZIMGS07, SH05, FB98, GWA07, MS99, DLES07, RS07, ZOQN03] CGP02, [SES07, MWJ04,SES04, JL08] CC06, [GGB ASK05, BW02,ASK04, GTC07, CDDV07] SGN05, [WG04, SN06, SNOES05, NOES04, HC08, STTN07, SN07, KEE07,CH08] PION08, [BMR06, ZS05, YLP04] References for est- 3 d ∗ . d rithms in the context of navigation and tracking. nal complexity are based upon the material in [Dau05]. Computational complexity In the ordermation of of a statedimension vector of Roughly the sameEKF. as the For low dimensional problems approximately asthe for EKF, butdimensional for problems higher is it enormous.[DH03, Dau05, KSG05] for details. Refer to ∗ May bepoor good, compared to but theoretical often the- bound. Generally generates anificant sig- improvement relative to thealways. EKF, but not Optimal forsional low state dimen- suboptimal vectors, for but mensional problems high due to limited di- processing power. Not availableNot available Not available Not available The comments regarding estimation accuracy and computatio ∗ Table 2.3: List of papers describing different filtering algo Fusion methodExtended Kalman Filters (Adaptive EKF) Estimation accuracy Sigma-Point Filtersscented (Un- Kalman filters) Particle Filter (sequential Monte Carlo methods) Neural Networks Nonlinear Observer 42 CHAPTER 2. IN-CAR NAVIGATION TECHNOLOGIES

Table 2.4: Bandwidth of the true motion dynamics of a land-vehicle as estimated by [CHNES04]. 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 Chapter 3

Time Synchronization in GPS-aided INS

3.1 Introduction

Depending on the target application, a constructer of a global positioning system (GPS)-aided inertial navigation system (INS) has several design choices [SH09a]. Targeting flexible, low-cost consumer applications, a system constructed around commercial, off-the-shelf (COTS) components connected via a universal serial bus (USB) or Bluetooth interfaces to each other is preferred owing to their ease of use and from the economical point of view [HM00]. The drawback of such a platform is the limited access to the software and hardware of COTS components that often restrict the system integration to a loosely coupled, position-aided approach (generally, low-cost GPS receivers do not output pseudo-range measurements via their standard communication protocol), and introduce the problem of how to time-synchronize the data streams from the different components without having access to their low-level hardware. The latter task is an intricate problem as the GPS-receiver and the inertial measurement unit (IMU) use independent, unsynchronized clocks. Further, the data transfer to the processing unit together with the data buffering introduces unknown delays to the data streams. In [BRG06] a GPS-receiver processing delay in the range of 90–95 ms was reported. Further, in [CCF+06], solely the data transfer delay in a small Bluetooth network of an unmmanned aerial vehicle was reported to vary between 20 and 150 ms. Thus, even if the signal processing unit (personal digital assistant (PDA), laptop, etc.) time stamps the data when received, it is difficult to align the data stream from the GPS-receiver and the IMU to an accuracy of more than a few tenths of a second. The time synchronization problem is illustrated in Figure 3.1. This timing error gives rise to several

43 44 CHAPTER 3. TIME SYNCHRONIZATION IN GPS-AIDED INS

t = kTs

IMU Position Navigation Velocity t = ℓTs − Td, ℓ = k M Filter Attitude

GPS

Figure 3.1: Eﬀects of the timing error Td in the synchronization between the sampling of the GPS-receiver and IMU are studied in terms of the navigation system’s increased mean square error. In the study, the GPS- receiver sampling period is assumed a multiple M of the IMU sampling period Ts.

questions: What is the impact on the accuracy of the navigation system? How large can the errors be for a given accuracy? How to estimate and compensate for it? The first study on the impact of data timing errors in integrated navigation systems [BIV85], showed that the data timing errors may cause a bias in the estimated forward acceleration during the transfer alignment of a strap-down INS. In [LLJ02, LLJ00] and [LLJ01], the effects of using time-delayed GPS measurements in a GPS-aided strap-down INS were studied by examining the steady-state condition of the Kalman filter that was used to fuse the navigation data. Further, in [LLJ02], a calibration procedure was proposed based on driving the vehicle with the GPS-aided INS according to a predefined trajectory and studying the system’s steady state conditions for the different segments of the trajectory. A few methods for synchronizing the data streams at the hardware level are described in [LRLL06, Li04, WAHR+07] and [DWM+05]. In [Fox06] a Kalman filter framework for time-synchronization of input and output data, where the synchronization error is in the order of a fraction of the IMU sampling period, was developed. In [LL04] the temporal data synchronization problem in a multi-sensor system for cooperative driving was addressed using an unscented Kalman filter. In [Far01] it was assumed that the internal clock of the IMU was periodically reset using the 1-pulse-per-second signal from the GPS-receiver, and an interpolater was used to compensate for the IMU clock drift between the clock resets. A description of different time synchronization approaches together with the proposal of a mixed hardware- and software- based synchronization approach is found in [DWL+08]. 3.2. MSE OF THE STATE ESTIMATE 45

3.2 MSE of the State Estimate

To evaluate the effects of the time synchronization error on the performance of the navigation system, the method that is used to fuse the sensor data must be specified. In this study, a feedback implementation of a comple- mentary filter is used to fuse the navigation data. In other words, given a linear state space model of how the position, velocity, attitude, and sensor errors in the mechanization of the INS develop with time, a Kalman filter is used to estimate the error state of the INS. The estimated navigation errors are then fed back into the INS to correct its internal states. As the model for the error plant of the INS depends on the current navigation state, an indirect extended Kalman filter is created. Hence, to evaluate how the timing error will affect the performance of the navigation system, the state-space model of the error plant and the extended Kalman filter recursions must be studied. T T T Accordingly, a state vector xk of the form xk = δrk δuk where δr contains the position, velocity, attitude perturbation of the INS, and k δuk, the IMU sensor errors, is introduced. Here, T denotes the transpose operation. Subsequently, the error plant of the INS can be described by a state-space model of the form

xk+1 = Ψk xk + Gk ek (3.1) where the state transition matrix Ψk describes how the errors develop from one time instant to another and can be found by a perturbation analysis or linearization of the mechanized navigation equations and sensor models. For more details on how this process is carried out [FB98] can be referred. Further, the process noise gain matrix Gk describes how the measurement noise ek from the inertial sensors aﬀects the navigation error. As the GPS and the IMU in most cases are sampled at two diﬀerent rates, the following down-sampling function is introduced

1, k = nM, n =1, 2,... ∆(k)= . (3.2) 0, otherwise Here, M denotes the ratio between the sampling rate of the IMU and the GPS-receiver, and is considered as an integer. If the diﬀerence between the position estimates of the GPS and the INS is observed, the observation equation for the state-space model is given by

zk = Hk xk + ∆(k)wk (3.3) where the measurement matrix Hk is deﬁned as

Hk = ∆(k)[Iβ 0β,(α−β)]. (3.4) 46 CHAPTER 3. TIME SYNCHRONIZATION IN GPS-AIDED INS

Navigation Sensor error − u˜ uk k compensation equations rk IMU − r = fd(rk, uk) uk = g(u˜k , ξu) k+1 b − b ξu = ξu + δuk rk = rk + δrk b b b b Hk b b b b δuk δrk INS pk zk − b b Kk + + bGPS GPS pk b Figure 3.2: Block diagram of a closed-loop loosely coupled, position-aided, GPS-aided INS. The EKF recursions used to fuse the GPS-receiver and the IMU data may be interpreted as a method of choosing the gain Kk.

The integers α and β denote the length of the state vector xk and the position vector zk, respectively. The notations Iβ and 0β,(β−α) denote a unity matrix of size β and a β (β α) matrix with zeros, respectively. × 1 − The GPS-receiver positioning error wk and the measurement noise ek of the inertial sensors are considered as white noises, uncorrelated with each E T E T other and with covariance matrices R = wk wk and Q = ek ek . Here E denotes the expectation operation.{ } { } For{·} an error model of the form (3.1)–(3.4) the corresponding extended Kalman ﬁlter recursions, used to fuse the IMU and the GPS-receiver data in a closed-loop system, are given in Table 3.1, as derived in [SH05]. This set of equations may graphically be interpreted as the closed-loop system, as shown in Figure 3.2, where the feedback gain is chosen as the Kalman ﬁlter gain.

3.2.1 Closed-Loop Error in the System without Timing Errors To determine the difference equation for the error in the closed-loop system described by the indirect extended Kalman filter algorithm given in Table 3.1, it should be noted that when the GPS data is available, the current error state is estimated as xk = Kk zk (throughout the chapter denotes an estimate). In other words, the estimate of the current perturbation in position, velocity, andb attitude δrk and the current sensor bperturbation δuk are equal to the difference between the position estimate 1 b In reality, the errors in the positioning estimates of the GPS-receiver are correlated both in timeb and space with a covariance structure depending on the positioning algorithm of the GPS-receiver [FB98]. 3.2. MSE OF THE STATE ESTIMATE 47

Table 3.1: The indirect extended Kalman filter algorithm for integration between GPS-receiver and IMU data. No GPS data available. (k = nM, n =1, 2, ...) 6 uk = g(u˜k, ξu) Sensor error compensation − − rk+1 = fd(rk , uk) Navigation equation update b − − T T Pk+1 = Ψk Pk Ψk + Gk Q Gk Covariance update b bGPSb data available. (k = nM, n =1, 2, ...) − T − T −1 Kk = Pk Hk (Hk Pk Hk + R) Kalman gain update r− z = pGPS H k Calculation of the difference k k − k 0 between the position estimates b bδrk xˆk = = Kk zk Estimation of the state errors δuk − rk brk = + xk Error correction ξu bξu − Pbk = (I b Kk Hk) Pk Covariance update − b uk = g(u˜k, ξu) Sensor error compensation − rk+1 = fd(rk, uk) Navigation equation update b − − T T Pk+1 = Ψk Pk Ψk + Gk Q Gk Covariance update b b b of the GPS-receiver and the INS, scaled by the Kalman filter gain. This error estimate is used to correct the current navigation state of the INS, i.e., − rk , and the IMU calibration parameters, i.e., ξu, before the time update. Thus, the state error after the feedback is the error before feedback xf,k bminus the estimated error xk. With reference to (3.1), describing how the INS state errors (in the open-loop system) relate from one time instance to the next, the error of theb closed-loop system xf,k is found to be

xf,k+1 = Ψk (xf,k xk)+ Gk ek − (3.5) = Ψk xf,k Ψk Kk zk + Gk ek. − b Equation (3.5) can be written in a more convenient form by using the deﬁnition of the observation vector zk in (3.3) and by noting that Kp,k = Ψk Kk is the one-step-ahead Kalman prediction gain. Thus (3.5) can be rewritten as

xf,k+1 = Ψkxf,k Kp,k(Hkxf,k + ∆(k)wk)+ Gkek − (3.6) = (Ψ K H )x K w + G e . k − p,k k f,k − p,k k k k In the second equality, one can consider that the Kalman gain Kk as well as the prediction gain Kp,k per definition are zero matrices, whenever the 48 CHAPTER 3. TIME SYNCHRONIZATION IN GPS-AIDED INS downsampling function ∆(k) in (3.2) is zero. Equation (3.6) can be identified as the difference equation describing the error of the one-step-ahead Kalman filter predictor. Indeed, [BS71] demonstrated that for a linear system, the optimal choice of the feedback gain is the Kalman predictor gain, resulting in a system with an error covariance equal to that of the linear Kalman predictor. Thus, since the error estimates in the system are unbiased, the MSE of the system with feedback is given by the one-step-ahead Kalman predictor error covariance. In summary, the stochastic difference equation (3.6) describes how the errors in the estimated position, velocity, attitude, and sensor errors propa- gate with time in the closed-loop implementation of a GPS-aided INS. The dynamics of the errors in the system are determined by the time varying closed-loop system matrix (Ψk Kp,k Hk), which depends on the trajectory dynamics of the navigation− platform. Therefore, analytical results about the convergence of the errors in the navigation system are difficult to present.

3.2.2 Closed-Loop Error in the System with Timing Errors Now, let us consider the case when there is a timing error between the sampling of the GPS-receiver and the IMU, so that their sampling instants are not perfectly aligned (see Figure 3.1). Under the assumption that the GPS- receiver and the IMU clocks are suﬃciently stable or that the clock skews are estimated, using for example the technique described in [MST99], and compensated for, then the misalignment in the sampling instant, denoted by Td, can be considered as constant. Next, without loss of generality, let us assume the timing error to occur in the sampling of the GPS-receiver and let the function p(t) describe the true trajectory of the navigation platform. Thus, the position estimates from the GPS-receiver at time k can be described as

pGPS = ∆(k)(p(kT T )+ w ) (3.7) k s − d k where wk is the estimation error of the GPS-receiver. Furthermore, let us assume that a roughb synchronization of the GPS-receiver and the IMU data has been carried out by means of correlation or by time stamping the data when received at the signal processing unit, so that T is in the order | d| of a few sampling periods Ts. Moreover, let us assume that the jerk of the navigation platform is much smaller than the reciprocal of the cube of Td . Accordingly, p(kT T ) in (3.7) can be approximated by a second-order| | s − d Taylor expansion around t = kTs, i.e.,

T 2 p(kT T ) p v T + a d (3.8) s − d ≃ k − k d k 2 3.2. MSE OF THE STATE ESTIMATE 49

where pk, vk, and ak denote the navigation platform position, velocity, and acceleration at sampling instance k, respectively. The symbol denotes an equality where only the signiﬁcant terms are retained (i.e.,≃ third and higher-order terms in Td are neglected). By inserting (3.8) into (3.7), we get

T 2 pGPS ∆(k) p v T + a d + w . (3.9) k ≃ k − k d k 2 k a Thus, the trueb observation zk of the system, which is fed back into the closed-loop system, can be described as

za = ∆(k)(pGPS pINS) k k − k T 2 (3.10) ∆(k)(p v T + a d + w pINS). ≃ bk − k b d k 2 k − k

INS Here pk is the position estimate from the INS at timeb k. Subsequently, identifying that H x = ∆(k)(p pINS) and substituting it into (3.10) k f,k k − k yieldsb the following description of the true observation for the system with a timing error b

za ∆(k)(H x + w + d ). (3.11) k ≃ k f,k k k In (3.11), the position error bias vector dk is introduced as

T 2 d = v T + a d . (3.12) k − k d k 2 By comparing (3.11) and (3.3) it can be clearly noted that the actual observation contains not only the position error and the GPS-receiver estimation error wk, but also a bias term dk that depends on the timing error Td and a the dynamics of the navigation system. By substituting zk with zk in (3.5) and inserting (3.11) into this, along with a slight misusage of the subscript notation, the following diﬀerence equation for the error state in the closed- loop navigation system with timing errors can be obtained

x = Ψ x K za + G e fd,k+1 k fd,k − p,k k k k = Ψ x K ∆(k)(H x k fd,k − p,k k fd,k (3.13) + wk + dk) + Gk ek. Here the subscript ( )fd is used to denote the diﬀerence from the state errors in the closed-loop· system without timing error. By examining the recursions in Table 3.1, it is clear that the Kalman ﬁlter gain Kk and 50 CHAPTER 3. TIME SYNCHRONIZATION IN GPS-AIDED INS

consequently the Kalman ﬁlter prediction gain, Kp,k are zero whenever ∆(k) is zero. Thus, (3.13) can be rewritten into a more familiar form as

xfd,k+1 = (Ψk Kp,k Hk) xfd,k Kp,k wk − − (3.14) + G e K d . k k − p,k k Equation (3.14) has similarities with the diﬀerence equation (3.6) describing the estimation error in the closed-loop system without timing error except for the last additional term Kp,k dk reﬂecting the contribution owing to timing error. Subsequently, if we introduce then the MSE of the navigation solution may be expressed as

P = E x (x )T fd,k+1 { fd,k+1 fd,k+1 } T T T = Φk Pfd,k Φk + Kp,k RKp,k + Gk Q Gk (3.15) + K d dT KT Φ x dT KT p,k k k p,k − k fd,k k p,k K d xT ΦT − p,k k fd,k k where the mean error x = E x can be calculated as fd,k { fd,k} x = Φ x K d . (3.16) fd,k+1 k fd,k − p,k k The derived expressions (3.15) and (3.16) describe the dynamics of the MSE and the mean error of the navigation solution in a closed-loop GPS-aided INS with timing error. The three last terms in (3.15) are the contribution to the MSE owing to the time synchronization error, whereas the three former ones correspond to the case when there is no time synchronization error. From (3.15) and (3.16) it is clear that the system error dynamics are determined by the time-varying closed-loop system matrix, Φk, and position bias, dk, both depending on the trajectory of the navigation platform. Therefore, general analytical results of the convergence of the estimation errors are diﬃcult to derive. However, the derived expressions can be numerically evaluated to investigate how the timing error will aﬀect the MSE of the navigation solution for a certain trajectory.

3.2.3 Example: Single-Axis GPS-aided INS Consider the single-axis navigation problem illustrated in Figure 3.3 with reference to, e.g., performance measurements in drag racing type of activities [Fox73, HYUS09]. The purpose is to determine the position of the vehicle along a horizontal straight track. The vehicle is equipped with a GPS-receiver that gives noisy position estimates at 10 Hz. The acceleration and tilt rate of the vehicle are measured by an accelerometer and a 3.2. MSE OF THE STATE ESTIMATE 51

accelerometer sensitivity axis

ω˜(t) acceleration a˜(t)

gravity vector gravity vector

Figure 3.3: Example of a single-axis GPS-aided INS system. gyroscope, respectively. When the vehicle accelerates it will start to tilt and the accelerometer will not only sense the forward acceleration but also the gravitational acceleration. If we assume the gravity vector to have the magnitude, g, then the relationship between the acceleration and angular rate of the vehicle and its tilt angle, velocity, and position can be described by the following set of diﬀerential equations:

p˙(t) = v(t) (3.17) v˙(t) = s(t)/ cos(φ(t)) g tan(φ(t)) (3.18) − φ˙(t) = ω(t) (3.19) Here, p(t), v(t), s(t), φ(t), and ω(t) denote the position, velocity, specific force, tilt, and angular rate of the vehicle, respectively. These equations are referred to as the navigation equations used in the INS to calculate the position, velocity, and tilt from the measured specific force and angu- T lar rates. By introducing the state vector rc = p(t) v(t) φ(t) and T input vector uc = s(t) ω(t) , the continuous time navigation equations (3.17)–(3.19) may be written as r˙ = f (r , u ). Assuming that there c c c c is a bias, δωc, and noise, ec,2, in the gyroscope measurements, and that the specific force measurements are only disturbed by some noise ec,1, the measured output from the accelerometer and gyroscope can be described as s˜c = sc + ec,1 and ω˜c = ωc δωc + ec,2, respectively. Using these measurements as the input to the INS− results in an error in the calculated position, velocity, and tilt. A linear model describing how the errors develop with time may be obtained by linearizing the navigation equations around the true trajectory, i.e.,

r˙ = r˙ δr˙ = f (r , u˜ ) c c − c c c c ∂fc(r, u) ∂fc(r, u) fc(rc, uc) δrc + ec,1 b ≃ − b ∂r ∂sc (3.20) r=rc u=uc

∂fc(r, u) (δωc ec,2). − ∂ωc − u=uc

52 CHAPTER 3. TIME SYNCHRONIZATION IN GPS-AIDED INS

Here, rc and u˜c denotes the estimated state and measured input vector, respectively. Hence, by assuming that the bias, δωc, in the angular rate measurementsb is constant, the errors in the single-axis INS can be modeled as

δr˙ x˙ = c = Ψ xe + G e (3.21) c δω˙ c c c c c T where ec = ec,1 ec,2 , 01 0 0 0 0 sc sin(φc)−g 0 0 cos2(φ ) 0 κ 0 Ψc =  c  , Gc =   , (3.22) 00 0 1 0 1  00 0 0   0 0          and κ =1/ cos(φc). In the error model (3.21)–(3.22), both the state transition matrices Ψc and Gc are time varying, as they depend on the speciﬁc force, sc, and tilt angle, φc. If the sample rate of the INS is high compared with the rate of change in Ψc and Gc, then a zero-order hold sampling of the system results in the following discrete time model for the error in the navigation system.

xk+1 = Ψk zk + Gk ek (3.23) where

Ψ = exp(T Ψ (kT )) I + T Ψ (kT ) k s c s ≈ 4 s c s 1 Ts 0 0 sc(k Ts) sin(φc(k Ts))−g 0 1 Ts 2 0 (3.24) =  cos (φc(k Ts))  , 00 1 Ts  00 0 1      0 0 T / cos(φ (kT )) 0 G = G (kT )= s c s , (3.25) k c s  0 1   0 0    and  

σ2 0 Q = E e eT = acc . (3.26) { k k } 0 σ2 gyro 2 2 Here, σacc and σgyro denote the accelerometer and gyroscope noise variance, respectively. Further, as the diﬀerence in the estimated position of the 3.2. MSE OF THE STATE ESTIMATE 53

Position 15000

] 10000 m [ 5000 0 0 100 200 300 400 500 Velocity ] 20 m/s [ 0 0 100 200 300 400 500 Tilt

5 ] ◦ 0 [ −5 0 100 200 300 400 500 Time [s]

Figure 3.4: Trajectory used in the single-axis GPS-aided INS example.

GPS-receiver and the INS is observed, the observation equation for the error model becomes

zk = Hk xk + ∆(k) wk (3.27) where Hk = ∆(k) 1 01,3 (3.28) and

R = E w wT = σ2 . (3.29) { k k } gps Having derived the state-space model for the error plant in the single axis INS system, which has the same structure as the error model in (3.1)– (3.4), the results in (3.15)–(3.16) can, given the vehicle trajectory, be applied to evaluate how the time synchronization error aﬀects the accuracy of the integrated navigation system. For example, consider the vehicle trajectory described in Figure 3.4, then the expected performance (i.e., the performance predicted by evaluating (3.15)–(3.16) given the system error model and vehicle trajectory) in terms of the root mean square error (RMSE) in the navigation solution of the integrated navigation system with an inherent time synchronization error of 50 ms is shown in Figure 3.6. In the example, the system parameters are those given in Table 3.2. A Monte Carlo (MC) simulation of the single-axis navigation example was also conducted to illustrate the error model in (3.15)–(3.16). The MC simulation calculated RMSE, indicated in red in Figure 3.6, demonstrate good agreement with those calculated from the error model. 54 CHAPTER 3. TIME SYNCHRONIZATION IN GPS-AIDED INS

Table 3.2: Setting used when evaluating the single-axis GPS-aided INS. Parameters common to both simulations. Parameter Value Unit

σgps 0.5 [m] 2 σacc 0.01 [m/s ] ◦ σgyro 0.1 [ /s] Ts 10 [ms] M 10 — 2 2 [P]1,1 0.1 [m ] 2 2 [P]2,2 0.1 [(m/s) ] 2 ◦ 2 [P]3,3 2 [( ) ] 2 ◦ 2 [P]4,4 0.1 [( ) ] ext [xfd,0]1:4 = [xfd,0]1:4 0 — Parameters only used with the ordinary error model. Parameter Value Unit

Td 0.05 [s] Parameters only used with the extended error model. Parameter Value Unit

Td U(0, 0.1) [s] ext 2 [Pfd,0]5,5 1/300 [s ] ext [xfd,0]5 0.05 [s]

ext Note: The Kalman ﬁlter covariance matrix P and the MSE matrices Pfd and Pfd used in the evaluation of the derived MSE expressions, were all initialized as diagonal matrices with diagonal elements as given in the table. All the initial values in the simulation were drawn from independent zero mean Gaussian distributions, with the exception of the time synchronization error in the simulation of the system with the extended error model, which was drawn from a uniform distribution. Further, all second-order moments were set to consist with the values given for P. 3.3. ESTIMATING AND REMOVING THE TIMING ERROR 55

3.3 Estimating and Removing the Timing Error

The dependence between the timing error Td and the bias in the observed a position difference zk, see (3.10), can be exploited to calculate an estimate of the timing error. A description of how this can be done, using the closed-loop system structure and extended Kalman filtering described in Section 3.2, is given next; a model for the MSE of the state estimate of the corresponding system is also derived. To use the closed-loop system structure employed in Section 3.2 to also estimate and compensate for the timing error, two things must be done. First, the state space model used in the extended Kalman filter must be extended to also included the timing error. Secondly, a method for time- shifting the IMU measurements, in accordance with estimated timing error must be implemented.

3.3.1 Estimating the Timing Error

To ﬁnd the extended error model, let Td,k be the estimate of the timing error at time instant k, which is used to control a variable delay ﬁlter synchronizing the GPS-receiver and theb IMU data streams, as illustrated in Figure 3.5. (Note that the sample index k from now on refers to the time instant t = kTs Td,k.) Further, let δTd,k = Td Td,k be the corresponding estimation error.− The error propagation in the− open-loop system system can then be describedb by the extended error modelb

ext ext ext ext xk+1 = Ψk xk + Gk ek, (3.30)

ext T T where xk = [xk δTd,k] is the extended error state vector and

Ψ 0 G Ψext = k α,1 and Gext = k . (3.31) k 0 1 k 0 1,α ext ext The structure of Ψk and Gk are due to the fact that the timing error was assumed constant. Next, to ﬁnd the measurement matrix for the ex- ext tended error model, Hk , let us once again do a second-order Taylor series expansion of the GPS-receiver position estimate as in (3.9), but this time around kT T . That, is s − d,k b pGPS = ∆(k) p(kT T )+ w k s − d k = ∆(k) p(kT T δT )+ w b s − d,k − d,k k (3.32) δT 2 ∆(k) p v δTb + a d,k + w . ≃ k − k d,k k 2 k 56 CHAPTER 3. TIME SYNCHRONIZATION IN GPS-AIDED INS

Navigation Fractional Sensor error − u˜d,k uk delay ﬁlter compensation equations rk IMU − r = fd(rk, uk) uk = g(u˜d,k , ξu) k+1 Td = Td + δTd,k b − b ξu = ξu + δuk rk = rk + δrk b b b b b b b Hk b b b b δTd,k δuk δrk INS pk zk − b b b Kk + + bGPS GPS pk b Figure 3.5: Block diagram of a closed-loop loosely coupled, position-aided, GPS-aided INS with feedback for the estimated timing error.

Here, pk, vk, and ak are the short-hand notations for p(kTs Td), etc. The diﬀerence between the GPS-receiver and INS position est−imates can thus be described as b

za,ext = ∆(k) pGPS pINS k k − k δT 2 = ∆(k)p pINS v δT + a d,k + w bk − k b − k d,k k 2 k (3.33) δT 2 = H x + ∆(bk) v δT + a d,k + w k k − k d,k k 2 k INS where pk is the INS position estimate at time instant k calculated from ext IMU data that have been shifted in time by Td,k seconds. Let Hk be deﬁnedb as b Hext = H ∆(k) v (3.34) k k − k then a,ext ext ext ext zk = ∆(k) (Hk xk + wk + dk ) (3.35) where δT 2 dext = a d,k . (3.36) k k 2 ext Thus, neglecting the bias dk in (3.35), then the measurement model (3.34)–(3.35) together with the error propagation model (3.30)–(3.31) can be used with the extended Kalman ﬁlter in Table 3.1 to estimate both the navigation error and the error in the estimated timing error Tˆd,k.

3.3.2 Implementing a Variable Time Delay In the previous section, it was assumed that the inertial measurement unit data could be shifted in time with an arbitrary amount of time using a 3.3. ESTIMATING AND REMOVING THE TIMING ERROR 57

RMSE of the position estimate. 2 ] m [ 1 RMSE 0 0 20 40 60 80 100 RMSE of the velocity estimate.

] 0.4 m/s [ 0.2

RMSE 0 0 20 40 60 80 100 RMSE of the tilt estimate. 0.04 ] ◦ [ 0.02 RMSE 0 0 20 40 60 80 100 Time [s]

Figure 3.6: RMSE of the estimated position, velocity, and tilt for a time synchronization error of 50 ms. (a) No timing error – dashed black line with triangles, (b) MSE model – blue line with squares, and (c) MC simulation – red line with circles. variable delay. However, in reality the IMU data are sampled at certain time instances. To have a continuously variable time-shift D = Td/Ts, an adjustable interpolator or fractional delay filter must be used. One of the most commonly used techniques for fractional delay filter designb is the Lagrange interpolation, in which the filter coefficients of the fractional delay FIR filter is calculated as [LVKL96]

N D k h(n)= − for n =0, 1, 2, ..., N. (3.37) n k k=0 − kY6=n

Here N is the order of the ﬁlter and D [(N 1)/2, (N + 1)/2] is the desired delay. If a delay larger than D = (N∈ +1)/−2 is required, this may be solved by adding an appropriate number of unit-delays before the fractional delay FIR ﬁlter.

3.3.3 MSE of the State Estimate in the Extended System A model for the MSE and mean error of the state vector estimate in the system that also estimates the timing error can be found by substituting a,ext zk in (3.5) with zk . Thus, the estimation error in the extended system develops with time as 58 CHAPTER 3. TIME SYNCHRONIZATION IN GPS-AIDED INS

ext ext ext ext ext ext xfd,k+1 = (Ψk Kp,k Hk ) xfd,k Kp,k wk − − (3.38) + Gext e Kext dext. k k − p,k k To evaluate the MSE of the navigation ﬁlter using the extended error model, it may be appealing to use the expressions in (3.15)–(3.16). However, as ext ext the bias term dk is a function of the last entry in xfd,k, i.e., δTd,k, this cross-correlation must be considered when evaluating the last three terms in (3.15). Under the assumption that the state estimates are Gaussian distributed, the MSE of the navigation solution in the extended system with feedback becomes

ext ext ext ext T ext ext T Pfd,k+1 = Φk Pfd,k (Φk ) + Kp,k R (Kp,k ) ext ext T ext ext T + Gk Q (Gk ) + Kp,k Πk (Kp,k ) (3.39) Φext Γ (Kext)T Kext ΓT (Φext)T − k k p,k − p,k k k where Φext = (Ψext Kext Hext). Further, k k − p,k k a aT Π = k k (3 [Pext ]2 2 [xext ]4), (3.40) k 4 fd,k i,i − fd,k i

ext ext ext Γk = [Pfd,k]i,i xf,k + 2 [xf,k ]i (3.41) aT [Pext ] [xext ] xext k · fd,k 1:α,i − fd,k i fd,k 2 and the mean error is given by 1 xext = Φext xext Kext a [Pext ] . (3.42) fd,k k fd,k − 2 p,k k fd,k i,i

Here i denotes the index of the element δTd,k in the state vector of the extended error model. A derivation of the above expression for the MSE in the system with the additional state for the time synchronization error is found in Appendix 3.A. To illustrate the proposed time synchronization method, and to validate the derived MSE model, we rely on the following single-axis example.

3.3.4 Example: Single-axis GPS-aided INS Revisited Let us again consider the single-axis GPS-aided INS example given in Sec- tion 3.2.3. However, this time the timing error has been included in the feedback loop of the integrated navigation system. An adjustable version of 3.3. ESTIMATING AND REMOVING THE TIMING ERROR 59

RMSE of the position estimate. 2 ] m [ 1 RMSE 0 0 100 200 300 400 500 RMSE of the velocity estimate.

] 0.4 m/s [ 0.2

RMSE 0 0 100 200 300 400 500 RMSE of the tilt estimate 0.04 ] ◦ [ 0.02 RMSE 0 0 100 200 300 400 500 Time [s]

Figure 3.7: RMSE of the estimated position, velocity, and tilt for the case when the time synchronization error is included in the error model used in the navigation ﬁlter. (a) No timing error – dashed black line with triangles, (b) MSE model – blue line with squares, and (c) MC simulation – red line with circles.

a third-order Lagrange interpolator has been implemented using a Farrow filter structure [Väl95]. The adjustable fractional delay FIR filter is used as variable time-shift operator in a MC simulation of the one-dimensional GPS-aided INS with an extended error model as described in Section 3.3.1. In Figures 3.7–3.8, the results from the MC simulation of the single-axis GPS-aided integrated navigation system with the feedback for the timing error are shown along with the MSE and mean error, predicted from the error model in (3.39)–(3.42). The trajectory and setting used in the simulation is again given by Figure 3.4 and Table 3.2, respectively. Further, the timing error has been modeled as uniformly distributed between 0 and 100 ms and the filter has been initialized for a time synchronization error equal to zero. The results of the MC simulation show that the empirical calculated MSE agree well with those predicted by the MSE model in (3.39)–(3.42). Further, from the figures, it is clear that by including the timing error in the feedback loop of the integrated navigation system the effects of the timing error will become negligible as the filter converge. 60 CHAPTER 3. TIME SYNCHRONIZATION IN GPS-AIDED INS

RMSE of the time synchronization error estimate. 0.06

] 0.04 s [

0.02 RMSE

0 0 100 200 300 400 500 Time [s] Mean error of time synchronization error estimate. 0.06

] 0.04 s [

Mean 0.02

0 0 100 200 300 400 500 Time [s]

Figure 3.8: RMSE and mean error of the time synchronization error estimate. (a) MSE an mean error model – blue line (squares), (b) MC simulation – red line (circles).

3.4 Time Synchronization in GPS-aided INS

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

3.4.1 Simulated Data In Figure 3.9, the trajectory used in the MC simulations and evaluation of the derived MSE expression of the GPS-aided INS is shown. The results of the MC simulation along with the values predicted by the MSE models are 3.4. TIME SYNCHRONIZATION IN GPS-AIDED INS 61

Simulation trajectory 50

−50 ] m −100 North [

−150

−200

−250 −60 −40 −20 0 20 40 60 80 100 120 East [m] Figure 3.9: Trajectory used in the MC simulation of the GPS-aided INS and the evaluation of the covariance expressions.

shown in Figures 3.10-3.14. In Table 3.3, the setting used in the simulations is summarized. Figures 3.10–3.11 illustrate the case when the timing error is not included in the system model. It can be observed that there is a good agreement between the MSEs and mean errors predicted by the model and those calculated from the MC simulation. In Figure 3.11, the bias in the forward acceleration owing to the timing error can be observed. When the timing error is included in the model (see Figures 3.12–3.14), the derived MSE expression do not perfectly capture the transient behavior of the integration filter. This is probably owing to the extra nonlinearity introduced by the time shifting of the IMU data using the fractional delay filter, which is not properly captured by the MSE expressions. However, as the filter converges the agreement between the model predicted MSE and those calculated from the MC simulation improves, and it is clearly evident that the effects of the timing error are removed. Moreover, from Figure 3.13 it is obvious that the bias in the forward acceleration is substantially reduced.

3.4.2 Real-world Data Real-world data was collected using a 100 Hz GPS-receiver and data logger (VBOX III) together with an additional MEMS IMU, all from Racelogic. In Figure 3.15, the trajectory of the vehicle is shown. The test was conducted in a parking lot, outside Stockholm, Sweden, under the following conditions: clear sky view with no large surrounding obstacles blocking the satellite signals and with 8–9 satellites in view during the entire run. The GPS 62 CHAPTER 3. TIME SYNCHRONIZATION IN GPS-AIDED INS

RMSE of the north position estimate. 2

] 1.5 m [ 1

RMSE 0.5

0 0 50 100 150 200 250

RMSE of the east position estimate. 3 ]

m 2 [

RMSE 1

0 0 50 100 150 200 250 Time [s]

Figure 3.10: RMSE of the north and east position estimates for a time synchronization error of 100 ms. (a) No timing error – dashed black line, (b) MSE model – blue line with squares, (c) MC Simulation – red line with circles.

Mean of the accelerometer bias estimation error.

0.06 ] 2 0.04 m/s [ 0.02

Mean 0 −0.02 0 50 100 150 200 250

Mean of the gyroscope bias estimation error. 0.1 0.05 ] /s ◦

[ 0 −0.05 Mean −0.1

0 50 100 150 200 250 Time [s]

Figure 3.11: Mean of the error in the accelerometer and gyroscope bias estimates for a time synchronization error of 100 ms. MC simulation: X-axis (forward) – red line with circles, Y-axis (sideways) – green line with squares, and Z-axis (downward) – blue line with triangles. Mean error model: X- axis (forward) – cyan line with circles, Y-axis (sideways) – magenta line with squares, and Z-axis (downward) – yellow line with triangles. 3.4. TIME SYNCHRONIZATION IN GPS-AIDED INS 63

RMSE of the north position estimate.

] 1 m [

0.5 RMSE

0 0 50 100 150 200 250

RMSE of the east position estimate.

] 1 m [

0.5 RMSE

0 0 50 100 150 200 250 Time [s]

Figure 3.12: RMSE the north and east position estimates when the timing error is included in the estimation problem of the data integration. (a) MSE model – blue line with squares, (b) MC Simulation – red line with circles.

position estimates and the IMU data were integrated using the software developed in-house. To test the time synchronization approach, a data set with an intentional time synchronization error of 100 ms was constructed2. From this data set, another 28 data sets, each with a 20-s GPS outages, starting with 1-s intervals during the penultimate lap of the trajectory, were created and used as input for the integration software. Further, the GPS data was downsampled to 5 Hz. The drift rate of the system during GPS outages is often used as an indicator of the MEMS sensors quality as well as to judge how well the sensors have been calibrated by the integration filter [ESN07]. As seen in Figure 3.11, as well as shown in [LLJ02], the timing error causes a false bias in the forward acceleration. Thus, the drift rate during GPS outages should be larger when the integration filter does not consider the time synchronization error, than when the integration filter estimates and compensates for the timing error. In Figure 3.16, the arithmetic mean horizontal position errors for the different GPS outages as a function of their starting times during the penultimate lap are shown. Clearly, the position drift is smaller when the synchronization error has been included as a state in the integration filter,

2 The VBOX III GPS-receiver and IMU are hardware synchronized and the collected data is synchronized to an accuracy of a few milliseconds. 64 CHAPTER 3. TIME SYNCHRONIZATION IN GPS-AIDED INS

−3 x 10 Mean of the accelerometer bias estimation error. 15 ]

2 10 m/s [ 5

Mean 0

−5 0 50 100 150 200 250

Mean of the gyroscope bias estimation error.

0.04 ] /s ◦

[ 0.02 0 Mean −0.02 −0.04 0 50 100 150 200 250 Time [s]

Figure 3.13: Mean of the error in the accelerometer and gyroscope bias estimates when the timing error is included in the estimation problem of the data integration. MC simulation: X-axis (forward) – red line with circles, Y-axis (sideways) – green line with squares, and Z-axis (downward) – blue line with triangles. Mean error model: X-axis (forward) – cyan line with circles, Y-axis (sideways) – magenta line with squares, and Z-axis (downward) – yellow line with triangles.

Mean error of the time synchronization error estimate. 0.1 ] s [ 0.05 Mean

0 0 50 100 150 200 250

RMSE of time synchronization error estimate. 0.1 ] s [ 0.05 RMSE

0 0 50 100 150 200 250 Time [s]

Figure 3.14: Mean error and RMSE of the time synchronization error estimate. (a) MSE and mean error model – blue line with squares, and (b) MC Simulation – red line with circles. 3.4. TIME SYNCHRONIZATION IN GPS-AIDED INS 65

Table 3.3: Setting used in the simulations and MSE evaluations. Parameters common to both simulations. Parameter Value Unit

σgps 3/√3 [m] 2 σacc 0.02 [m/s ] ◦ σgyro 0.15 [ /s] Ts 10 [ms] M 20 — 2 2 [P]1:3,1:3 (3/√3) [m ] 2 2 [P]4:6,4:6 0.1 [(m/s) ] 2 ◦ 2 [P]7:8,7:8 1 [( ) ] 2 ◦ 2 [P]9,9 3 [( ) ] 2 2 [P]10:12,10:12 0.1 /12 [(m/s) ] 2 ◦ 2 [P]13:15,13:15 1 /12 [( /s) ] ext [xfd,0]1:15 = [xfd,0]1:15 0 — Parameters only used with the ordinary error model. Parameter Value Unit

Td 0.1 [s] Parameters only used with the extended error model. Parameter Value Unit

Td U(0.05, 0.15) [s] ext 2 [Pfd,0]16,16 13/1200 [s ] ext [xfd,0]16 0.1 [s]

ext Note: The Kalman ﬁlter covariance matrix P and the MSE matrices Pfd and Pfd used in the evaluation of the derived MSE expressions were all initialized as diagonal matrices with diagonal elements, as given in the table. All initial values in the simulation were drawn from independent zero-mean Gaussian distributions, with the exception of the time synchronization error in the simulation of the system with the extended error model which was drawn from a uniform distribution. Further, all second-order moments were set to comprise the values given for P. 66 CHAPTER 3. TIME SYNCHRONIZATION IN GPS-AIDED INS

Real world trajectories

20 0 [s]

0 25 [s] −20 5 [s] −40 ] m −60

North [ 10 [s] −80 20 [s] −100

−120

−140 15 [s] −60 −40 −20 0 20 40 60 80 100 East [m] Figure 3.15: Real-world trajectory used for testing the time synchronization approach using a fractional delay ﬁlter in the feedback loop. The red squares indicate the vehicle’s position after 0, 5, 10, 15, 20, and 25 s in the penultimate lap.

Mean horizontal position error during a 20 second GPS outage 9

6 ] m 5

4 Mean error [ 3

2 Time synchronization error not taken into account by the ﬁlter. 1 Time synchronization error taken into account by the ﬁlter.

0 0 5 10 15 20 25 Time [s] Figure 3.16: Arithmetic mean of the horizontal position errors during a 20-s GPS outage as a function of the start time of the outage during the penultimate lap. 3.5. OBSERVABILITY ANALYSIS 67

Table 3.4: Estimated time synchronization errors.

Td [ms]: 0 50 100 150 Td [ms]: 0.7 46 98 147

b than when time synchronization error is not considered by the filter, indicating that the filter works as intended. Here, it should be pointed out that when evaluating the drift rate, the trajectory estimated by the filter using all the available data and without any intentional time synchronization errors is used as a reference trajectory. The horizontal position accuracy of the VBOX III GPS-receiver when using satellite based augmentation system (SBAS) differential correction data is specified to 1.8 m at a 95% confidence interval. The reduction in position error shown in Figure 3.16 should therefore only be viewed as indication of the benefits of estimating the time synchronization error and not as absolute figures. The dependency between the position error growth and the start time of the outage can be explained by the differences in the trajectory dynamics during the outages, and thus, the excitation of different errors modes of the IMU sensors [Far07]. To further test the proposed time synchronization approach, three new data sets with the intentional synchronization errors of 50, 100, and 150 ms, were constructed and used as input for the integration software. Further, the GPS data was downsampled to 5 Hz. In Table 3.4, the estimated timing errors for the three different time synchronization errors are shown. For all the different timing errors, time synchronization in the order of a few milliseconds was achieved, again indicating that the filter works as intended.

3.5 Observability Analysis

The observability of the error states in GPS-aided INSs have been exten- sively studied in [GMBI92a, GMBI92b, HLC+05], and [RAHS04]. There- fore, only the question regarding the observability of the additional timing error state is addressed here. For time-varying systems such as (3.1)– (3.4), (3.65)–(3.67), the observability analysis is in general cumbersome. However, for certain trajectories, the characteristics of the system model can be captured by a piece-wise constant model. An observability analysis of the piece-wise constant model, which is fairly straightforward, can then give valuable insight regarding the observability of the time-varying model [GMBI92a]. Accordingly, the observability of the timing error in the extended version of the state-space model (3.30)–(3.31) is examined for a straight-line, constant attitude (SLCA) trajectory with piece-wise constant 68 CHAPTER 3. TIME SYNCHRONIZATION IN GPS-AIDED INS acceleration segments, for which the system dynamics can be described by a piece-wise constant model. The observability analysis of the time synchronization error is carried out in four steps. First, the deﬁnition of observability is stated and a mathematical tool to examine the observability is shortly described. Sec- ond, the observability of the GPS-aided INS with the standard error model (3.1)–(3.4), (3.65)–(3.67), is studied during an SLCA trajectory where the navigation platform experience a constant acceleration. Thereafter, the observability of the GPS-aided INS with the extended state-space model (3.30)–(3.31) is examined for the same trajectory conditions. Finally, the observability of the GPS-aided INS with the extended state-space model is examined for an SLCA trajectory consisting of two constant acceleration segments. For simplicity, the ratio M between the sampling of the IMU and the GPS-receiver is set to one, throughout the observability analysis.

3.5.1 Observability Deﬁnition

A system is said to be observable at time k, if the state vector, xk can be determined from the outputs zk, ..., zk+N for a ﬁnite N [ÅW84]. By neglecting the process noise and measurement noise in the state space model (3.1)–(3.4), this is equivalent to the existence of a unique solution to

qk = W xk (3.43) where T T T T T qk = zk zk+1 zk+2 . . . zk+N (3.44) and Hk Hk+1 Ψk   W = Hk+2 Ψk+1 Ψk . (3.45)  .   .     H N Ψ   k+N i=1 N+k−i    This will be the case if the observabilityQ matrix has full column rank, i.e., rank W = α. Otherwise, the set Ωxk of possible solutions to (3.43) is given{ by}

n Ωx = xk R ; qk = W0 xk k { ∈ } (3.46) = x Rn ; x = xa + Υ η, η Rℓ { k ∈ k k ∈ } a where W Υ = 0α,ℓ and rank(Υ) = ℓ. Further, xk denotes the true state vector that produced the observations. Here, ℓ is the rank of the null space and the columns of Υ span the null space of the observability matrix. Hence, if the observability matrix does not have full rank, then the 3.5. OBSERVABILITY ANALYSIS 69 examination of the span of the null space may reveal the states that are observable (has a unique solution) and those that are not.

3.5.2 Observability During a SLCA Trajectory During an SLCA trajectory with constant acceleration, the state transition matrix of the GPS-aided INS becomes time-invariant, i.e., Ψk = Ψ. Fur- ther, by utilizing the structure (strictly upper diagonal) of Ψc in (3.66), it can be shown that

m m Ψ = (Iα + Ts Ψc)

Iα + Ts Ψc, m =1 2 2 Iα +2 Ts Ψc + Ts Ψc , m =2 (3.47) =  1 2 2  Iα + mTs Ψc + µmTs Ψc  2 3 3 +µmTs Ψc , m > 3  where  m m µ1 = and µ2 = . (3.48) m 2 m 3 Thus, it is then straight forward that from (3.45) show that the observability matrix for the GPS-aided INS (without the extended error model) during an SLCA trajectory with constant acceleration becomes

I3 03 03 03 03 I3 Ts I3 03 03 03  2 n 2 n  I3 2 Ts I3 Ts [s ]× Ts Cp 03 2 n 2 n 3 n n W =  I3 3 Ts I3 3 Ts [s ]× 3 Ts Cp Ts [s ]×Cp   2 n 2 n − 3 n n   I3 4 Ts I3 6 T [s ]× 6 T C 4 T [s ]×C   s s p − s p   . . . . .   . . . . .     I NT I µ1 T 2 [sn] µ1 T 2 Cn µ2 T 3[sn] Cn   3 s 3 N s × N s p − N s × p   (3.49) From the structure of the observability matrix (3.49), it can be observed the ﬁrst six columns are linearly independent of each other and all the other 1 2 columns (note the scaling with µi or µi , where i =1, 2,...,N in the other columns). Further, columns 7–12 are linearly independent of all the other n n columns, but is internally linearly dependent as rank( [s ]× Cp )=3. n This is because rank(An,m) min(n,m) and the unitary matrix Cp has full rank [HJ85]. Moreover, columns≤ 13–15 are linearly independ ent of all the n n other columns, but is internally linearly dependent as rank [s ]× Cp = 2. This is because rank(An,k)+ rank(Bk,m) k rank(An,k Bk,m) n − ≤ ≤ min(rank(A), rank(B)) and rank([s ]×)=2 [HJ85]. Hence, the column vectors of Υ that spans the null space will be of the form 70 CHAPTER 3. TIME SYNCHRONIZATION IN GPS-AIDED INS

06,1 (1) Υ1:α,i = ζ , i =1,..., 3 (3.50)  i  03,1   and

0 Υ = 12,1 , i =4, 5 (3.51) 1:α,i ζ(2) i respectively. From the structure of Υ and the description of the set Ωxk , in (3.46) it can be concluded that for the GPS-aided INS (without the extended error model), during an SLCA trajectory with constant acceleration:

• The position and velocity errors are observable.

• The attitude errors and the accelerometer biases can only be determined up to certain linear combinations.

• The gyro biases can only be determined up to certain linear combinations.

Given the observability matrix W in (3.49), the observability matrix for the navigation system with the extended error model Wext, under the same trajectory conditions, can be determined by noting that

Ψm 0 (Ψext)m = α,1 (3.52) 0 1 1,α and thereby

ext ext m m n n Hm (Ψ ) = Hm Ψ v0 + mTs a0 . (3.53)

n n Here, v0 and a0 denote the initial velocity and acceleration of the navigation platform during the trajectory segment, respectively. Hence, the observability matrix, Wext for the system that also estimates the time synchronization error becomes as that given in (3.68) (found in Appendix 3.C n at the end of the chapter). Here, s0 denotes the speciﬁc force correspond- n ing to the acceleration a0 of the platform. From (3.68), it is clear that the ﬁrst six columns and the last column now are linearly dependent, since

T Wext (vn)T (an)T (0 )T 1 = 0 . (3.54) 0 0 9,1 − 3(N+1),1 Therefore, from (3.46), it can be concluded that the time synchronization error is only determinable up to a linear combination with certain position and velocity errors during an SLCA trajectory with constant acceleration. 3.6. CONCLUSIONS 71

Thus, a necessary condition for the time synchronization error to be observable is that the vehicle trajectory includes acceleration or heading changes. Subsequently, let us again consider the case when the movements of the navigation platform can be approximated by an SLCA trajectory. Further, let us assume that this trajectory can be further divided into two subseg- ments that span from time k to k + N and from time k + N +1 to the end of the SLCA trajectory, respectively. During the first subsegment, the n acceleration of the platform is a0 and during the second subsegment, it n n n is a1 . Hence, the corresponding specific force vectors are s0 and s1 , re- ext spectively. Under these conditions the observability matrix W2 at time k becomes as in (3.69) (found in Appendix 3.C at the end of the chapter). Owing to the change in acceleration, the first six and the last columns are no longer linearly dependent. Moreover, as previously stated, they are also linearly independent of all the other columns. Therefore, from (3.46), it can be concluded that the position, velocity, and time synchronization errors under these conditions are observable. Hence, sufficient conditions for the time synchronization error to be observable are that the vehicle trajectory should include acceleration changes.

3.6 Conclusions

The effects of large time synchronization errors in a loosely coupled closed- loop GPS-aided INS with error feedback have been studied in terms of the MSE of the system. These large time synchronization errors may occur in low-cost systems, where precise time synchronization (e.g., using 1-pulse- per-second from the GPS-receiver) is not possible. Expressions for the MSE of the navigation system, given the vehicle trajectory and a state-space model of the error plant of the integrated navigation system, have been derived. In addition, under the assumption of Gaussian-distributed estimates, an MSE expression when the time synchronization error is included in the feedback loop of the navigation filter has also been determined. The use of the two MSE expressions is illustrated by applying them to an example of a one-dimensional, GPS-aided INS with an inherent time synchronization error. The results from the example show that by extending the error model used in the navigation filter to include a state for the timing error as well, the effects of the timing error become negligible as the filter converges. The accuracy of the derived MSE expressions is illustrated by MC simulations of the one-dimensional GPS-aided INS, which shows good agreement with the analytical results calculated from the derived MSE models. Moreover, a practical approach on how to implement the variable delay for time synchronization of the IMU and the GPS-receiver data is also briefly discussed. Furthermore, the proposed time synchronization approach using a frac- 72 CHAPTER 3. TIME SYNCHRONIZATION IN GPS-AIDED INS tional delay filter to interpolate the IMU data was implemented as a part of the software developed in-house for the integration of the GPS and IMU data. The time synchronization was then tested on both simulated and real- world data. When the timing error was not considered by the integration software, the MSE calculated by MC simulations was in agreement with that predicted by the derived MSE expression. However, when the timing error was considered by the integration filter, the derived MSE expressions failed to fully capture the transient behavior of the filter. However, as the filter converged, the agreement improved and the effects of the timing error were removed. In the test with real-world data, with intentionally included timing errors, time synchronization within a few milliseconds was obtained. Further, the differences in arithmetic mean error during the simulated GPS outages indicated that the sensor biases were better estimated when the timing error was considered by the integration filter (i.e., no bias in the estimated forward acceleration). To conclude, large synchronization errors in the sampling instances of the GPS-receiver and the IMU sensor in a GPS-aided INS are observed to seriously degrade the performance of the system. The derived MSE expression provides an efficient way of evaluating the systems performance for a certain timing error and vehicle trajectory. By extending the integration filter to estimate the timing error also, and by feeding back the estimates to synchronize the GPS-receiver and the IMU data by means of a fractional delay filter, almost perfect time synchronization (in the order of a few milliseconds) can be achieved and the effects of the timing error can be cancelled. Hence, the proposed synchronization method can provide a substantial improvement to the accuracy of the navigation solution for loosely coupled GPS-aided INS with large timing errors, whereas, for the system already synchronized within a few milliseconds, the proposed method brings marginal, if any, improvement to the accuracy of the navigation solution. 3.A. EXTENDED STATE SPACE MODEL ERROR COVARIANCE 73

3.A Extended State Space Model Error Covariance

From the derived diﬀerence equation (3.38) for the error in the system with the extended state-space model, the MSE of the system can be written as

Pext = E xe,ext (xe,ext )T fd,k+1 { fd,k+1 fd,k+1 } = Φext Pext (Φext)T + Kext R (Kext)T k fd,k k p,k k (3.55) ext ext T ext ext T + Gk Q (Gk ) + Kp,k Πk (Kk ) Φext Γ (Kext)T Kext ΓT (Φext)T − k k k − p,k k k where

a aT Π = E dext (dext)T = k k E δT 4 (3.56) k { k k } 4 { d,k} and

aT Γ = E xext (dext)T = E xext δT 2 k . (3.57) k { fd,k k } { fd,k d,k} 2 By assuming the state estimates to be Gaussian-distributed, (3.56) and ext ext E ext (3.57) can be rewritten in terms of Pfd,k and xfd,k = xfd,k by using E 4 { } the equalities found in [BD71]. That is, δTd,k in (3.56) can be expressed as { }

E δT 4 =3 E δT 2 2 2 E δT 4. (3.58) { d,k} { d,k} − { d,k} By inserting (3.58) into (3.56), Πk can be written as

a aT Π = k k E δT 4 k 4 { d,k} a aT = k k 3 E δT 2 2 2 E δT 4 (3.59) 4 { d,k} − { d,k} a aT = k k 3 [Pext ]2 2 [xext ]4 . 4 fd,k i,i − fd,k i Further, E δT 2 xext in (3.57) can be rewritten as { d,k f,k }

E δT 2 [xext ] = E δT 2 E [xext ] { d,k fd,k j } { d,k} { fd,k j } +2 E δT E [xext ] δT { d,k} { fd,k j d,k} (3.60) E δT E [xext ] . − { d,k} { fd,k j } 74 CHAPTER 3. TIME SYNCHRONIZATION IN GPS-AIDED INS

ext Equation (3.60) can be expressed in terms of the MSE matrix Pfd,k and ext mean value vector xfd,k as

E 2 ext ext ext ext δTd,k [xfd,k]j = [Pfd,k]i,i [xfd,k]j + 2 [xfd,k]i { } (3.61) ([Pext ] [xext ] [xext ] ). · fd,k j,i − fd,k i fd,k j Thus, the expected value of the vector E δT 2 xext is given by { d,k fd,k}

E 2 ext ext ext ext δTd,k xfd,k = [Pfd,k]i,i xfd,k + [xfd,k]i { } (3.62) ( [Pext ] [xext ] xext ). · fd,k 1:α,i − fd,k i fd,k As given earlier, α is the length of the state vector. Inserting this into (3.57), then Γk can be written as

ext ext ext Γk = [Pfd,k]i,i xfd,k + 2 [xfd,k]i (3.63) aT ( [Pext ] [xext ] xext ) k · fd,k 1:α,i − fd,k i fd,k 2 where xext = Φext xext Kext E dext fd,k k fd,k − p,k { k } 1 (3.64) = Φext xext Kext a [Pext ] k fd,k − 2 p,k k fd,k i,i ext ext ext ext and Φk = (Ψk Kp,k Hk ). Thus, under the assumption of Gaussian- distributed state estimates,− the MSE of the estimated state vector in the navigation system with the augmented state vector, recursively, can be calculated from (3.55), (3.59), (3.63), and (3.64).

3.B INS Error Model

The following is the error model used in the software for integrating GPS and IMU data. Let the error state vector be deﬁned as

n T n T T b T p T T xc = (δp ) (δv ) θ (δf ) (δωip) . (3.65) Here, δpn and δvn denotes the position and velocity error, respectively. The superscript n indicates that the variables are described in the navigation coordinate frame. Further, the vector θ contains the roll, pitch, and heading error. The accelerometer and gyroscope sensor errors in platform p p coordinates are denoted as δf and δωip, respectively. With the above- described state vector, the continuous-time state transition matrix of the system becomes 3.B. INS ERROR MODEL 75

03 I3 03 03 03 0 0 [sn] Cn 0 Ψ = 3 3 × p 3 (3.66) c  0 0 0 0 Cn  3 3 3 3 − p  0 0 0 0 0   6,3 6,3 6,3 6,3 6,3  and the process noise gain matrix 

03 03 n Cp 03 Gc =  n  . (3.67) 03 Cp  0 0   6,3 6,3    n Here, [s ]× denotes the skew-symmetric matrix representation of the spe- ciﬁc force vector sn expressed in the navigation coordinate frame. The n matrix Cp is the coordinate rotation matrix transforming a vector in the platform coordinates into navigation coordinates. 76 CHAPTER 3. TIME SYNCHRONIZATION IN GPS-AIDED INS

3.C Observability Matrices                 ) n 1 (3.69) (3.68) a n 0 n n 0 0 n 0 + n 0 a a a a n 0 a s s s s a s T T T T n 0 ...... NT v N            + ( +2 +3 +4 + s n 0 n 0 0 n 0 n 0 T n n n 0 0 0 n 0 v n 0 a v v v a a a v a + s s s s s n 0 T T T T n 0 . . . v NT v + +4 +3 +2 + n p n 0 n 0 n 0 n 0 n 0 n p v C v v v n p v C × n p C ] × ] n 0 × C n p ] n 0 s n p [ × n 0 n p s C ] 3 3 3 [ 3 ...... s s C n 0 [ 3 0 0 0 × C s T ] 3 s × s [ T × ] n 0 ] 3 T s n 0 s +1 3 3 3 n 0 2 N [ . . . s 4 T s 3 [ 0 0 0 2 N s µ [ 3 − s 3 T µ s − T T − 2 N 4 µ − − − n p n p n p n p C n p C 2 s n p C C 2 3 3 s C n p n p T ...... 2 2 n p s s C 0 0 T 2 s C C T T 2 3 3 +1 s C . . . T 2 2 1 N s s 0 0 3 6 T 2 1 N s µ T T µ T 1 N 6 3 µ ) × ] × ] n 1 × × ] ] × n 0 s ] [ n 0 n 0 s [ n 0 × s s 3 3 ] [ [ . . . s × × 2 N s [ ] ] 0 0 × n 0 2 2 ] s s T n 0 n 0 2 s s + [ n 0 T T s s 3 3 [ [ T ...... s 2 1 N s [ 6 3 0 0 × 2 2 ] s s µ T 2 s n 0 T T T s 1 N 3 6 [ 3 µ 3 3 3 I 3 I I I 1 N I s 3 s s s . . . µ s 0 ( T T T 2 T s 4 3 2 NT T 3 3 3 3 3 3 3 . . . I I I I I I I s 3 3 3 3            I T 3 I I I I s 3 s s s ...... = s 0 T T T T 3 4 2 + 1) NT ext N ( W 3 3 3 3 3 3 3 ...... I I I I I I I                 = ext 2 W Chapter 4

Synchronization by Message Exchanges

4.1 Introduction

Within wireless sensor networks (WSNs), a common notation of time between network nodes is essential for many applications. Therefore, a large amount of research has been carried out in the field of time synchronization within WSNs. The result is a diverse set of time synchronization algorithms and protocols such as: Timing-synch Protocol for Sensor Net- works (TPSN) [GKS03], TSync [DH04], Flooding Time Synchronization Protocol (FTSP) [MKSL04], and Time Synchronization for High Latency (TSHL) [SH06b]. For a detailed description of different time synchronization protocols within WSNs, refer to [SBK05, SSBNS08, SY04]. A variety of error sources set a bound on the achievable performance of a performed clock synchronization. Bounds for the achievable clock synchronization accuracy using different protocols and clock models have been derived under a variety of assumptions [YZC05, NCSS07, LL84, CSQ08a, NSQ08, Tru01]. The study presented in this chapter considers clock synchronization with the aid of a two-way message exchange between a master unit and a plurality of slave units, as illustrated in Fig. 4.1. The setup in Fig. 4.1 is common in a variety of applications, for example offshore submarine positioning, as studied in some detail at the end of the chapter. The slave nodes are assumed to have no common time reference, i.e., they all have their local oscillator providing them with the local time. Thus, it is sufficient to study the two-way synchronization message exchange between the master and an arbitrary slave unit. In the study, the theoretical achievable clock synchronization accuracy between a master and slave node is examined, subject to a two-way clock message exchange, where the synchronization requests are periodically repeated. Further, a low complexity estimator is

77 78 CHAPTER 4. SYNCHRONIZATION BY MESSAGE EXCHANGES

Master

Cm(t)

Slave #1 Slave #2 Slave #N

Cs1 (t) Cs2 (t) CsN (t)

Figure 4.1: A WSN comprising the master node and a plurality of N slave nodes.

derived for the estimation problem at hand, based on the concept of maximum likelihood, and shown to have near-optimal performance in terms of accuracy and precision. The applicability of the proposed method is studied in a positioning scenario in an offshore underwater application. The study is conducted under a set of assumptions. The most important assumptions being: the clocks can be described by linear models (including affine mappings), the (wireless) communication channel is symmetric, and the communication latency within the channel can be described as Gaussian. These conditions are in many ways similar to the conditions used in [NCSS07], in which the practical implications of the mathematical assumptions were justified. In [NCSS07], the Cramér-Rao bound (CRB) for the accuracy at which the clock skew and initial clock offset can be estimated was derived. In my approach, the theoretical accuracy of the estimation problem is studied by using a slightly different perspective on what is to be considered as observations obtained from the two-way message exchange, together with a few weak additional assumptions. The results include an asymptotic CRB unveiling that the lower bound for the estimation of the clock skew is inversely proportional to the cube of the number of message exchanges, and the lower bound for the estimation of the initial clock offset is inversely proportional to the number of message exchanges. A point worth noting is that in contrast to the results in [NCSS07], where the CRB depends on the actual realization of the message latency times, the CRB derived within this study only depends on nonstochastic parameters. This makes the derived CRB a versatile tool for accessing the achievable system performance at an early design stage. The derived CRB is mapped into a lower bound on the mean square error (MSE) of the clock offset. Moreover, by constructing an ”artificial” observation through a linear combination of 4.2. PROBLEM DESCRIPTION 79 the original observations, a low-complexity, approximate maximum likelihood estimator (AMLE) is derived and its performance investigated.

4.2 Problem Description

Let the functions Cm(τ) and Cs(τ) describe the clock states as a function of the time τ for the master and slave node, respectively. The clock oﬀset at time τ between the two clocks is then deﬁned as

∆(τ)= C (τ) C (τ). (4.1) s − m The aim of the following analysis is to provide a lower bound for the error variance at which the clock oﬀset can be estimated using a two-way message exchange between the nodes and methods for estimation. The bound is applicable under the following two assumptions regarding the clocks of the nodes:

A.1 The clock resolution of the nodes is considered inﬁnite, i.e., the clocks Cm(τ) and Cs(τ) are modelled by continuous time functions. A.2 The clocks are assumed to have constant skews and initial oﬀsets, but no other errors.

In practice, the clocks revolve with discrete time steps proportional to the inverse of the frequency of the oscillator driving the clock register. However, assumption A.1 is valid if the step size of the clock is small in comparison with the time duration of the events that the clock is measuring. Refer to [Sad06] for details on the practical eﬀect that the limited clock resolution imposes on the clock synchronization problem. The validity of A.2 can be related to a local time interval for which the assumptions hold true, i.e., the model errors should be small when compared with the time frame spanned by the employed clock skew estimation algorithm. Next, let the time indicated by the master clock Cm(τ) be the local 1 reference time t of the system, i.e., t Cm(τ). Then, given assumptions A.1 and A.2 the clock state of the slave≡ node as a function of the reference time t may be modeled as

Cs(t)=(1+ α) t + β. (4.2) Here, α denotes the clock skew relative to the master node clock. Further, β is the oﬀset between the slave node clock and the master node clock at

1Note that the duration of a ”second” in the local reference time diﬀers from the SI-unit deﬁnition of a second. However, for simplicity, the author allow a slight misuse of units when specifying the duration of time events. 80 CHAPTER 4. SYNCHRONIZATION BY MESSAGE EXCHANGES

Clock oﬀset as a function of time. 0 Clock oﬀset

] −0.05 s

[ Linear ﬁt −0.1

−0.15

Clock oﬀset −0.2

−0.25 0 500 1000 1500 2000 2500 3000 3500 Time [s] x 10−5 Residual from linear ﬁt. 1.5

1 ] s [ 0.5

0 Residual −0.5

−1 0 500 1000 1500 2000 2500 3000 3500 Time [s]

Figure 4.2: Clock offset between the clock of an AVR ATmega 8 µ-controller with a 3.6854 Mhz quartz crystal controlled clock oscillator, and the clock of a GlobalSat EM-411 GPS-receiver module, and the residual of a linear fit to the clock offset. The clock skew is in the order of 60 [µs/s]. − t =0. In the sequel, β is referred to as the initial clock offset. Clock models similar to (4.2) are used in, e.g., [SY04, NCSS07, Sad06]. The offset (as a function of the local reference time t between the master and slave node clocks can be described as

∆(t)= C (t) C (t)= αt + β. (4.3) s − m With reference to (4.3), determining the clock offset is under assumptions A.1 and A.2 equivalent to estimating the clock skew α and the initial clock offset β of the slave node clock. In Figure 4.2, the clock offset between a clock of a micro controller and the clock of a GPS-receiver during one hour is shown together with the residual of a least-squares fit of the model in (4.3) to the clock offset. Even though the correlation in the residual indicates that the model does not resemble the full picture of the clock’s behavior, the model describes the main characteristics and justifies the use of the employed model for the forthcoming analysis. For a description of more refined clock models and their characteristics, refer to [All87, All84, Bre97]. 4.3. THE TWO-WAY MESSAGE EXCHANGE MODEL 81

(2) (3) Cs(tn ) Cs(tn ) Cs(t)

Request

t Cm(τ) ≡ (1) (2) Tn ǫn Tn (1) (2) (3) (4) tn tn tn tn

Figure 4.3: Illustration of the two-way message exchange model.

4.3 The Two-Way Message Exchange Model

In the two-way message exchange model for time synchronization, as illus- (1) trated in Figure 4.3, the master node starts at time tn by sending the nth message with a request concerning the current clock state of the slave (1) node. After a transmission delay Tn , the slave receives the request at time (2) (2) tn and reads its own clock state Cs(tn ). After ǫn seconds, i.e., at time (3) (2) tn , the slave node replies by returning the two clock states Cs(tn ) and (3) (2) Cs(tn ) to the master node. After a delay Tn , the master node receives (4) the reply and reads its own clock state tn .

4.3.1 Prerequisites Let the following assumptions regarding the communication between the nodes hold true:

A.3 The clock state can be communicated from the slave node to the master node with inﬁnite resolution.

Then, the observations available at the master node after each message (1) exchange starting at tn are given by

(1) (2) zn = Cs(tn ), (4.4) (2) (3) zn = Cs(tn ), (4.5) 82 CHAPTER 4. SYNCHRONIZATION BY MESSAGE EXCHANGES and

(3) (4) zn = tn . (4.6) Having received the observations, the master node is to estimate the skew α and initial oﬀset β. The accuracy at which this may be done depends on the probability distribution function (PDF) of the observations and the prior knowledge about the shape parameters of the distribution. In the two-way message exchange model, there are three nondeterministic quantities, with corresponding PDFs, which determine the probability distribution of the observations. First, there are the two end-to-end mes- (1) (2) sage latencies Tn and Tn , which in addition to the actual propagation delay across the communication channel, also account for the time needed to prepare and process the message, access the channel, and interpret the message together with reading the clock state at the receiving node. Refer to [GKS03, MAP07, YZC05, EGE02] for a discussion on the causes and characteristics of the latency sources mentioned above. Then, there is also the slave node reply delay ǫn, which depends on the scheduling and current process load of the slave node. Note that the introduction of ǫn, into the model of the two-way message exchange (when compared with the model used in [NCSS07]) is the key step in deriving a CRB only dependent on predeﬁned system parameters. Next, introduce the following four assumptions regarding the PDFs of the quantities discussed earlier:

A.4 The communication channel between the master and slave node is sym- (1) (2) metric, and the message delays Tn and Tn are ruled by the same distribution.

A.5 The combined uncertainty of the nondeterministic quantities included in each message delay may be modeled as Gaussian-distributed, i.e., (i) Tn (µT , σT ) for i =1, 2, where the mean µT and standard deviation∼σ N fulﬁll the condition µ σ . T T ≫ T

A.6 The delay ǫn is Gaussian-distributed with mean µǫ and standard deviation σ , where µ σ . ǫ ǫ ≫ ǫ

(1) (2) A.7 The delays Tn , Tn and ǫn are mutually independent stochastic variables and i.i.d. with respect to n.

The condition on the mean to be much larger than the standard deviation (1) (2) in assumptions A.5 and A.6 is because the probability for Tn , Tn , and 4.3. THE TWO-WAY MESSAGE EXCHANGE MODEL 83

ǫn to be negative should be sufficiently close to zero. The assumption may seem to be imprecise, due to a nonzero probability for negative delays. For the practical scenarios considered, the assumption is a natural trade-off between reasonability and our efforts to tackle the mathematical issues. The assumption regarding Gaussian distribution is partly arguable and other common distributions used to model the message latencies are the Gamma distribution (having the chi-squared, exponential, and Erlang distribution as special cases [EHP00]), and the log-normal distribution [CSQ08b]. In [YZC05], both uniformly and exponentially distributed message delays are considered in the analysis of the clock synchronization accuracy within a WSN. However, the Gaussian assumption may be motivated by the central limit theorem, if the delays are viewed to be the addition of numerous independent delays originating from the channel, scheduling, processing, etc. [LKS08, NSQ08]. Gaussian-distributed message delays are motivated and employed, for example in [vGR03, XK09, GBH04, CLYX06]. One may note in [GvCCS05] that the end-to-end message delays in the communication between Micro2 Motes were measured. The statistics of the observed data fitted, at a 99.97% confidence interval, a Gaussian distribution with a mean of 762 µs and standard deviation of 2.82 µs. Accordingly, the Gaus- sian distribution is indeed relevant in several applications, and the above assumptions are justified.

4.3.2 Time Stamp Observations Next, a model for the observations is derived. Rewriting the observations (1) (1) (2) (4.4), (4.5), and (4.6) in terms of tn , Tn , Tn , and ǫn yield

(1) (1) (1) zn = Cs(tn + Tn ) (4.7)

(2) (1) (1) zn = Cs(tn + ǫn + Tn ) (4.8) and

(3) (1) (1) (2) zn = tn + ǫn + Tn + Tn . (4.9)

The clock model in (4.2) implies that the observations are linear combinations of Gaussian-distributed random variables. Let

T (1) (2) (3) zn = zn zn zn , (4.10) h i where ( )T denotes the transpose operation. Then, the vector of observations z · (u (θ), R(θ)), with the mean given by n ∼ N n 84 CHAPTER 4. SYNCHRONIZATION BY MESSAGE EXCHANGES

(1) Cs(tn + µT ) (1) un(θ)= E zn = C (t + µ + µ ) (4.11) { }  s n ǫ T  t(1) + µ +2 µ  n ǫ T  The covariance matrix is given by  

R(θ)= COV z = ΨΛΨT (4.12) { n} where Ψ and Λ read (1 + α) 0 0 Ψ = (1 + α) (1+ α) 0 (4.13)  1 11    and 2 σT 0 0 2 Λ = 0 σǫ 0 . (4.14)  2  0 0 σT In (4.11) and (4.12), the E  and COV denote the expectation and covariance operation, respectively.{·} Further,{·} the structure of (4.14) follows from the independent assumption A.7. Moreover, the parameter vector θ in the deﬁnition of the mean (4.11) and covariance (4.12) is used to indicate the explicit dependency of these parameters on the PDF of the observations, which in the Gaussian case is fully determined by the mean 2 2 T and covariance, e.g., θ = α β µT σT µǫ σǫ . With the model of the observations stated, the lower bound on estimation accuracy is derived below.

4.4 Cramér-Rao Bound

Let ym be a vector of m observations with the joint PDF fy(ym; θ), parameterized by the vector θ. The CRB then provides a lower bound on the mean square error for any unbiased estimator θ(ym) and is defined as [Kay93b, Tre68] b E (θ θˆ(y ))(θ θˆ(y ))T I (θ)−1 0 (4.15) − m − m − m where the operatorn denotes that the left-hando side is positive semidefi- nite. Further, Im(θ) is the Fisher information matrix (FIM) defined as

∂2 [I (θ)] , E ln fy(y ; θ) . (4.16) m i,j − ∂θ ∂θ m i j Here, [ ]i,j denotes the matrix element of the ith row and the jth column. To calculate· the CRB for the parameters in the two-way message exchange 4.4. CRAMÉR-RAO BOUND 85

3 m model, let the vector ym R contain the observations obtained from m executions of the message∈ exchange procedure. That is

T T T ym = z0 . . . zm−1 (4.17) where the single-message observation zn was given in (4.10). Next, assumption A.7 implies that the observations obtained from the diﬀerent executions are independent. Thus, it holds that

m−1

fy(ym; θ)= fzn (zn; θ), (4.18) n=0 Y where fzn (zn; θ) is the Gaussian PDF of the observations at the nth executions of the message exchange procedure. Inserting (4.18) into the expression for the FIM in (4.16) yields

m−1 [Im(θ)]i,j = [Jn(θ)]i,j , (4.19) n=0 X where

∂2 [J (θ)] = E ln fz (z ; θ) . (4.20) n i,j − ∂θ ∂θ n n i j Further, since the observations are obeying a Gaussian distribution it holds that [Kay93b]

∂u (θ) T ∂u (θ) [J (θ)] = n R−1(θ) n n i,j ∂θ ∂θ i j 1 ∂R(θ) ∂R(θ) + tr R−1(θ) R−1(θ) . (4.21) 2 ∂θ ∂θ i j where un(θ) and R(θ) are explicitly given in (4.11) and (4.12), respectively. Further, tr( ) denotes the trace operation. Inserting (4.21) into (4.19), evaluating the· ﬁnite sum yields an expression for the FIM. The lower bound on accuracy then follows by a matrix inversion.

4.4.1 CRB for Four Diﬀerent Scenarios The objective of the two-way message exchange for clock synchronization is to estimate the clock skew α and the initial clock oﬀset β. However, depending on the prior knowledge about the means and variances of the delays in the message exchange, nuisance parameters must be estimated as well. The following four cases will be examined in some detail: 86 CHAPTER 4. SYNCHRONIZATION BY MESSAGE EXCHANGES

C.1 Known channel and processing delay. The ﬁrst- and second-order mo- (1) (2) T ment of the delays Tn , Tn , and ǫn are known, i.e., θ = α β .

C.2 Known processing delay. The mean and variance of ǫn are known but (1) (2) mean and variance of the message delays Tn and Tn are unknown, 2 T i.e., θ = α β µT σT . (1) (2) C.3 Known channel delay. The mean and variance of Tn and Tn are known, but mean and variance of the processing delays ǫn are un- 2 T known, i.e., θ = α β µǫ σǫ . C.4 No prior knowledge. The means and variances of all delays are un- 2 2 T known, i.e., θ = α β µT σT µǫ σǫ . Straightforward but tedious calculations reported in [SH09b], evaluating (4.21) for case C.1 yields

2 (t(1) + µ )2 2 (t(1) + µ )µ [J (θ)] = n T + n T ǫ n 1,1 (1 + α)2σ2 (1 + α)2σ2 T T (4.22) 2 2 2 2 µǫ + σǫ 5σǫ + µǫ + 2 2 + 2 2 , (1 + α) σT (1 + α) σǫ

(1) 2 (tn + µT )+ µǫ [Jn(θ)]1,2 = [Jn(θ)]2,1 = 2 2 , (4.23) (1 + α) σT and

2 [Jn(θ)]2,2 = 2 2 . (4.24) (1 + α) σT (1) Here, (4.22) and (4.23) depend on the known starting time tn of the nth message exchange. To proceed, introduce the following assumption regarding the starting time of each message exchange:

A.8 The master node sends a request to the slave node regarding the cur- (1) rent clock state with a periodicity of Ts seconds, i.e., tn = nTs for n =0,...,m 1. − 4.4. CRAMÉR-RAO BOUND 87

Note that for the two-way message exchange model to be valid, Ts must be chosen so large that the statistics of delays are unaﬀected by the communication and processor load induced by the synchronization message exchange itself. Otherwise, a more sophisticated model for the message exchange is required. With assumption A.8, the four elements of the FIM for case C.1 become

2 Ts m (2 m 1)(m 1) [Im(θ)]1,1 = − 2 2 − 3(1+ α) σT Ts m (m 1)(2 µT + µǫ) + − 2 2 (1 + α) σT 2 2 2 2 (4.25) m 2 µ +2 µT µǫ + µ +5σ + σ + T ǫ T ǫ (1 + α)2σ2 T 2 m µǫ + 2 2 , (1 + α) σǫ

m(µǫ +2 µT + Ts (m 1)) [Im(θ)]1,2 = [Im(θ)]2,1 = 2 2 − , (4.26) (1 + α) σT and

2 m [Im(θ)]2,2 = 2 2 . (4.27) (1 + α) σT For cases C.2–C.4, the additional nonzero elements of the corresponding FIMs are reported in Appendix 4.A. The final step in calculating the CRBs for the different cases are the inversion of the four different FIMs. The inversions may be done analytically, for example using computational software such as Mathematica. The resulting expressions are extensive, and therefore, only the parts related to the bound on the variance of the clock skew α and the initial clock offset β are presented. The matrix inverses as a whole are reported in [SH09b]. Let i (for i = 1,..., 4) denote the case under consideration, then

2 2 2 −1 6(1+ α) σT σǫ Im (θ) 1,1 = 2 2 2 , (4.28) σǫ (m 1) mTs + ci(m) −

−1 −1 Im (θ) 1,2 = Im (θ) 2,1 2 2 2 (4.29) 3(1+α) σ σ ((m 1) Ts +2 µT + µǫ) = T ǫ − , − σ2(m2 1) mT 2 + c (m) ǫ − s i 88 CHAPTER 4. SYNCHRONIZATION BY MESSAGE EXCHANGES and

2 2 2 2 (1 + α) σ σ (2 m 3) mT + di(m) I−1(θ) = T ǫ − s . (4.30) m 2,2 σ2(m2 1) mT 2 + c (m) ǫ − s i In (4.28)–(4.30), the results for the diﬀerent scenarios C.1–C.4 are distinguished by the ci(m) and di(m) terms, as reported in Appendix 4.B. The bounds on the clock skew α and the oﬀset β are given (by the upper left) −1 part of the inverse of the FIMs, i.e., [Im (θ)]1:2,1:2. When the number of message exchanges increases, the CRB tends toward its asymptotic values, as derived below.

4.4.2 Asymptotic CRB

Since ci(m) and di(m) (for i =1,..., 4) grow linearly in m, for large values of m they become negligible compared to the other terms in (4.28)–(4.30). Hence, as m , → ∞ 6 3 3 2 2 [I−1(θ)] (1 + α)2σ2 m Ts −m Ts (4.31) m 1:2,1:2 ≃ T  3 2  2  −m Ts m  where denotes an equality where only the dominating terms have been retained.≃

4.4.3 CRB Illustrations In Figures 4.4–4.7, the CRBs for the estimation of the skew α and initial oﬀset β for cases C.1–C.4 are illustrated by employing the parameter values given in Table 4.1. Further, the common asymptotic CRB given by (4.31) is shown as reference in Figures 4.4–4.7. From the results displayed in Figures 4.4–4.7, one may conclude that:

• The bounds quickly converge toward their asymptotic values. In practice, the performance behavior is accurately predicted by the asymptotic bound (4.31) after a few message exchanges. • Excluding the case of a single message exchange, the result for cases C.1–C.4 are practically identical for m 2, implying that prior knowledge on channel and processing delays≥ do not improve the estimation accuracy. For a single message exchange, on the other hand, prior knowledge about the delays may play a signiﬁcant role on the achievable accuracy. • Case C.3 corresponds to the scenario in [NCSS07] with an additional assumption regarding the slave nodes reply delay ǫn. Accordingly, 4.4. CRAMÉR-RAO BOUND 89

CRB for α given parameterizations case C.1. 5 10 CRB case C.1 ] Asymptotical CRB µs/s MSE [ √

0 10 0 1 2 10 10 10 Number of message exchanges m CRB for α given parameterizations case C.2. 5 10 CRB case C.2 ] Asymptotical CRB µs/s MSE [ √

0 10 0 1 2 10 10 10 Number of message exchanges m

Figure 4.4: CRBs for α given parametrization case C.1 and C.2 together with the asymptotical CRB for α.

it is relevant to compare the arithmetic mean of the CRB derived in [NCSS07], with the closed-form results derived herein. The comparison is given in Figure 4.8, from which one may note an excellent agreement.

The bounds on the parameters α and β are often not as interesting as the bound on the actual clock oﬀset and how well it may be predicted, which can be found by a proper variable transformation.

4.4.4 A Lower Bound on the Clock Offset The CRB for the clock parameters can be used to derive a lower bound on MSE for the clock offset estimation. Let θˆ(ym) be an unbiased estimator of the unknown parameters, i.e., clock skew, initial clock offset, etc., based on the observations obtained from m message exchanges. Note that parameter estimate m will be available at some time instant t such that t tm, where (1) ≥ tm = (m 1) Ts + ηm = tm−1 + ηm and ηm denote the sum of the round- trip delays− for the mth message exchange, including the processing time needed to calculate the estimate. Thus, the mth update of the clock offset 90 CHAPTER 4. SYNCHRONIZATION BY MESSAGE EXCHANGES

CRB for α given parameterizations case C.3. 5 10 CRB case C.3 ] Asymptotical CRB µs/s MSE [ √

0 10 0 1 2 10 10 10 Number of message exchanges m CRB for α given parameterizations case C.4. 6 10 CRB case C.4 ] 4 Asymptotical CRB 10 µs/s

MSE [ 10 √

0 10 0 1 2 10 10 10 Number of message exchanges m

Figure 4.5: CRBs for α given parametrization case C.3 and C.4 together with the asymptotical CRB for α.

T estimate is not available before tm. Next, introduce x(t)= t 1 and

T Πm = E (θ θˆ(ym))(θ θˆ(ym)) . (4.32) − − 1:2,1:2 h n oi Then, for the model of the clock oﬀset in (4.3), the MSE of the clock oﬀset is given by

MSE ∆(t) = x(t)T Π x(t), t [t ,t ) : m N. (4.33) { } m ∀ ∈ m m+1 ∈ −1 From the deﬁnition of the CRB in (4.15), it holds that Πm [Im (θ)]1:2,1:2 0, and thus is the MSE of the clock oﬀset lower bounded− by

MSE ∆(t) g(t) (4.34) { }≥ where

g(t)= g (t), t [t ,t ) : m N (4.35) m ∀ ∈ m m+1 ∈ and 4.4. CRAMÉR-RAO BOUND 91

CRB for β given parameterizations case C.1. 2 10 CRB case C.1

] Asymptotical CRB µs 1 10 MSE [ √

0 10 0 1 2 10 10 10 Number of message exchanges m CRB for β given parameterizations case C.2. 2 10 CRB case C.2

] Asymptotical CRB µs 1 10 MSE [ √

0 10 0 1 2 10 10 10 Number of message exchanges m

Figure 4.6: CRBs for β given parametrization case C.1 and C.2 together with the asymptotical CRB for β.

T −1 gm(t)= x(t) [Im (θ)]1:2,1:2 x(t). (4.36) The bound g(t) is a stochastic function, since it is dependent on a set of stochastic variables η , η ,... , thus making it diﬃcult to evaluate. { 1 2 } However, since ηm is strictly positive this fact may be circumvented by relaxing the bound slightly. To do so, note that

g (ξ + t(1) ) g (t(1) ), ξ 0 : m Ω , (4.37) m m−1 ≥ m m−1 ∀ ≥ ∈ m where Ω = m m N and m (2 µ + µ )/T +1 , and that m { | ∈ ≥ T ǫ s } g (t) g (t), t. (4.38) m ≥ m+1 ∀ For proofs of (4.37) and (4.38), we refer to Appendix. 4.C. Accordingly, for all t such that t [t(1) ,t ) : m Ω , it holds that ∈ m−1 m ∈ m g(t)= g (t) g (t) g (t(1) ) (4.39) m−1 ≥ m ≥ m m−1 (1) and t [t ,tm ) : m Ω it holds that ∀ ∈ m ∈ m g(t)= g (t) g (t(1) ). (4.40) m ≥ m m−1 92 CHAPTER 4. SYNCHRONIZATION BY MESSAGE EXCHANGES

CRB for β given parameterizations case C.3. 2 10 CRB case C.3

] Asymptotical CRB µs 1 10 MSE [ √

0 10 0 1 2 10 10 10 Number of message exchanges m CRB for β given parameterizations case C.4. 3 10 CRB case C.4

] 2 Asymptotical CRB 10 µs

1 MSE [ 10 √

0 10 0 1 2 10 10 10 Number of message exchanges m

Figure 4.7: CRBs for β given parametrization case C.3 and C.4 together with the asymptotical CRB for β.

Table 4.1: Parameter values used in simulations and numerical examples. Parameter Value Unit α 50 [µs/s] µT 1 [ms] σT 10 [µs] µǫ 0.5 [ms] σǫ 10 [µs] Ts 10 [ms]

Combining (4.39) and (4.40), and their corresponding intervals gives that

MSE ∆(t) g (t(1) ), t [t(1) ,t(1)) : m Ω . (4.41) { }≥ m m−1 ∀ ∈ m−1 m ∈ m The bound in (4.41) depends only on nonstochastic quantities and can be numerically evaluated given the diﬀerent parameterization cases. See Figure 4.10 for an illustration. An asymptotic bound for the clock oﬀset may be found by substitut- −1 ing the large m approximation for [Im ]1:2,1:2, given by (4.31), into the 4.5. APPROXIMATIVE MAXIMUM LIKELIHOOD ESTIMATOR 93

Comparison between CRBs for α. 105 Arithmetic mean of CRB in [NCSS07]

] CRB case C.3 µs/s MSE [ √

100 100 101 102

Comparison between CRBs for β. 102 Arithmetic mean of CRB in [NCSS07] CRB case C.3 ] µs 101 MSE [ √

100 100 101 102 Number of message exchanges m

Figure 4.8: Comparison between the CRB for case C.3 and the arithmetic mean of the CRB proposed in [NCSS07]. deﬁnition of g (t), given by (4.36). It then holds that as m m → ∞ 2(1+ α)2 σ2 g (t(1) ) T (4.42) m m−1 ≃ m Hence, for large t, the bound for MSE ∆(t) is given by { } 2(1+ α)2 σ2 MSE ∆(t) g(t)= T (4.43) { }≥ 1+ t ⌊ Ts ⌋ One may note that the approximate bound in (4.43) is inversely proportional to the absolute time duration t normalized by the message update- 2 rate 1/Ts, the clock skew α, and the channel spread σT . In particular, it is independent of the average channel delays µT , slave node average delay 2 µǫ, and spread σǫ , and initial oﬀset β.

4.5 Approximative Maximum Likelihood Estimator

For many estimation problems, it is desirable to find the maximum likelihood estimator (MLE) because of its asymptotical statistical efficiency. In [NCSS07], the MLE for the clock skew α and the initial clock offset β is 94 CHAPTER 4. SYNCHRONIZATION BY MESSAGE EXCHANGES derived assuming the statistics of the delays to be known. However, when the statistics of the delays are unknown the estimation problem becomes highly nonlinear [NCSS07]. Therefore, an AMLE for the clock skew α and the initial clock offset β is proposed. The proposed AMLE is independent of the nuisance parameters (i.e., the delays statistics), and has a complexity equivalent to solving a linear least squares problem. Further, it is shown that asymptotically the AMLE is nearly optimal, i.e., it nearly obtains the CRB.

4.5.1 Derivation To derive the AMLE, introduce the artiﬁcial observation

t(1) z˘ = hT z n (4.44) n n − 2

1 T where h = 2 1 1 1 . Since z˘n is a linear combination of Gaussian- distributed variables it− will also be Gaussian-distributed. Further, the mean of z˘n is given by

t(1) E z˘ = hT u (θ) n = αt(1) + β + κ (4.45) { n} n − 2 n 1 and the variance by

σ2 σ2 = COV z˘ = hT R h = T + κ . (4.46) z˘ { n} 2 2 where κ1 and κ2 are given by µ κ = α µ + ǫ (4.47) 1 T 2 and α σ2 κ = α (1 + α) σ2 + ǫ . (4.48) 2 T 4 One should note that κ1 O(α) and κ2 O(α), where O(x) denotes terms such that O(x)/x is∈ bounded when ∈x 0. Thus, since the clock skew α in general| is in the| order of 10−5 or less→ for a quartz crystal oscillator [SFCC08], the artiﬁcial observation (4.44) can be approximately described as

z˘ αt(1) + β + w , (4.49) n ≈ n n 2 where wn (0, σT /2) and independent in between diﬀerent time instants. For the approximate∼ N observation model (4.49), it holds that the MLE for 4.5. APPROXIMATIVE MAXIMUM LIKELIHOOD ESTIMATOR 95 the clock skew α and initial clock oﬀset β is given by the linear least squares estimator. Hence, introduce

T y˘m = z˘0 . . . z˘m−1 , T 1m = 1 . . . 1 ,

T (4.50) (1) (1) tm = t0 ... tm−1 ,

h T i θ′ = α β , and

Ψ = tm 1m . (4.51) The MLE estimator of θ given m observations originating from the approximated artiﬁcial observation model (4.49) is then given by [Kay93b]

′ T −1 T θ = (Ψ Ψ) Ψ y˘m. (4.52) Since the estimator in (4.52) is derived based on an approximation of the true artiﬁcial observationb in (4.44), the estimator is referred to as the AMLE. (For an interesting comparison with respect to the linear trend MLE in the case of wn being exponentially distributed, refer to [Tru01].) Before comparing the AMLE to the CRB derived in Section 4.4, it should be noted that the estimate of the AMLE is biased, as analyzed later.

4.5.2 Performance Denote the bias by δθ′, that is

′ ′ ′ T −1 T 0 δθ = E θ θ = κ1 (Ψ Ψ) Ψ 1m = . (4.53) { }− κ1 Hence, the AMLE doesb not belong to the class of estimators for which the unbiased CRB provides a lower bound on the MSE. For the AMLE, the MSE matrix of the estimated parameter vector is given by

Π = E (θ′ θ′)(θ′ θ′)T m { − − } = σ2(ΨT Ψ)−1 + δθ′ δθ′T z˘ b b 2 12 −6 σ 3 2 2 T (m −m) Ts (m +m) Ts (4.54) = + κ2 −6 4 m−2 2 2 2 " (m +m) Ts m +m # 0 0 + . 0 κ2 1 96 CHAPTER 4. SYNCHRONIZATION BY MESSAGE EXCHANGES

MSE of the AMLE estimated clock skew. 5 10 AMLE ] CRB case C.1 µs/s MSE [ √

0 10 0 1 2 10 10 10 Number of message exchanges m MSE of the AMLE estimated initial clock oﬀset. 2 10 AMLE

] CRB case C.1 µs 1 10 MSE [ √

0 10 0 1 2 10 10 10 Number of message exchanges m

Figure 4.9: MSE of the AMLE estimated clock skew α and initial oﬀset β together with the CRB for parameterization case C.1.

This may be further simpliﬁed to

6 −3 3 2 2 2 2 (m −m) Ts (m +m) Ts 2 Πm =(1+ α) σT −3 2 m−1 + O(α ). (4.55) 2 2 " (m +m) Ts m +m # For a large set of observations, i.e., m , the MSE of the AMLE becomes approximately → ∞

Π [I−1(θ)] + O(α2) (4.56) m ≈ m 1:2,1:2 In Figure 4.9, the empirical MSE of the AMLE, obtained from a Monte Carlo simulation of the two-way message exchange is shown. Further, in Figure 4.10, the MSE of the estimated clock offset based on the skew α and initial offset β estimated by the AMLE, together with the lower bound on MSE of the clock offset based on the CRB for parameterization case C.1, is shown. The system parameters used in the simulation are found in Table 4.1. With reference to Figure 4.9, note that the MSE of initial clock offset estimate of the AMLE approaches the CRB from below. This phenomenon, known as statistical superefficiency, can be explained by the fact that the AMLE trades bias against a reduction in variance, causing 4.6. SIMULATION STUDY 97

MSE of the estimated clock oﬀset.

MSE of the AMLE calculated clock oﬀset Lower bound on the MSE of the clock oﬀset

1 10 ] µs MSE [ √

0 10 0 0.2 0.4 0.6 0.8 1 Time [s]

Figure 4.10: MSE of the estimated clock offset with skew α and initial offset β estimated by the AMLE together with the lower bound on MSE of the clock offset based on the CRB for parameterization case C.1.

a reduction in MSE. For more details regarding statistically supereﬃcient estimators, refer to [SO96, SM90].

4.6 Simulation Study

Consider the following oﬀshore engineering scenario. An exploration vessel arrives at an oil drilling site with the intent to carry out work at the seabed using a remotely operated vehicle (ROV). On the seabed surrounding the drill site, several sensor nodes for environmental monitoring have been deployed and their locations determined and mapped. The sensor nodes act as slaves in the WSN used to position the ROV. Once at the intended operation site, the vessel anchors up to the seabed and puts the ROV into the sea. Simultaneously, the exploration vessel sends out a com- mand to the nearby sensor nodes to get into ”positioning mode”, i.e., to listen for the sound bursts sent out from the ROV and record their arrival times and relay the information to the vessel. The vessel then uses the time of arrival (TOA) information to determine the position of the ROV, and to calibrate its inertial navigation system. Refer to [CWCE06] for a 98 CHAPTER 4. SYNCHRONIZATION BY MESSAGE EXCHANGES

Illustration of the simulation scenario.

20 Vessel 0

−20

−40 [m] z −60 ROV Node −80

−100 500 400 500 300 400 200 300 200 100 100 0 0 y [m] x [m]

Figure 4.11: Illustration of the oﬀshore engineering scenario used in the simulation study.

survey on localization in underwater sensor networks. However, positioning the ROV using the TOA information requires the clocks in the sensor network to be synchronized or the clock oﬀsets of the nodes to be known by the master node located at the vessel. To access this clock information the master node uses the two-way message exchange procedure described in Section 4.3, in combination with the AMLE for the clock parameter estimation in Section 4.5. With the above-described scenario as a basis, a Monte-Carlo simulation study was conducted to illustrate:

• The applicability of the two-way message exchange procedure together with the AMLE to improve the accuracy of a WSN-based positioning system.

• How the bound on the MSE of the clock oﬀset can be used to lower bound the accuracy of the positioning system.

The study was conducted under the following assumptions. Four nodes, each in the corner of a square with the side 500 m, on the sea ﬂoor 100 m below the average water surface level, were used in the positioning. See 4.6. SIMULATION STUDY 99

Figure 4.11 for an illustration of the scenario under study. The ship was anchored so that on average it was located in the middle of the square, but due to waves its position randomly deviated according to a zero-mean 2 2 Gaussian distribution with covariance 4 I3 m ; adding to the uncertainties in the transmission delays. Moreover,· the ROV was moving in a circle with a radius of 100 m and with its center at (250, 250, 90) and at a velocity of 2 m/s (a typical speed for ROVs [XHQHCD08]). The ROV was equipped with a depth gauge and thus only the location in the horizontal plan was unknown. The slave nodes on the sea bed and the master node on the vessel communicated using acoustic signaling at a bit rate of 1250 bits/s and with messages of a length of 56 bits, whereas the vessel and the ROV were communicating using a wired connection, to allow high data throughput for video information, etc. Refer to [APM05] for a discussion on underwater acoustic sensor networks and typical bit rates versus communication distances. Further, the nodes were assumed to be able to time stamp the message arrivals with an error drawn from a zero-mean Gaussian distribution with a standard deviation proportional to the reciprocal of the bit rate. A study on underwater acoustic localization with many similarities to the above-described scenario is [XHQHCD08], but where a more advanced signal propagation model was employed. Moreover, the nodes’ response time to a ”current clock state” request was modeled as Gaussian distributed with a mean time of 50 ms and a standard deviation of 1 ms. The propagation speed of the sound in the water was assumed to be 1500 m/s. The bound on the MSE of the clock oﬀset, derived in Section 4.4.4, can be mapped into a bound on the MSE of the position error of the ROV. To do so, let i (for i = 1,..., 4) denote the sensor node under consideration. The clock state of the node, when receiving the positioning pulse sent from the ROV, can then be described as

i i i Csi (trx)= trx + ∆i(trx) i i i = h (p)+ t + ∆i(ttx)+ w tx (4.57) + α (hi(p)+ wi) hi(p)+ t + ∆ (t )+ wi ≈ tx i tx where ri p hi(p)= k − k. (4.58) vs i i Here, ttx and trx denote transmission and receiving time of the position pulse, respectively. Further, wi denotes the stochastic error in the time stamping of the received burst, and is assumed zero-mean Gaussian with 2 i variance σw. Moreover, r and p denotes the position of the ith node 100 CHAPTER 4. SYNCHRONIZATION BY MESSAGE EXCHANGES

MSE of the horizontal position error of the ROV. 3.5 Monte Carlo Simulation. CRB based bound. Monte Carlo Simulation, 360 message exchanges. 3 CRB based bound, 360 message exchanges. Position error with perfect clock synchronization.

] 2.5 m MSE [

√ 2

1.5

1 500 1000 1500 2000 2500 3000 3500 Time [s]

Figure 4.12: Monte Carlo and CRB based MSE of the horizontal position error of the ROV in the simulation study.

and the ROV, respectively. The constant vs is the speed of sound in the water. Assuming that the TOA algorithm uses these four observations together with the current clock offset estimates, i.e., ∆i(ttx)=α îttx + βî for i = 1,..., 4, to calculate a position estimate pˆ using a nonlinear least square approach. Then, the MSE of the observationsb becomes approximately [FB98]

MSE(p pˆ) H†(Υ + σ2 I )(H†)T (4.59) − ≈ ∆ w 3 where

h1(p) ∇h (p) H = 2 . (4.60)  ∇h (p)  ∇ 3  h4(p)   ∇  Here, and denote the gradient and matrix pseudo inverse, respectively. ∇ † Further, Υ∆ is the MSE matrix of the clock offset estimates. Assuming the clock offset estimates to be uncorrelated and using the lower bound for the MSE of the clock offset derived in Section 4.4.4, a bound on the MSE of the positioning error of the ROV can be calculated. 4.7. CONCLUSIONS 101

The by Monte Carlo simulation calculated MSE of the position estimates of the ROV is shown in Figure 4.12. Also shown is the MSE of the position estimates for the case when the clock synchronization was stopped after 1800 s (i.e., after 360 message exchanges with each node) and thereafter the clock offset was predicted using the estimated clock parameters. Moreover, the lower bound on the MSE of the position estimates, based on (4.59) and (4.43), has been evaluated with the same parameters as the simulation scenario and is also shown in Figure 4.12. The improvements in position accuracy owing to the time synchronization are evident from the results in Figure 4.12. After 3600 s (i.e., 720 message exchanges with each node), the effect of the clock synchronization errors is negligible when compared with the other error sources in the trilateration of the ROV, the position accuracy being close to the accuracy achievable with perfect clock synchronization. The results of the simulation in which the clock synchronization was stopped after 360 message exchanges (i.e., 1800 s) show the benefit of estimating the clock skew. Even though the synchronization has been stopped, the offset of the slave node clocks can be predicted and the position error growth thereby reduced. Moreover, the bounds on the position accuracy for the two simulation cases agree well with the results from the Monte Carlo simulations, illustrating the application of the derived CRB as a tool for accessing the impact of the time synchronization at a system level. In summary, to illustrate the application of the derived CRB, beyond being a versatile tool for accessing the theoretical limitations of the two- way message exchange clock synchronization, a simulation study on TOA positioning in an underwater WSN was conducted. In the study, the two- way message exchange together with the AMLE was used to synchronize the sensor nodes used in the TOA positioning of an ROV, illustrating the relationship between the estimation accuracy of the clock parameters and the accuracy at which the ROV could be positioned. Moreover, the lower bound on the MSE of the clock offset was mapped into a lower bound on the accuracy of the TOA positioning algorithm.

4.7 Conclusions

The accuracy with which the clock offset between a master and a slave node in a sensor network can be estimated using a two-way message exchange between the nodes has been investigated. The study has been conducted under a set of assumptions. The most important assumptions being: the clocks can be described by linear models (including affine mappings), the (wireless) communication channel is symmetric, and the communication latency within the channel can be described as Gaussian. Under these conditions, the CRB for the estimation of the clock skew and initial clock offset 102 CHAPTER 4. SYNCHRONIZATION BY MESSAGE EXCHANGES were derived for four different scenarios (distinguished by the set of nuisance parameters), reflecting different levels of prior knowledge concerning the system parameters. The asymptotic CRB is derived as well and shown to be the same for the considered scenarios. For large sample sets the lower bound on the variance of the skew is inversely proportional to the cube of the number of message exchanges, and the variance of the initial offset is inversely proportional to the number of message exchanges, i.e., it is easier to estimate the clock skew than the initial clock offset. Further, the derived CRB for the clock skew and initial clock offset was mapped into a lower bound on the MSE of the clock offset. Asymptotically, the lower bound on the MSE is inversely proportional to the absolute time duration t normalized by the message update-rate 1/Ts, the clock skew, and the channel spread. In particular, it is independent of the average channel delays, slave node average delay and spread, and the initial clock offset. By creating a set of artificial observations through linear combinations of the observations originally obtained from the two-way message exchange, a low-complexity AMLE was derived. The estimator’s performance was examined both theoretically and through numerical evaluations, and compared with the derived CRB. Asymptotically, the AMLE was shown to be nearly optimal, i.e., it nearly obtains the CRB. 4.A. FIM ELEMENTS 103

4.A FIM Elements

In case C.2 the FIM is given by are given by the FIM for case C.1 plus the following nonzero elements

µǫ m [Im(θ)]1,3 = [Im(θ)]3,1 = 2 , (4.61) −(1 + α) σT m [Im(θ)]1,4 = [Im(θ)]4,1 = 2 , (4.62) (1 + α) σT 2 m m [Im(θ)]3,3 = 2 , and [Im(θ)]4,4 = 4 . (4.63) σT σT In case C.3 the FIM is given by are given by the FIM for case C.1 plus the following nonzero elements

µǫ m [Im(θ)]1,3 = [Im(θ)]3,1 = 2 , (4.64) (1 + α) σǫ m [Im(θ)]1,4 = [Im(θ)]4,1 = 2 , (4.65) (1 + α) σǫ m m [Im(θ)]3,3 = 2 , and [Im(θ)]4,4 = 4 . (4.66) σǫ 2 σǫ In case C.4 the FIM is given by FIM for case C.2 plus the following nonzero elements

µǫ m [Im(θ)]1,5 = [Im(θ)]5,1 = 2 , (4.67) (1 + α) σǫ m [Im(θ)]1,6 = [Im(θ)]6,1 = 2 , (4.68) (1 + α) σǫ m m [Im(θ)]5,5 = 2 , and [Im(θ)]6,6 = 4 . (4.69) σǫ 2 σǫ

4.B Polynomial Functions in the CRB Expressions

The polynomial functions in the expression for the CRB in (4.28)–(4.30) reads

2 2 4 2 2 2 2 c1(m)=3 m (10 σT σǫ +2 σǫ +2 µǫ σT + µǫ σǫ ), (4.70)

2 2 2 2 4 c2(m)=6 m (4 σT σǫ + µǫ σT + σǫ ), (4.71) 104 CHAPTER 4. SYNCHRONIZATION BY MESSAGE EXCHANGES

2 2 2 2 c3(m)=3 m(6 σT +2σǫ + µǫ ) σǫ , (4.72)

2 2 2 c4(m)=6 m(2 σT + σǫ ) σǫ , (4.73)

d (m)= T 2 +6 µ2 + 15 σ2 +6 µ ((m 1) T + µ ) 1 s T T T − s ǫ 2 (4.74) 2 σT 2 + 3 ((m 1)Ts µǫ + µǫ (1 + 2 )+ σǫ ), − σǫ

2 2 2 2 3µǫ σT 2 d2(m)= Ts + 12σT + 2 +3σǫ σǫ (4.75) 3 + (2µ + µ ) (2 (m 1) T +2µ + µ ) , 2 T ǫ − s T ǫ

2 2 d3(m)= T +6 µ +6 µT ((m 1) Ts + µǫ) s T − (4.76) +3 µ ((m 1) T + µ )+9 σ2 +3 σ2, ǫ − s ǫ T ǫ and

2 2 2 d4(m)= Ts +6 σT +3σǫ 3 (4.77) + (2 µ + µ ) (2(m 1) T +2 µ + µ ) . 2 T ǫ − s T ǫ

4.C Proofs of the Inequalities

Proofs of the inequalities in (4.37) and (4.38), used in the derivation of the lower bound for the clock oﬀset.

4.C.1 First Inequality To show the inequality

g (ξ + t(1) ) g (t(1) ), ξ 0 : m Ω (4.78) m m−1 ≥ m m−1 ∀ ≥ ∈ m where Ω = m m N and m (2 µ + µ )/T +1 , let ν (ξ)= g (ξ + m { | ∈ ≥ T ǫ s } 1 m t(1) ) g (t(1) ). From the deﬁnition of g (t) in (4.36) it then holds that m−1 − m m−1 m 2 (1) ν1(ξ) = [Im]1,1ξ +2 [Im]1,2 + tm−1 [Im]1,1 ξ, (4.79) 4.C. PROOFS OF THE INEQUALITIES 105 which can be identiﬁed as a convex quadratic polynomial function. Further, its roots are located at ξ1 =0 and

ξ = 2([I ] /[I ] + t(1) ) 2 − m 1,2 m 1,1 m−1 (4.80) = (1 m) T +2µ + µ , − s T ǫ and thus ν (ξ) 0, ξ max(ξ , ξ ). Hence, given that m (2µ + 1 ≥ ∀ ≥ 1 2 ≥ T µǫ)/Ts +1, then ξ2 0 and ν1(ξ) 0, ξ 0, from which the inequality in (4.78) follows directly.≤ ≥ ∀ ≥

4.C.2 Second Inequality To show the inequality g (t) g (t), t, let ν (t) = g (t) g (t). m−1 ≥ m ∀ 2 m−1 − m From the deﬁnition of gm(t) in (4.36) it then holds that

ν (t)= xT (t)[I−1 I−1] x(t) 2 m−1 − m 1:2,1:2 T −1 −1 = x (t)[I (Im−1 + Jm) ]1:2,1:2 x(t) m−1 − (4.81) T −1 −1 = x (t)[Im−1(Jm + Im−1) Im−1]1:2,1:2 x(t) 0, ≥ where Im Im(θ) and Jm Jm(θ). In the second equality the matrix inversion lemma≡ was used and≡ the ﬁnal inequality follows from the fact that Jm(θ) and Im−1(θ) are positive semideﬁnite symmetric matrices. From (4.81) the inequality in follows directly.

Chapter 5

Zero-Velocity Detection in Pedestrian Navigation Systems

5.1 Introduction

The integrative nature of the inertial navigation system (INS) conveys that its sensor errors give rise to a position error that grows proportional to the cube of the system’s operation time [ESN07]. Therefore, with the performance of the low-cost inertial sensors currently available, free inertial navigation is only feasible for time periods in the range of a few seconds [ESN07, SH09a, RSV07]. However, the devastating cubic error growth can be bro- ken by imposing constraints on the navigation solution using information about the system dynamics [ESN07, SH09a, GC07, DSNDW01, Fox05]. A type of information commonly used for this purpose is knowledge about the time epochs when the system is in a stationary phase, i.e., when the system has a constant position and attitude. This method is referred to as zero-velocity updates. A conceptual description of a zero-velocity update constrained INS is shown in Figure 5.1. The use of zero-velocity updates is especially appealing for constraining foot-mounted INSs, as during ordinary gait, the foot returns to a "stationary" state on a regular base [Sab06]. Indeed, this idea has been success- fully applied [GL08, OB07], resulting in systems where the position error grows proportionally with respect to the number of steps taken by the user [OB07, Fox05]. For details on the benefits of zero-velocity updates, one can refer to [GBYT02] and the references therein. However, the use of zero-velocity updates requires identification of the time epochs when the inertial measurement unit (IMU) is stationary [GL08]. Accordingly, a range of detectors have been proposed for detecting the stationary epochs on the basis of the inertial sensor data [VBdV+96, GLC06, KLC08, GL08, KR08, FZGB09, OB07]. Moreover, within the field of gait analysis in biomedicine,

107 108 CHAPTER 5. ZERO-VELOCITY DETECTION

Navigation solution IMU INS mechanization

Error feedback

Pseudo Zero-velocity observations Filter detector algorithm

Figure 5.1: Illustration of a zero-velocity-constrained INS. When the zero- velocity detector decides that the IMU is stationary the system generates a pseudo observation and inputs it to the filter algorithm. The filter compares the pseudo observation with the velocity estimate of the INS and uses the difference to estimate the errors of the INS. The error estimates are then fed back to calibrate the INS.

different detectors for extracting gait motion parameters, such a foot-flat1, based on IMU data have been proposed [BBS+08, SMSC05]. However, the proposed detectors are generally derived in an ad-hoc manner and the literature lacks a study on their characteristics and performance. The aim of the study presented in this chapter is to formalize the detection problem as a hypothesis testing problem, and use the results from the statistical detection theory to analyze the detectors. The theoretical findings are then supported by testing the detectors on data recorded under different gait conditions.

5.2 Problem Deﬁnition

Assume that an IMU has been attached to the instep of a foot as illustrated in Figure 5.2. Let y R6 be the vector k ∈ ya y = k (5.1) k yω k that denotes the output from the IMU, i.e., the measured three-dimensional a R3 ω R3 speciﬁc force, yk , and the angular velocities, yk , at the time instant k. Thus,∈ the objective of the zero-velocity detection∈ is to decide

1An IMU placed on the instep of the foot will be approximately stationary during the foot-ﬂat phase of the stance phase. See Figure 5.4 for an illustration. 5.2. PROBLEM DEFINITION 109

x-axis

IMU

y-axis

z-axis FSR FSR

Figure 5.2: Illustration of the on the instep-mounted IMU and the force- sensitive resistors (FSRs) used in the reference system.

whether, during a time epoch consisting of N observations2 between the time instants n and n + N 1, the IMU is moving or stationary, given the measurement sequence −

z , y n+N−1. (5.2) n { k}k=n Further, the decisions should be made in such a way that the probability of deciding that the IMU is stationary when it is not should be kept low. This is owing to the ruinous eﬀect of imposing an erroneous constraint on the navigation solution. However, as imposing the constraints when it holds true is the key to break the cubic error drift, the probability of detecting the stationary events should be maximized given a certain error decision probability. Mathematically, we can formalize the detection problem as a binary hypothesis testing problem, where the detector can choose between the two hypotheses and , deﬁned as H0 H1

0 : IMU is moving H (5.3) : IMU is stationary H1 The detector performance is then speciﬁed by the false-alarm probability P = Pr (i.e., the probability of deciding on the hypothesis FA {H1|H0} 1 when hypothesis 0 is true) and the probability of detection PD = HPr (i.e., theH probability of deciding hypothesis when it is {H1|H1} H1 2The choice on the number of observations N will depend upon the sample rate of the system and the length of the time epochs for which the IMU is assumed stationary. 110 CHAPTER 5. ZERO-VELOCITY DETECTION true). Further, Neyman-Pearson (NP) theorem tells us how to choose between the two hypotheses to maximize P for a given P . Let p(z ; ) D FA n H0 and p(zn; 1) denote the probability density functions (PDFs) of the ob- servationsH for the two hypotheses. Thus, the NP theorem then states that [Kay93a]

Theorem 1 (Neyman-Pearson) To maximize PD for PFA = α decide on 1 if H p(z ; ) L(z )= n H1 > γ, (5.4) n p(z ; ) n H0 where the threshold γ is determined from

P = p(z ; )= α. (5.5) FA n H0 Z{zn:L(zn)>γ}

The function L(zn) in (5.4) is referred to as the likelihood ratio, as for each value z , it indicates the likelihood of the hypothesis versus the n H1 0 hypothesis. Moreover, the test in (5.4) is referred to as the likelihood Hratio test (LRT). Clearly, the LRT require complete knowledge about the PDFs of the observations for the two hypotheses. Therefore, as the PDFs of the observations are mostly only partly known, we often have to choose approximate hypothesis tests methods.

5.3 Signal and Sensor Model

The PDFs of the observed data depends both upon the true signal and the distortion introduced to the signal by the sensors. Hence, to ﬁnd the PDFs of the observations, we must specify a signal and sensor model. The choice of models should be balanced between the mathematical tractability and realistic description of the signal and the sensor.

5.3.1 Sensor Model For IMU sensors, there is a vast amount of models with various complex- ities [SH06a, GWA07, Rog03, Sab05]. We will restrict our analysis to the case when the sensor output may be described as

yk = sk(θ)+ vk, (5.6) where

sa(θ) s (θ)= k , (5.7) k sω(θ) k 5.3. SIGNAL AND SENSOR MODEL 111 and va v = k . (5.8) k vω k a R3 ω R3 Here, sk(θ) and sk (θ) denote the IMU-sensed speciﬁc force and angular∈ velocity, respectively.∈ Further, θ denotes the set of unknown a R3 ω R3 parameters needed to describe the signals. Moreover, vk and vk denote the noise associated with the accelerometer and∈ gyroscope sensor∈ assembly, respectively. We will assume that the noise is i.i.d., zero-mean, Gaussian with covariance matrix

2 E T σa I3 03 C = vk vk = 2 , (5.9) { } 03 σ I3 ω where I3 (03) denotes an identity (zero) matrix of size 3 3. Further, E 2 R1 2× R1 {·} denotes the expectation operation, and σa and σω denote the accelerometer and gyroscope noise variance,∈ respectively. ∈

5.3.2 Signal Model

Modeling the specific force and angular velocities, i.e., sk(θ), experienced by the foot-mounted IMU under the hypothesis 0 (that the IMU is in motion) in a consistent way that reflects a diverseH set of gaits, is difficult. However, under hypothesis 1 (that the IMU is stationary), the following conditions hold true (neglectingH the effect of the earth rotation rate):

• The only force sensed by the accelerometers assembly is the gravita- tion acceleration, whose magnitude is known. • The attitude of the IMU is constant, i.e., the angular velocity experienced by the IMU is zero.

In other words, mathematically, for the two hypotheses, the signal fulﬁlls the conditions

a ω 0 : s (θ) = gun or s (θ) = 03,1 k Ωn H k 6 k 6 ∀ ∈ (5.10) : sa(θ)= gu and sω(θ)= 0 k Ω H1 k n k 3,1 ∀ ∈ n Here u Ω , where Ω = u R3 : u = 1 and g R1 is the n ∈ u u { ∈ k k } ∈ magnitude of the local gravity vector. Moreover, u = √uT u where T k k denotes the transpose operation and Ωn = ℓ N : n ℓ

n+N−1 0 : θ sk H ≡{ }k=n (5.11) : θ u . H1 ≡ n 112 CHAPTER 5. ZERO-VELOCITY DETECTION

That is, under , the signal s (θ) is completely unknown, whereas under H0 k 1, only the direction of the speciﬁc force vector is unknown. In Figure 5.4, Hthe output from an instep-mounted IMU during the stance phase of the gait is shown.

5.3.3 PDFs of Observations Owing to the lack of knowledge about the signal parameters θ, we cannot completely specify the PDFs of the observations under the two hypotheses. However, with the sensor model speciﬁed by (5.6)–(5.9), the measurements may originate from the family of PDFs, which are given by (for i =0, 1)

p(z ; θ, )= p(y ; θ, ) n Hi k Hi k∈Ω Yn (5.12) = p(ya; θ, ) p(yω; θ, ), k Hi k Hi kY∈Ωn where

a 1 1 a a 2 p(yk; θ, i)= 2 3/2 exp( 2 yk sk(θ) ), (5.13) H (2πσa) −2σa k − k and

ω 1 1 ω ω 2 p(yk ; θ, i)= 2 3/2 exp( 2 yk sk (θ) ). (5.14) H (2πσω) −2σω k − k

5.4 Generalized Likelihood Ratio Test

As the PDFs of the observations under the two hypotheses are parameterized by a set of, to us, unknown parameters θ, i.e., the (partly) unknown signal sk(θ), we cannot apply the LRT in (5.4). However, by substituting the unknown parameters with their maximum likelihood estimates (MLEs), we can derive a GLRT. The GLRT decides on , if H1 1 p(zn; θˆ , 1) LG(zn)= H > γ, (5.15) p(z ; θˆ0, ) n H0 1 where θˆ is the MLE of the unknown parameters assuming 1 is true, and 0 H θˆ is the MLE of the unknown parameters assuming 0 is true. The MLE is obtained by maximizing (5.12) with respectH to the unknown parameters. In our case, under 0, the signal is completely unknown and θˆ0 = y n+N−1. Thus, H { k}k=n 5.4. GENERALIZED LIKELIHOOD RATIO TEST 113

ˆ0 1 1 p(zn; θ , 0)= 2 3 N/2 2 3 N/2 . (5.16) H (2πσa) · (2πσω)

Under 1 we can obtain the MLE of the unknown signal parameter un by maximizingH

1 1 p(z ; u , )= exp( ya gu 2) n n H1 (2πσ2)3 N/2 −2σ2 k k − nk a a k∈Ω Xn (5.17) 1 1 ω 2 2 3 N/2 exp( 2 yk ) · (2πσω) −2σω k k kX∈Ωn with respect to u Ω . That is, n ∈ u

uˆn = argmax p(zn; u, 1) u∈Ωu H a 2 = argmin yk gu u k − k ∈Ωu k∈Ω Xn a 2 a T 2 2 (5.18) = argmin yk 2 (yk) u + g u u∈Ωu k k − k k k∈Ωn X a a T yn = argmax (yn) u = a , u y ∈Ωu k nk where 1 ya = ya. (5.19) n N k kX∈Ωn The last equality in (5.18) follows from Cauchy’s Schwarz’ inequality: aT b a b , with equality observed only if a = c b; c being a real constant.≤ k kk k Substituting uˆn into (5.17) yields

p(z ; θˆ1, )= p(z ; uˆ , ) n H1 n n H1 1 1 = 2 3 N/2 2 3 N/2 (2πσa) · (2πσω) a 1 a yn 2 (5.20) exp( 2 yk g a ) · −2σa k − yn k kX∈Ωn k k 1 ω 2 exp( 2 yk ). · −2σω k k kX∈Ωn Next, if we combine (5.15), (5.16), and (5.20), the GLRT becomes: decide on if H1 114 CHAPTER 5. ZERO-VELOCITY DETECTION

1 ya L (z ) = exp( ya g n 2) G n −2σ2 k k − ya k a k∈Ω n Xn k k (5.21) 1 ω 2 exp( 2 yk ) > γ. · −2σω k k kX∈Ωn We may further simplify the test with respect to the fact that the logarithm is monotonically increasing. To do so, let

2 T (z )= ln L (z ). (5.22) n −N G n We can then state the GLRT as: choose if H1

a 1 1 a yn 2 1 ω 2 ′ T (zn)= 2 yk g a + 2 yk <γ . (5.23) N σa k − yn k σω k k kX∈Ωn k k We can interpret this as follows: if the mean square error of ﬁtting a vector of magnitude g, with the direction of the average speciﬁc force vector to the accelerometer data in combination with the energy in the gyroscope signal, each weighted by quality of the measurements, falls bellow the threshold γ′, the GLRT chooses the hypothesis that the IMU is stationary. In Figure 5.3, the test statistics T (zn) based upon the data from a foot-mounted IMU, sampled at 30 Hz, during leveled ground, forward gait, is shown. Figure 5.3 also shows the time moments n when all the data samples in zn originate from a stationary phase according to our reference system. Details on the reference system are presented in Section 5.5.

5.4.1 Acceleration Moving Variance Detector A detector proposed in the literature to detect when the IMU is stationary, is the acceleration moving variance detector [VBdV+96, GLC06, KLC08, GL08]. The moving variance detector is solely based upon the accelerometer data and is deﬁned as follows: decide on if H1 1 T (za )= ya ya 2 <γ , (5.24) v n N k k − nk v kX∈Ωn where

za , ya n+N−1. (5.25) n { k}k=n We can derive the acceleration moving variance detector within the GLRT frame work with respect to the fact that the orientation of the accelerometer 5.4. GENERALIZED LIKELIHOOD RATIO TEST 115

6 x 10 GLRT test statistics T (zn) 8 6 ) n z

( 4 T 2 0 10 20 30 40 50 60 70 Window start sample n 4 ′ a x 10 GLRT test statistics Tv(zn)

10 ) a n z ( ′ v 5 T

0 10 20 30 40 50 60 70 Window start sample n 5 a x 10 GLRT test statistics Tm(zn) 2 ) a n z ( 1 m T

0 10 20 30 40 50 60 70 Window start sample n 6 ω x 10 GLRT test statistics Tω(zn ) 8 6 ) ω n z

( 4 ω T 2 0 10 20 30 40 50 60 70 Window start sample n

′ a a ω Figure 5.3: Test statistics T (zn), Tv(zn), Tm(zn), and Tω(zn ) as a function of n for a window size N =5, during slow (approx. 3 km/h), leveled ground, forward gait. The dots indicate the moments n when all data samples in zn originates from a phase when the IMU was stationary according to the reference system. 116 CHAPTER 5. ZERO-VELOCITY DETECTION assembly is constant when the IMU is stationary, but neglecting the fact that the magnitude of the speciﬁc force vector is equal to g. To derive the test, let us assume that under the two hypotheses, the signal from the accelerometer cluster fulﬁlls the conditions

a a 0 : s (θv) = s k Ωn H k 6 ∀ ∈ (5.26) : sa(θ )= sa k Ω H1 k v ∀ ∈ n where sa R3 is unknown. Hence, the unknown parameters needed to describe the∈ signal under the two hypotheses are

a n+N−1 0 : θv s H ≡{ k}k=n (5.27) : θ sa H1 v ≡ and thus, the family of PDFs describing the accelerometer observations is given by (for i =0, 1)

p(za ; θ , )= p(ya,θ , ). (5.28) n v Hi k v Hi kY∈Ωn ˆ0 a n+N−1 With this information in hand, under 0, the MLE θv = yk k=n . Thus, H { }

a ˆ0 1 p(zn; θv, 0)= 2 3N/2 . (5.29) H (2πσa) Further, the MLE of the unknown parameters under are given by H1

a a ˆs = argmax p(zn; s, 1) s H a 2 a (5.30) = argmin yk s = yn. s k − k k∈Ω Xn Therefore,

1 a ˆ1 a ˆa p(zn; θv, 1)= p(zn; s , 1)= 2 3N/2 H H (2πσa) (5.31) 1 a a 2 exp( 2 yk yn ). · −2σa k − k kX∈Ωn Hence, if we combine (5.15), (5.22), (5.29), and (5.31), the GLRT becomes as follows: choose if H1 5.4. GENERALIZED LIKELIHOOD RATIO TEST 117

a ˆ1 ′ a 2 p(zn; θv, 1) Tv(zn)= ln H −N p(za ; θˆ0, ) n v H0 (5.32) 1 a a 2 ′ = 2 yk yn <γv, σa N k − k kX∈Ωn which is equivalent to the test in (5.24). In Figure 5.3, the test statistics ′ a Tv(zn) based upon the data from a foot-mounted IMU, sampled at 30 Hz, during leveled ground, forward gait, is shown. Figure 5.3 also shows the a time moments n when all the data samples in zn originate from a stationary phase according to the reference system.

5.4.2 Acceleration Magnitude Detector The acceleration magnitude detector is another detector proposed in the literature [GLC06, GL08, KR08], and is often used as a supplement to the moving variance detector. The acceleration magnitude detector checks if the measured specific force vector is close to g, and if that is the case, concludes that the IMU is stationary. We can derive this type of detector within the GLRT framework based on the fact that the magnitude of the specific force vector is g when the IMU is stationary, but neglecting the fact that the direction of the vector should be constant. To derive the test, let us assume that under the two hypotheses, the signal from the accelerometer cluster fulfills the conditions

a 0 : s (θm) = guk k Ωn H k 6 ∀ ∈ (5.33) : sa(θ )= gu k Ω H1 k m k ∀ ∈ n where uk Ωu. Hence, the unknown parameters needed to describe the signal under∈ the two hypotheses are

a n+N−1 0 : θm sk H ≡{ }k=n (5.34) : θ u n+N−1 H1 m ≡{ k}k=n Again, the family of PDFs describing the accelerometer observations is given by (5.28); however, now the unknown parameters are those in (5.34). ˆ0 a n+N−1 Under 0, when the signal is completely unknown, the MLE θm = yk k=n Ha ˆ0 { } and p(zn; θm, 0) is given by (5.29). Further, under 1, the MLEs of the unknown parametersH are given by (for ℓ = n,...,N +Hn 1) − 118 CHAPTER 5. ZERO-VELOCITY DETECTION

a n+N−1 uˆℓ = argmax p(zn; uk k=n , 1) uℓ∈Ωu { } H a 2 = argmin yk uk uℓ∈Ωu k − k (5.35) k∈Ωn X a a 2 yℓ = argmin yℓ uℓ = a . u k − k y ℓ∈Ωu k ℓ k Thus,

p(z ; θˆ1 , )= p(z ; uˆ n+N−1, ) n m H1 n { k}k=n H1 1 = 2 3 N/2 (2πσa) (5.36) a 1 a yk 2 exp( 2 yk g a ). · −2σa k − yk k kX∈Ωn k k Hence, if we combine (5.15), (5.22), (5.29), and (5.36), the GLRT becomes as follows: choose , if H1 a Tm(zn) <γm (5.37) where a ˆ1 a 2 p(zn; θm, 1) Tm(z )= ln H n a ˆ0 −N p(zn; θm, 0) H a 1 a yk 2 = 2 yk g a (5.38) σa N k − yk k kX∈Ωn k k 1 a 2 = 2 ( yk g) . σa N k k− kX∈Ωn a In Figure 5.3, the test statistics Tm(zn) based upon the data from a foot- mounted IMU, sampled at 30 Hz, during leveled ground, forward gait, is shown. Figure 5.3 also shows the time moments n when all the data samples a in zn originate from a stationary phase according to the reference system.

5.4.3 Angular Rate Energy Detector Sometimes, only the energy in the gyroscope signal is used to detect when the IMU is stationary [FZGB09, OB07]. The angular rate energy detector decides on if H1 1 T (zω)= yω 2 <γ , (5.39) ω n N k k k ω kX∈Ωn 5.5. PERFORMANCE EVALUATION 119 where

zω , yω n+N−1. (5.40) n { k }k=n ω The test statistics Tω(zn ) corresponds to the last part of the GLRT statis- 2 tics in (5.22), except for the scaling with σω. Thus, we can also derive the angular rate energy detector within the GLRT framework. In Figure 5.3, ω the test statistics Tω(zn ) based upon the data from a foot-mounted IMU, sampled at 30 Hz, during leveled ground, forward gait, is shown. Figure 5.3 ω also shows the time moments n, when all the data samples in zn originate from a stationary phase according to the reference system.

5.5 Performance Evaluation

To evaluate the GLRT detectors examined in Section IV, we collected the gait data from an instep-mounted IMU. The IMU, an Analog Devices ADIS16355, was mounted on the instep of the user’s right foot, with the coordinate axis, as illustrated in Figure 5.2. A data sample rate of 30 Hz was selected. We asked the user to repeatedly walk a predeﬁned square trajectory on leveled ground. The user was ﬁrst told to walk slowly (approx. 3 km/h) for 30 min, and then fast (approx. 5 km/h) for another 30 min. The results obtained were two data sets, corresponding to slow and fast, leveled ground, forward gait, respectively.

5.5.1 Reference System As a reference indicating when the IMU was stationary, we used a system of two switches mounted beneath the heel and the front sole of the user’s right shoe. This is illustrated in Figure 5.2. The switches were constructed around force-sensitive resistors in an approach similar to that given in [HLW95]. The two switches were connected to the ADIS16355’s spare analog-to-digital converter channel, via a logical AND-gate, and sampled simultaneously with the IMU sensors. Even though the foot and the shoe are non-ridge bodies, we declared the IMU as stationary when both switches were pressed down simultaneously. For details regarding the motion of the foot during the stance phase, one can refer to [LBB+07, Whi02]. To verify that the stationarity approximation was valid, we recorded the motion of the IMU, the state of the reference system, and the output of the IMU sensors, during a stance phase. The result is shown in Figure 5.4, where the state of the reference system is indicated by the light-emitting diode (LED) on the leg of the user. When the LED is on, the reference system classiﬁes the IMU as stationary. From the pictures, we can see that the IMU is close to stationary during the time interval when the LED is turned on, and that the stationarity 120 CHAPTER 5. ZERO-VELOCITY DETECTION approximation is valid3. However, one should note that the behavior of the reference system depends upon several factors such as the precise place of the switches beneath the user’s shoe, the pronation or supination of the user’s foot, etc. Thus, the results presented next should be regarded as an indication of the achievable performance, rather than an absolute measure.

5.5.2 Detector Statistics Having collected the data, we calculated the detection and false-alarm probabilities for the detectors using a data window size N =5 (i.e., approx. 167 ms). The calculated probabilities for slow and fast gait are shown in Fig- ures 5.5 and 5.6, respectively. From the ﬁgures, we can see that:

• The angular rate energy detector and the new GLRT detector have the highest performance, and behave basically in the same manner, indicating that the gyroscope signal holds the most reliable information for stationarity detection.

• There is a signiﬁcant diﬀerence in the performance of the detectors for slow and fast gait, respectively.

At first glance it may seem that the performance of the detectors is far from sufficient for any zero-velocity constrained navigation application. However, the PD versus PFA figures are partly misleading for our application. For a zero-velocity-constrained INS, it is better to have short periods of zero-velocity updates on a regular basis at a low PFA level, than applying longer periods of zero-velocity updates at a higher PFA level. Thus, if the detector’s threshold is adjusted to a lower PFA level, but the detector is still capable of detecting that the IMU is stationary at least once for most gait cycles, then the periods of free inertial navigation will be sufficiently short to avoid large error drifts. A better measure of the performance of zero-velocity detectors is the probability of detecting at least once that the IMU is stationary in the ”IMU stationary” part of the gait cycle. We therefore introduced PSD, that denotes the probability that the detector decides on 1 at least once in the ”IMU stationary” part of the gait cycle. By recalculatingH the statistics of the detector in terms of PSD, using the data sets previously employed, we obtained the results for slow and fast forward gait, which are shown in Figures 5.7 and 5.8, respectively. From the figures, we can conclude that:

3The IMU will never be truly stationary during the stance phase. However, the small motions during the times of zero-velocity updates may be considered as the measurement noise, and the covariance matrix associated with the pseudo observations should be set to reﬂect the size of these motions. 5.5. PERFORMANCE EVALUATION 121

Foot position

Accelerometer signals 2 ]

g 0 [

−2 0 0.05 0.1 0.15 0.2 0.25

Gyroscope signals 200 ]

/s 0 ◦ [ −200 0 0.05 0.1 0.15 0.2 0.25 Time [s] Foot position

Accelerometer signals 1 ]

g 0 [

−1 0.25 0.3 0.35 0.4 0.45 0.5

Gyroscope signals 100 ]

/s 0 ◦ [

−100 0.25 0.3 0.35 0.4 0.45 0.5 Time [s]

Figure 5.4: The stance phase as a function of time together with the output from the accelerometers and gyroscopes. When the LED is on (white dot in the pictures), the reference system classiﬁes the IMU as stationary. Legend: x-axis, y-axis, —- z-axis. −· −− 122 CHAPTER 5. ZERO-VELOCITY DETECTION

PD versus PFA for slow, leveled ground, forward gait 1

0.9

0.8

0.7

0.6

D 0.5 P

0.4

0.3 T (zn) ′ a Tv(zn) 0.2 a Tm(zn) ω Tω(zn ) 0.1

0 0 0.001 0.002 0.003 0.004 0.005 0.006 0.007 0.008 0.009 0.01 PFA

Figure 5.5: PD versus PFA for the GLRT algorithms based upon data from slow, leveled ground, forward gait. The detectors used a data window size of N =5.

PD versus PFA for fast, leveled ground, forward gait 1

0.9 T (zn) ′ a Tv(zn) 0.8 a Tm(zn) ω Tω(zn ) 0.7

0.6

D 0.5 P

0.4

0.3

0.2

0.1

0 0 0.001 0.002 0.003 0.004 0.005 0.006 0.007 0.008 0.009 0.01 PFA

Figure 5.6: PD versus PFA for the GLRT algorithms based upon data from fast, leveled ground, forward gait. The detectors used a data window size of N =5. 5.5. PERFORMANCE EVALUATION 123

PSD versus PFA for slow, leveled ground, forward gait 1

0.9

0.8

0.7

0.6

SD 0.5 P

0.4

0.3 T (zn) ′ a Tv(zn) 0.2 a Tm(zn) ω Tω(zn) 0.1

0 0 0.001 0.002 0.003 0.004 0.005 0.006 0.007 0.008 0.009 0.01 PFA

Figure 5.7: PSD versus PFA for the GLRT algorithms based upon data from slow, leveled ground, forward gait. The detectors used a data window size of N =5.

PSD versus PFA for fast, leveled ground, forward gait 1

0.9

0.8

0.7

0.6

SD 0.5 P

0.4

0.3 T (zn) ′ a Tv(zn) 0.2 a Tm(zn) ω Tω(zn) 0.1

0 0 0.001 0.002 0.003 0.004 0.005 0.006 0.007 0.008 0.009 0.01 PFA

Figure 5.8: PSD versus PFA for the GLRT algorithms based upon data from fast, leveled ground, forward gait. The detectors used a data window size of N =5. 124 CHAPTER 5. ZERO-VELOCITY DETECTION

• For slow gait at low PFA values, the angular rate energy detector and the new GLRT detector still have the highest performance, whereas for high PFA values, the magnitude detector has the best performance.

• For fast gait, the new GLRT detector is marginally better than the angular rate energy detector outperforming the other detector for all PFA values.

5.6 Conclusions

We have investigated the problem of detecting the time epochs when zero- velocity updates can be applied in a foot-mounted pedestrian navigation system. We have looked at three commonly used detectors, in the literature: the acceleration moving variance detector, the acceleration magnitude detector, and the angular rate energy detector. We have shown that all detectors can be derived within the same GLRT framework - given the different prior knowledge about the sensor signals. Further, by combining all prior knowledge, we derived a new GLRT detector. We subsequently evaluated the performance of the detectors using slow (approx. 3 km/h) and fast (approx. 5 km/h), leveled ground, forward gait data. As a reference system, indicating when the IMU was stationary, we used force-sensitive resistors mounted beneath the foot of the user. The test results were presented in terms of the detection versus false-alarm probability. Moreover, we calculated the probability of detecting at least once that the IMU is stationary in ”IMU stationary” part of the gait cycle. This probability, being a function of the false-alarm probability, is often a better figure of merit for zero-velocity detectors used in constrained INSs. The results show that the new GLRT detector performs similar to the angular rate energy detector [FZGB09, OB07], which outperforms both the acceleration moving variance detector [VBdV+96, GLC06, KLC08, GL08] and acceleration magnitude detector [GLC06, GL08, KR08]. This indicates that the gyroscope signals hold the most reliable information for zero-velocity detection under the experimental condition stated. Further, the results show that the performance varies a lot between slow and fast gait, where for the latter, the proper work of the detectors is questionable. In the literature, there are rarely any discussions about the performance of accelerometer-based detectors versus the ones based on gyroscope signals. The majority of the works consider the former class of methods. A reason for this can be the fact that inexpensive off-the-shelf (COTS) accelerometers were available on the market some years before the gyroscopes. For instance, Analog Devices launched their first COTS accelerometer in the beginning of the nineties, but their first gyroscope did not hit the market until a decade later. Nowadays, six degrees-of-freedom IMUs are 5.6. CONCLUSIONS 125 readily available which opens up for detectors utilizing all six dimensions. We have shown that under the given assumptions the gyroscope-based detection clearly outperforms the accelerometer-based ones, and that there is no significant improvement of the detector using the both accelerometer and gyroscope data, over the one only employing the signals from the gyroscopes.

Chapter 6

Calibration of a MEMS Inertial Measurement Unit

6.1 Introduction

The development in micro-electro-mechanical system (MEMS) technology has made it possible to fabricate cheap single chip accelerometer and gyroscope sensors, which have been adopted into many applications where traditionally inertial sensors have been too costly. For example the MEMS sensors have made it possible to construct low-cost global navigation satellite system (GNSS) aided inertial navigation systems (INS) for monitoring vehicle behavior [SSH06]. The obtained accuracy and convergence time of a GNSS aided INS is highly dependent on the quality of the inertial measurement unit (IMU) sensors output [ESNN04], and therefore a calibration of the IMU is critical for the over all system performance. Traditionally the calibration of an IMU has been done using a mechanical platform, turning the IMU into diﬀerent precisely controlled orientations and at known rotational velocities [Rog03, Cha97, HTW89]. At each orientation and during the rotations the output of the accelerometers and gyroscopes are observed and compared with the precalculated force and rotational velocity, respectively. However, the cost of a mechanical platform can many times exceed the cost of developing and constructing a MEMS sensor based IMU. Therefore, in [KG04] a calibration procedure using a optical tracking system is studied. In [Cha97, WWG02] calibration procedures for the accelerometer cluster, where the requirements of a precise control of the IMU’s orientation is relaxed are proposed. These calibration methods utilize the fact that given a set of ideal sensors the norm of the measured output of the accelerometer and gyroscope cluster should be equal to the magnitude of the applied force and rotational velocity, respectively. However there are one major disadvantage with such a method; not

127 128 CHAPTER 6. CALIBRATION OF A MEMS IMU

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

60 mm

Gyroscope z-axis Micro controller

Figure 6.1: The in-house constructed inertial measurement unit. In the upper part of the picture the three gyroscopes and the double axed and single axed accelerometer can be seen. In the lower part of the picture the micro-controller can be seen, responsible for sampling of the sensors. Altogether the IMU measures 60 60 25 mm. × × all sensor parameters of the IMU are observable. This implies that these parameters (errors sources) most be taken into account in the integration of the IMU and GNSS data, increasing the computational complexity of the data fusion algorithm. In the study presented in this chapter, the performance of a norm based calibration method, when applied to linear sensor model, is evaluated. The accuracy of the calibration method is accessed through a Monte Carlo simulation and compared to the Cramér-Rao (CRB) bound for the traditional calibration method where a mechanical platform is used to rotate the IMU in to precisely controlled orientations and at precisely controlled angular velocities. The practical use of the norm based calibration method is then evaluated through a calibration of the accelerometer cluster of the in-house constructed MEMS IMU. 6.2. SENSOR ERROR MODEL 129

za za zp

ωz ωy αzx αzy αyx a ya y αxy αyz αxz yp ω x xp xa xa (a) Accelerometer and gyroscope sensitiv- (b) Accelerometer and platform coordi- ityaxes. nate axes. Figure 6.2: The accelerometer sensitivity axes xa, ya, and za are mounted to span a 3-dimensional space and the gyroscopes to measure the angular velocities ωx, ωy, and ωz around these axes. The nonorthogonal axes of accelerometer cluster can be aligned with the orthogonal platform axes xp, p p y , and z through the six angles αxy, αxz, αyx, αyz, αzx, and αzy.

6.2 Sensor Error Model

An IMU has been constructed around MEMS accelerometers and gyroscopes from Analog-Devices, see Figure 6.1. The double and single axis accelerometers ADXL 203 and ADXL 103, respectively have been mounted so their sensitivity axes xa, ya, and za span a three dimensional space. The three ADXRS 150 gyroscopes are mounted to measure the angular velocities ωx, ωy, and ωz around the accelerometers sensitivity axes, see Figure 6.2a. This gives a six degree-of-freedom IMU capable of measuring accelerations and angular rates between 15 m/s2 and 150 ◦/s, respectively. Ideally, the sensor sensitivity axes± should be orth±ogonal, but due to the impreciseness in the construction of the IMU this is seldom the case [KG04]. If the nonorthogonal sensitivity axes of the accelerometer clusters differ only by ”small” angles from the orthogonal set of platform coordinate axes, see Figure 6.2b, the specific force in accelerometer cluster coordinates can be transformed into specific force estimates in platform coordinates as [Bri71]

p p a s = Ta s (6.1) where 130 CHAPTER 6. CALIBRATION OF A MEMS IMU

1 αyz αzy p − Ta = αxz 1 αzx . (6.2)  α α − 1  − xy yx Here sp and sa denote the speciﬁc force in platform and accelerometer coordinates, respectively. Further, αij is the rotation of the i-th accelerometer sensitivity axis around the j-th platform axis. By deﬁning the platform coordinate system so that the platform coordinate axis xp coincides with the xa accelerometer sensitivity axis, and so that the yp axis is lying in the a a plane spanned by x and y the angles αxz, αxy, and αyx become zero. That is (6.2) is reduced to

1 αyz αzy p − Ta = 0 1 αzx . (6.3)  00− 1  Because the platform coordinate axes already have been deﬁned, six small rotations around the platforms axes are required to deﬁne the rotation from the gyroscope input axes to the platform axes. That is, three rotations are required to make the sensitivity axes of the gyroscope cluster orthogonal and three rotations are needed to align the orthogonal coordinate axes with the platform coordinate axes. The relationship between the angular rates in platform and gyroscope cluster coordinates reads

p p g ωip = Tgωig (6.4) where

1 γyz γzy p − Tg = γxz 1 γzx . (6.5)  γ γ −1  − xy yx   Here γij is the small rotation of the i-th gyroscope sensitivity axis around the j-th platform axis. This may equivalently be written as

p p o g ωip = RoTg ωig (6.6) where

1 γyz γzy o − Tg = 0 1 γzx . (6.7)  00− 1  o   Here Tg transforms the nonorthogonal gyroscope sensitivity axes into a p set of orthogonal coordinate axes. The matrix Ro denotes the directional cosine matrix transforming the angular velocities in orthogonal sensitivity axes coordinates into platform coordinates. 6.2. SENSOR ERROR MODEL 131

V (f) [V olt]

non linearity − ∆V

∆V Ka i = ∆f { } ∆f

bias

f [g]

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

The MEMS sensors output a voltage proportional to the physical quantities sensed by the sensors, acceleration and angular rates, respectively. The typical relationship between the output voltage and the physical quantity acting along the sensor sensitivity axes is given by the manufactures data sheet, but the true scaling varies between different specimens and with the input signal (due to inherent nonlinearities of the sensors). Moreover, there is often a small bias in the sensor output signal, that is even though there is no force acting onto the sensor, the sensor produces a non-zero output, see Figure 6.3. For the MEMS sensors used in our application the nonlinearities are in the order 0.1% of a best fit to a straight line and therefore neglected. Next, introduce the accelerometer scale factor matrix Ka and bias vector ba defined as

Ka = diag(kxa , kya , kza ) (6.8) and T ba = [ bxa bya bza ] , (6.9) 132 CHAPTER 6. CALIBRATION OF A MEMS IMU

where kia and bia are the unknown scaling and bias of the i-th accelerometer output, respectively. Further ( )T denotes the transpose operation and diag( ) the diagonal matrix with· the elements given within the parentheses. The measured· output of the accelerometer cluster may then be modelled as [Cha97]

a p −1 p ˜s = Ka (Ta) s + ba + va (6.10) a p −1 p where s = (Ta) s from (6.1) was employed. In (6.10), va is a noise term reﬂecting the measurement noise from the sensors. Applying the same model to the MEMS gyroscopes, the output from the gyroscope-cluster may be written as

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

6.3 Calibration Algorithm

Traditionally, a mechanical platform rotating the IMU into different precisely controlled orientations and angular rates has been used to calibrate IMU’s. Then, observing the output yk and the precalculated specific force or angular velocity uk acting upon the IMU for 12 or more different orientations and rotation sequences, respectively it is straightforward to estimate the misalignment, scaling and bias [Rog03, Cha97, HTW89]. Note, that replacemen 6.3. CALIBRATION ALGORITHM 133

b vk

Sought uk yk physical 1 Sensor T− K output quantity

Figure 6.4: Sensor model including misalignments T−1, scale factors K, biases b, and measurement noise vk.

there are 9 and 12 unknowns in the signal models, respectively - three scale factors, three biases, three orthogonal rotations and in addition for the gyroscope cluster the three rotations aligning the orthogonal gyroscope coordinate axes with the platform axes. The cost of a mechanical platform often exceeds the cost of developing and constructing a MEMS sensor based IMU. Therefore a calibration procedure is desirable where the requirements of a precisely controlled orientation of the IMU can be relaxed. Based upon the signal model in Figure 6.4 a natural estimator for the input uk based on the sensor output yk is

uˆ = h(y ,θ)= TK−1(y b) (6.12) k k k − where the sought parameters are collected in the parameter vector

2 T θ = kx ky kz αyz αzy αzx bx by bz σ . (6.13) In order to have a more uniﬁed notation throughout the chapte r the noise variance has been included in the parameter vector in (6.13). However, the proposed estimator does not depend on the noise variance and it can therefore be omitted in equations (6.12)-(6.19). Ideally, independent of the orientation of the IMU, the magnitude of the measured gravity force and angular velocity should be equal to the magnitude of the apparent gravity force and applied angular velocity, respectively. Therefore, the squared error between the squared magnitude of the input uk and the squared magnitude of the output from the compensated IMU output may serve as a cost function when calibrating the IMU. That is

θˆ = argmin L(θ) (6.14) θ { } where K−1 L(θ)= ( u 2 h(y ,θ) 2)2. (6.15) k kk −k k k Xk=0 Here, K = MN, where M is the number of orientations or rotations that the platform is exposed too and N the number of samples taken during 134 CHAPTER 6. CALIBRATION OF A MEMS IMU each rotation or at each orientation. Still, since there are nine unknowns the platform must be exposed to nine or more orientations and rotations, respectively. However, the demand of a precise control of the orientation is relaxed. Worth noting when calibrating the gyroscopes is that

u 2 = ωo 2 = Ro ωp 2 = ωp 2 (6.16) k kk k ipk k p ipk k ipk o where in the last equality the fact that the directional cosine matrix Rp o T o is an orthonormal matrix, i.e., (Rp) (Rp) = I3, has been used. Here I3 is the identity matrix of size 3. Therefore the three Euler angles relating the orthogonal coordinate axes of the gyroscope cluster and the platform coordinates are unobservable when the magnitudes are used to calibrate the IMU. At each orientation of the IMU, the speciﬁc force acting along the accelerometers sensitivity axes are constant, i.e., the input uk is constant during the N samples. This also holds for the gyroscope cluster model, if assuming that during each rotation of the IMU the rotation velocity is kept constant. The cost function may then be simpliﬁed as

M−1 L(θ)= ( um 2 h(ym,θ) 2)2 (6.17) k k −k k m=0 X where um and ym are the input and sample mean at the m-th orientation and rotation, respectively. This reduced cost function may be minimized using for example Newton’s method, that is [BV04]

2 −1 ˆ ˆ d L(θ) dL(θ) θk+1 = θk + T (6.18) dθ dθ dθ ˆ θ=θk

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

2 T θˆ0 = 111000000 σ . (6.19) 6.4 Cramér-Rao Bound

When evaluating the performance of an estimator it is often of interest to compare the estimation error of the estimator with that of an opti- 6.4. CRAMÉR-RAO BOUND 135 mal (unbiased with minimum variance) estimator. The optimal estimator may not exist, be unknown, or too complex to implement, still the performance of the optimal estimator only depends on the properties of the signal model [Ber99], and may therefore be calculated independently. Given the probability distribution function of the observed data, the CRB sets the lower limit for the variance of the estimation error for all unbiased estimators. The parametric CRB is given by [Kay93b]

var(θˆ) diag(J−1(θ)) (6.20) ≥ where ∂ log(p(y; θ)) J(θ) = E (6.21) ij ∂θ ∂θ i j is the Fisher information matrix and var(θˆ) denotes the variance of the parameter estimates. Further p(y; θ) is the probability density function for the observed data y, parameterized by θ. The measured output yk in Figure 6.4 may be described as

yk = µ(θ, uk)+ vk (6.22) where the signal part µ(θ, uk) reads

−1 µ(θ, uk)= K T uk + b. (6.23)

Next, assume the measurement noise vk to be zero mean, Gaussian distributed and uncorrelated both between the sensors and in time. Collecting the measurements yk and signal parts µ(θ, sk) into two vectors,

T T T T y = y0 y1 . . . yM (N−1) (6.24) h i and

T T T µ(θ)= µ(θ, u0) µ(θ, u1) ... µ(θ, uM (N−1)) (6.25) where N is the number of samples at each orientation of the platform and M the number of orientations, the vector y will be Gaussian distributed as y N(µ(θ), C(θ)). Assuming the variance of the noise from diﬀerent sensor∼ 2 specimens to be equal, then C(θ) = σ I3MN . The Fisher information matrix for the general Gaussian case is given by [Kay93b]

∂µ(θ) T ∂µ(θ) J(θ) = C(θ)−1 ij ∂θ ∂θ i j 1 ∂C(θ) ∂C(θ) + tr C(θ)−1 C(θ)−1 . (6.26) 2 ∂θi ∂θj 136 CHAPTER 6. CALIBRATION OF A MEMS IMU

Table 6.1: Settings used in the Monte Carlo simulation. The settings were chosen too reﬂect the speciﬁcations in the data sheet for the ADXL 103 accelerometer. The noise variance σ2 where set to 0.0095 m/s2, which corresponds to a sensor bandwidth of 30 Hz. Axis Scaling Bias [m/s2] Axis Misalignment [◦] x 1.05 0.32 αyz 2 y 0.93 0.63 αzy -5 z 1.06 -0.32 αzx 3

2 Here tr( ), denotes the trace operation. Noting that C(θ)= σ I3MN in the signal model,· equation (6.21) simpliﬁes to

1 ∂µ(θ) T ∂µ(θ) J(θ) = ij σ2 ∂θ ∂θ i j 3MN ∂σ2 ∂σ2 + 4 . (6.27) 2σ ∂θi ∂θj

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

M N 3MN ∂σ2 ∂σ2 J(θ) = Am + (6.28) ij σ2 ij 2σ4 ∂θ ∂θ m=1 i j X where ∂µ(θ, um) T ∂µ(θ, um) Am = . (6.29) ij ∂θ ∂θ i j In Appendix 6.A, the elements in the matrix Am for the proposed signal model are given. Equation (6.20) together with (6.28) and (6.29) give the CRB for the parameter estimation problem, under the assumption of Gaussian measurement noise and when the precise orientation of the IMU is known, i.e., there is full knowledge about uk. However, in the considered calibration approach the precise orientation of the IMU is unknown but the bound still provides a good benchmark when evaluating the performance of the estimator.

6.5 Results

To evaluate the performance of the calibration method a simulation study was conducted, where the accuracy of the calibration method was compared to the derived CRB. To test the practical application of the calibration 6.5. RESULTS 137

Table 6.2: IMU results. The average estimate of the accelerometer cluster parameters, calculated from 20 calibration of the in-house constructed IMU. At each calibration the platform was rotated into 18 diﬀerent orientation and at each orientation the sensors were sampled 100 times. Axis Scaling Bias [m/s2] Axis Misalignment [◦] x 0.998 -0.435 αyz 0.026 y 0.996 0.254 αzy -0.695 z 1.008 0.099 αzx 1.808

Scale factors 90

80 MSE) / (1 10

log 75 10

70 kx ky kz CRB 65 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

10 log10(N)

Figure 6.5: Empirical MSE of the scale factor estimates as a function of the number of samples. method the accelerometer cluster of the in-house constructed IMU was calibrated.

6.5.1 Performance Evaluation The proposed calibration approach has been evaluated by Monte Carlo simulations. In the simulation, the estimation of the accelerometer cluster parameters were studied when the IMU was rotated into 18 diﬀerent orientations, as proposed in [Cha97]. However, a uniformly distributed error between [ 5◦, 5◦] was added to the proposed orientations to reﬂect the − 138 CHAPTER 6. CALIBRATION OF A MEMS IMU

Misalignment 90

80 MSE) /

(1 75 10 log

10 70 αyz αzy αzx 65 CRBx

CRBy

CRBz 60 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

10 log10(N)

Figure 6.6: Empirical MSE of the misalignment angle estimates as a function of the number of samples. relaxed demands of a precise orientation. In Figure 6.5, 6.6, and 6.7 the empirical mean square error for the scale-factors, misalignment angles, and biases estimates are shown, calculated from 1000 simulated calibrations using the proposed calibration approach. The solid line is the CRB for the calibration, when the the precise orientation of the IMU is known. In Table 6.1, the settings used in the simulation are summarized. As can be seen from Figure 6.5, 6.6, and 6.7 the performance of the proposed calibration procedure is approximately 2, 8, and 5 decibel bellow the CRB for the scale factors, misalignments, and biases, respectively. Still the root mean square error, when 100 samples are taken at each orientation, of the estimated parameters are less than 10−2 of the magnitude of the parameters, and may therefore normally be considered acceptable for low- cost applications.

6.5.2 IMU Calibration The accelerometer cluster of the in-house constructed IMU has been calibrated using the proposed method. The IMU was by hand placed into 18 diﬀerent positions, the six sides and the twelve edges of the IMU. At each orientation 100 samples were taken. In Table 6.2, the average parameter 6.6. CONCLUSIONS 139

Bias 75

65 MSE) / (1 10

log 60 10

55 bx by bz CRB 50 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

10 log10(N)

Figure 6.7: Empirical MSE of the bias estimates as a function of the number of samples.

estimate out of 20 calibrations are shown. All the obtained estimates are in the expected region for the used sensors, that is the scale factor diverge less than 6% from the unit gain and the biases are less then 1 m/s2. The estimated misalignment angle αyz, as can be seen in Figure 6.2b corresponds to the misalignment between the x- and y-axis in the IMU, which should be close to zero. This is due to the fact that the x- and y-accelerometers are mounted into the same MEMS sensor case. The remaining two misalignments angles correspond to the misalignment between the x- and z-axis respectively y- and z-axis, which due to the impreciseness when the IMU was constructed, should be much larger. See Figure 6.1.

6.6 Conclusions

A MEMS sensor based inertial measurement unit has been constructed in- house, intended to be used in a low-cost GNSS aided inertial navigation systems. In order to improve the performance of the GNSS aided INS, which is highly dependent on the accuracy of the IMU, an approach for calibrating the IMU, requiring no mechanical platform for the accelerometer calibration and only a simple rotating table for the gyroscope cali- 140 CHAPTER 6. CALIBRATION OF A MEMS IMU bration has been studied. The performance of the calibration algorithm is compared with the CRB for the traditional calibration method, where a mechanical platform is used to rotate the IMU into diﬀerent precisely controlled orientations. Simulation results shows that the mean square error of the parameter estimates of the senor model increases with up to 8 decibel, when utilizing the proposed method. Further, not all parameters in the gyroscope sensor model are observable with the proposed calibration approach. This can increasing the computational complexity of the GNSS aided INS, which may have to estimate several additional system parameters. Still the proposed method can be considered acceptable and useful for many low-cost applications where the cost of constructing a mechanical platform many times can exceed the cost of developing the inertial measurement unit. 6.A. FIM ELEMENTS 141

6.A FIM Elements

The nonzero elements in Am are

Am = (um + α um + (α α α )um)2 1,1 x zy y yz zx − zy z Am = (um + α um + (α α α )um)(k um + α k um) 1,4 x zy y yz zx − zy z x y zx x z Am = (um + α um + (α α α )um)( k um) 1,5 x zy y yz zx − zy z − x z Am = (um + α um + (α α α )um)(α k um) 1,6 x zy y yz zx − zy z yz x z Am = (um + α um + (α α α )um) 1,7 x zy y yz zx − zy z m m m 2 A2,2 = (uy + αzxuz ) m m m m A2,6 = (uy + αzxuz )kyuz m m m A2,8 = (uy + αzxuz ) m m 2 A3,3 = (uz ) m m A3,9 = (uz ) m m m 2 A4,4 = (kxuy + αzxkxuz ) m m m m A4,5 = (kxuy + αzxkxuz )( kxuz ) m m m − m A4,6 = (kxuy + αzxkxuz )(αyzkxuz ) m m m A4,7 = (kxuy + αzxkxuz ) Am = ( k um)2 5,5 − x z Am = ( k um)(α k um) 5,6 − x z yz x z Am = ( k um) 5,7 − x z m m 2 m 2 A6,6 = (αyzkxuz ) + (kyuz ) m m A6,7 = (αyzkxuz ) m m A6,8 = (kyuz ) m m m A7,7 = A8,8 = A9,9 =1

Bibliography

[AAR03] S. Alban, D.M. Akos, and S.M. Rock. Performance analysis and architectures for INS-aided GPS tracking loops. In in Proc. National Technical Meeting of the Institute of Navi- gation, ION-NTM 03., January 2003. [All84] D. W. Allan. Clock characterization tutorial. In NRL Proc. of the 15th Ann. Precise Time and Time Interval (PTTI) Appl. and Planning Meeting, pages 459–475, April 1984. [All87] D. W. Allan. Time and frequency (time-domain) characterization, estimation, and prediction of precision clocks and os- cillators. IEEE Transactions on Ultrasonics, Ferroelectrics and Frequency Control, 34(6):647–654, November 1987. [AP99] E. Abbott and D. Powell. Land-vehicle navigation using GPS. Proceedings of the IEEE, 87(1):145–162, January 1999. [APM05] I. F. Akyildiz, D. Pompili, and T. Melodia. Underwater acoustic sensor networks: research challenges. Ad Hoc Net- works, 3(3):257–279, May 2005. [ASK04] B. Azimi-Sadjadi and P. S. Krishnaprasad. A particle filtering approach to change detection for nonlinear systems. EURASIP, Journal on Applied Signal Processing, 2004(15):2295–2305, 2004. [ASK05] B. Azimi-Sadjadi and P. S. Krishnaprasad. Approximate nonlinear filtering with applications to navigation. Auto- matica, 41:945–956, 2005. [ASNES06] 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, October 2006.

143 144 BIBLIOGRAPHY

[ÅW84] K.J. Åström and B. Wittenmark. Computer Controlled Sys- tems: Theory and Design. Prentice Hall, 1984. [BBS+08] S. Bamberg, A. Y. Benbasat, D. M. Scarborough, D. E. Krebs, and J. A. Paradiso. Gait analysis using a shoe- integrated wireless sensor system. IEEE Trans. Information Technology in Biomedicine, 12(4):413–423, July 2008. [BD71] W. Bar and F. Dittrich. Useful formula for moment com- putation of normal random variables with non-zero means. IEEE Trans. on Automatic Control, AC-16:263–265, 1971. [Ber99] N. Bergman. Recursive Bayesian Estimation, Navigation and Tracking Applications. PhD thesis, Dept. of Electrical Engineering, Linköpings Univeristy, 1999. [BF96] J. Borenstein and L. Feng. Measurement and correction of systematic odometry errors in mobile robots. IEEE Trans. Robotics and Automation, 12(6):869–880, December 1996. [BIV85] I.Y. Bar-Itzhack and Y. Vitek. The enigma of false bias detection in a strapdown system during transfer alignment. Journal of Guidance, Control and Dynamics, 8(2):175–180, March 1985. [BJD07] P. Bonnifait, M. Jabbour, and G. Dherbomez. Real-time implementation of a GIS-based localization system for intelligent vehicles. EURASIP Journal on Embedded Systems, 2007:1–12, 2007. [BK96] D. Bernstein and A. Kornhauser. An Introduction to Map Matching for personal navigation assistants. New Jersy TIDE Center http://www.njtide.org/reports/ mapmatchintro.pdf, 1996. [BL04] A.K. Brown and Y. Lu. Preformance test results of an integrated GPS/MEMS inertial navigation package. In Proc. ION GNSS Conf., pages 1251–1256, California, USA, March 2004. [BMR06] S. Bonnabel, P. Martin, and P. Rouchon. A non-linear symmetry-preserving observer for velocity-aided inertial navigation. In In Proc. of the 2006 American Control Con- ference, Minnesota, USA, June 2006. [BO07] U.I. Bhatti and W.Y. Ochieng. Failure modes and models for integrated GPS/INS systems. The Journal of Naviga- tion, 60:327–348, 2007. 145

[Bob05] B. Boberg. Robust navigation. Swedish Journal of Military Technology, (3):23–28, 2005. [BOF07a] U.I. Bhatti, W.Y. Ochieng, and S. Feng. Integrity of an integrated GPS/INS system in the presence of slowly growing errors. Part I: A critical review. GPS Solutions, 11(3):173– 181, 2007. [BOF07b] U.I. Bhatti, W.Y. Ochieng, and S. Feng. Integrity of an integrated GPS/INS system in the presence of slowly growing errors. Part II: A analysis. GPS Solutions, 11(3):183–192, 2007. [BRB+06] J. Bryzek, S. Roundly, B. Bircumshaw, C. Chung, K. Castellino, J.R. Stetter, and M. Vestel. Marvelous MEMS. IEEE Circuits and Devices Magazine, 22(2):8–28, March–April 2006. [Bre97] S. Bregni. Clock stability characterization and measurement in telecommunications. IEEE Trans. Instrumentation and Measurement, 46(6):1284–1296, December 1997. [BRG06] D.M. Bevly, J. Ryu, and J.C. Gerdes. Integrating INS sensors with GPS measurements for continuous estimation of vehicle sideslip, roll, and tire cornering stiﬀness. IEEE Trans. on Intelligent Transportation Systems, 7(4):483–493, December 2006. [Bri71] K.R. Britting. Inertial Navigation Systems Analysis. Wiley Interscience, 1971. [BS71] R.J. Brown and A.P. Sage. Estimation using stochastic feedback with applications to integrated navigation systems. IEEE Trans. on Aerospace and Electronic Systems, 7(2):355–, March 1971. [BS01] N. Barbour and G. Schmidt. Inertial sensor technology trends. IEEE Sensors Journal, 1(4):332–339, December 2001. [BV04] S. Boyd and L. Vandenberghe. Convex Optimization. Cam- bridge, 2004. [BW02] B. Boberg and S.L. Wirkander. Integrating GPS and INS: comparing the Kalman estimator and particle estimator. In In Proc. of ICARCV 2002. 7-th International Conference on Control, Automation, Robotics and Vision, 2002., pages 484–490, December 2002. 146 BIBLIOGRAPHY

[BW05] R. Babu and J. Wang. Analysis of INS derived doppler eﬀects on carrier tracking loop. Navigation, 58(3):493–507, September 2005.

[CATB04] R.K. Curey, M.E. Ash, L.O. Thielman, and C.H. Barker. Proposed IEEE inertial systems terminology standard and other inertial sensor standards. In Proc. Position Location and Navigation Symposium, 2004. PLANS 2004, April 2004.

[CC06] S.Y. Cho and W.S. Choi. Robust positioning technique in low-cost DR/GPS for land navigation. IEEE Trans. on Veh. Techn., 55(4):1132–1142, August 2006.

[CCF+06] E.T. Coelho, P. Carvalhal, M.J. Ferreira, L.F. Silva, H. Almeida, C. Santos, and J.A. Afonso. A bluetooth- based wireless distributed data acquisition and control system. In Proc. of IEEE International Conf. on Robotics and Biomimetics, 2006. ROBIO ’06., December 2006.

[CDDV07] F. Caron, M. Davy, E Duﬂos, and P. Vanheeghe. Particle ﬁltering for multisensor data fusion with switching observation models: Application to land vehicle positioning. IEEE Trans. on Signal Processing, 55(6):2703–2719, June 2007.

[CGES06] K.W. Chiang, C. Goodall, and N. El-Sheimy. Improving the attitude accuracy of INS/GPS integrated systems. European Journal of Navigation, 4(2):31–40, May 2006.

[CGP02] R.C. Carlson, J.C. Gerdes, and J.D. Powell. Practical position and yaw rate estimation with GPS and diﬀerential wheelspeeds. In Proc. AVEC 2002 6th Int. Symp. Advanced Vehicle Control, Hiroshima, Japan, September 2002.

[CGP04] R.C. Carlson, J.C. Gerdes, and J.D. Powell. Error sources when land vehicle dead reckoning with diﬀerential wheelspeeds. Navigation, 51(1):13–27, 2004.

[CH08] K.W. Chiang and Y.W. Huanga. An intelligent navigator for seamless INS/GPS integrated land vehicle navigation applications. Applied Soft Computing Journal, 8(1):722– 733, January 2008.

[Cha97] Chatﬁeld. Fundamentals of High Accuracy Inertial Naviga- tion. AIAA, 1997.

[CHNES04] K.W. Chiang, H. Hou, X. Niu, and N. El-Sheimy. Improving the positioning accuracy of DGPS/MEMS IMU integrated 147

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, September 2004. [CL04] R. Chen and X. Li. Virtual diﬀerential GPS based on SBAS signal. GPS Solutions, 8(4):238–244, September 2004.

[CLD94] J.H Chen, S.C Lee, and D.B. DeBra. Gyroscope free strapdown inertial measurement unit by six linear accelerometers. Journal of Guidance, Control and Dynamics, 17(2):286–290, 1994. [CLYC05] W. Chen, Z. Li, M. Yu, and Y. Chen. Effects of sensor errors on the performance of map matching. The Journal of Navigation, 58:273–282, 2005. [CLYX06] X. Chaonong, Z. Lei, X. Yongjun, and L. Xiaowei. Time synchronization simulator and its application. In Proc. of 1ST IEEE Conf. on Industrial Electronics and Applications, 2006. [CSQ08a] Q. M. Chaudhari, E. Serpedin, and K. Qaraqe. Cramer-Rao lower bound for the clock offset of silent nodes synchronizing through a general sender-receiver protocol in wireless sensornets. In Proc. EUSIPCO 2008, 16th European Signal Processing Conference., 2008. [CSQ08b] Q.M.Chaudhari,E.Serpedin, andK.Qaraqe. Onmaximum likelihood estimation of clock offset and skew in networks with exponential delays. IEEE Trans. on Signal Processing, 56(4):1685–1697, 2008. [CWCE06] V. Chandrasekhar, KG. S. Winston, Y. S. Choo, and H. V. Ee. Localization in underwater sensor networks: survey and challenges. In Proc. of the 1st ACM Int. workshop on Underwater networks, WUWNet ’06, 2006. [Dau05] F. Daum. Nonlinear filters: Beyond the Kalman filter. IEEE Aerospace and Electronic Systems Magazine, 20(8):57–69, August 2005. [DB04] J. Du and M. Barth. Lane-level positioning for in-vehicle navigation and automated vehicle location (AVL) systems. In Proc. ITSC 2004, 7th Int. IEEE Conf. on Intelligent Transportation Systems, pages 35–40, Washington, D.C, USA, October 2004. 148 BIBLIOGRAPHY

[DB06] J. Du and M. Barth. Bayesian probabilistic vehicle lane matching for link-level in-vehicle navigation. In Proc. of IEEE Int. Veh. Symp., pages 522–526, Tokyo, Japan, June 2006.

[DH03] F. Daum and J. Huang. Curse of dimensionality and particle ﬁlters. In In Proc. of IEEE Aerospace Conference., March 2003.

[DH04] H. Dai and R. Han. TSync: a lightweight bidirectional time synchronization service for wireless sensor networks. ACM SIGMOBILE Mobile Computing and Commun. Re- view, 8(1):125–139, January 2004.

[Dig07] .F Diggelen. GNSS Accuracy: Lies, Damm Lies, and Statis- tics. GPS World, (1):26–32, January 2007.

[Dix05] K. Dixon. Satellite positioning systems: Eﬃciencies, performance and trends. European Journal of Navigation, 3(1):58–63, February 2005.

[DLES07] H Dongliang, H. Leung, and N. El-Sheimy. Expectation maximization based GPS/INS integration for land-vehicle navigation. IEEE Trans. on Aerospace and Electronic Sys- tems, 43(3):1168 – 1177, July 2007.

[DR98] C. Drane and C. Rizos. Positioning Systems in Intelligent Transportation Systems. Artech House, 1998.

[DSNDW01] 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, 17(5):731–747, October 2001.

[DWL+08] W. Ding, J. Wang, Y. Li, P. Mumford, and C. Rizos. Time synchronization error and calibration in integrated GPS/INS systems. ETRI Journal, 30(1):59–67, February 2008.

[DWM+05] W.D. Ding, J.L. Wang, P. Mumford, Y. Li, and C. Rizos. Time synchronization design for integrated positioning and georeferencing systems. In Proc. SSC 2005 Spatial Intelli- gence, Innovation and Praxis: The national biennial Con- ference of the Spatial Sciences Institute, Melbourne, Aus- tralia, September 2005. 149

[EGE02] J. Elson, L. Girod, and D. Estrin. Fine-grained network time synchronization using reference broadcasts. SIGOPS Oper. Syst. Rev., 36:147–163, 2002. [EHP00] M. Evans, N. Hastings, and B. Peacock. Statistical distributions. Wiley, 2000. [EM99] P. Enge and P. Misra. Special issue on global positioning system. Proceedings of the IEEE, 87(1):3–15, January 1999. [ENB05] M.E. El Najjar and P. Bonnifait. A road-matching method for precise vehicle localization using belief theory and kalman ﬁltering. Auton. Robots, 19(2):173–191, 2005. [ENB07] M.E. El Najjar and P. Bonnifait. Road selection using multi- criteria fusion for the road-matching problem. IEEE Trans. on Intelligent Transportation Systems, 8(2):279–291, June 2007. [ESN07] N. El-Sheimy and X. Niu. The promise of MEMS to the navigation cummunity. InsideGPS, pages 46–56, March/April 2007. [ESNN04] N. El-Sheimy, S. Nassar, and A. Noureldin. Wavelet de- noising for IMU alignment. IEEE Aerospace and Electronic Systems Magazine, 19(10):32–39, October 2004. [EWP+96] P. Enge, T. Walter, S. Pullen, C. Kee, Y.C Chao, and Y.J. Tsai. Wide area augmentation of the global positioning system. Proceedings of the IEEE, 84(8):1063–1088, August 1996. [Far01] J.L. Farrell. GPS for avionics synchronization. In Proc. ION GPS 2001, Salt Lake City, UT, USA, September 2001. [Far07] J. Farrell. Inertial instrument error characterization. Navi- gation, 53(3):169–176, Fall 2007. [FB98] J. Farrell and M. Barth. The Global Positioning System and Inertial Navigation. McGraw-Hill, 1998. [FON08] W.T. Fong, S.K. Ong, and A.Y.C. Nee. Methods for in-ﬁeld user calibration of an inertial measurement unit without external equipment. Measurement Science and Technology, 19(8), August 2008. [Fox73] G.T. Fox. On the physics of drag racing. American Journal of Physics, 41(3):311–313, March 1973. 150 BIBLIOGRAPHY

[Fox05] E. Foxlin. Pedestrian tracking with shoe-mounted inertial sensors. IEEE Computer Graphics and Applications, 25(6):38–46, 2005.

[Fox06] J. Fox. Synchronization of input and output measurements using a kalman ﬁlter. In Proc. 25th IASTED international conference on Modeling, identiﬁcation, and control, Lan- zarote, Canary Islands, Spain, February 2006.

[FZGB09] R. Feliz, E. Zalama, and J. G. Garcia-Bermejo. Pedestrian tracking using inertial sensors. Journal of Physical Agents, 3(1):35—43, January 2009.

[GBH04] Q. Gao, K.J. Blow, and D.J. Holding. Simple algorithm for improving time synchronisation in wireless sensor networks. Electronics Letters, 40(14):889–891, July 2004.

[GBYT02] D. A. Grejner-Brzezinska, Y. Yi, and C. K. Toth. Bridg- ing GPS gaps in urban canyons: The beneﬁts of ZUPTs. Navigation, 48(4):217–225, 2002.

[GC07] S. Godha and E. Cannon. GPS/MEMS INS integrated system for navigation in urban areas. GPS Solutions, 11(3):193–203, July 2007.

[GERE+05] D. Gebre-Egziabher, A. Razavi, P.K. Enge, J. Gautier, S. Pullen, B.S. Pervan, and D.M. Akos. Sensitivity and performance analysis of Doppler-aided GPS carrier-tracking loops. Navigation, 52(2):49–60, 2005.

[GfK08] GfK. Portable satellite navigation systems on the road to success www.gfk.com/group/press\_information/ press\_releases/002095/index.en.html. 2008.

[GG07] A. Gorski and G. Gerten. Do we need augmentation systems. European Journal of Navigation, 5(4):6–12, Septem- ber 2007.

[GGB+02] F. Gustafsson, F. Gunnarsson, N. Bergman, U. Forssell, J. Jansson, R. Karlsson, and P.-J. Nordlund. Particle ﬁl- ters for positioning, navigation, and tracking. IEEE Trans. on Signal Processing, 50(2):425–437, February 2002.

[GKA93] Y. Gao, E.J. Krakiwsky, and M.A. Abousalem. Compari- son and analysis of centralized, decentralized, and federated ﬁlters. Navigation, 40(1):69–86, 1993. 151

[GKS03] S. Ganeriwal, R Kumar, and M. B. Srivastava. Timing-sync protocol for sensor networks. In SenSys ’03: Proceedings of the 1st international conference on Embedded networked sensor systems, pages 138–149, 2003.

[GL08] S. Godha and G. Lachapelle. Foot mounted inertial system for pedestrian navigation. Meas. Sci. Technol., 19, 2008.

[GLC06] S. Godha, G. Lachapelle, and M. E. Cannon. Integrated GPS/INS system for pedestrian navigation in a signal de- graded environment. In Proc. of ION GNSS, 2006.

[GMBI92a] D. Goshen-Meskin and I.Y Bar-Itzhack. Observability analysis of piece-wise constant systems-part 1: Theory. IEEE Trans. on Aerospace and Electronic Systems, 28(4):1056– 1067, October 1992.

[GMBI92b] D. Goshen-Meskin and I.Y Bar-Itzhack. Observability analysis of piece-wise constant systems-part 2: Application to inertial navigation in-ﬂight alignment. IEEE Trans. on Aerospace and Electronic Systems, 28(4):1068–1075, Octo- ber 1992.

[Gru08] B. Grush. The case against map-matching. European Jour- nal of Navigation, 6(3):22–25, November 2008.

[GS68] R.E. Griﬃn and A.P. Sage. Large and small scale sensitivity analysis of optimum estimation algorithms. IEEE Trans. on Automatic Control, 13(4):320–329, August 1968.

[GS69] R.E. Griﬃn and A.P. Sage. Sensitivity analysis of discrete ﬁltering and smoothing algorithms. AIAA Journal, 7(7):1890–1897, October 1969.

[GTC07] A. Giremus, J.Y. Tourneret, and V. Calmettes. A particle ﬁltering approach for joint detection/estimation of multipath eﬀects on GPS measurements. IEEE Trans. on Signal Processing, 55(4):1275–1285, April 2007.

[GvCCS05] S. Ganeriwal, S. Čapkun, H. Chih-Chieh, and M. B. Sri- vastava. Secure time synchronization service for sensor networks. In Proc. of the 4th ACM workshop on Wireless se- curity, WiSe ’05, 2005.

[GW07] Y. Geng and J. Wang. Adaptive estimation of multiple fading factors in Kalman ﬁlter for navigation applications. GPS Solutions, To Be Published (Available online), 2007. 152 BIBLIOGRAPHY

[GWA07] M.S. Grewal, L.R. Weill, and A.P. Andrews. Global Posi- tioning Systems, Inertial Navigation and Integration, Sec- ond Edition. Wiley, 2007. [Hay05] C. Hay. Turn, turn, turn. GPS world, pages 37–42, October 2005. [HC08] Y.W. Huang and K.W. Chiang. An intelligent and autonomous MEMS IMU/GPS integration scheme for low cost land navigation applications. GPS Solutions, To Be Pub- lished (Available online), 2008. [Hei00] G.W. Hein. From GPS and GLONASS via EGNOS to Galileo-positioning and navigation in the third millennium. GPS Solutions, 3(4):39–47, April 2000. [HJ85] R.A. Horn and C.R. Johnson. Matrix Analysis. Cambridge University Press, 1985. [HLC+05] S. Hong, M.H. Lee, H. Chun, S. Kwon, and J.L. Speyer. Observability of error states in GPS/INS integration. IEEE Trans. on Veh. Techn., 54(2):731–743, March 2005. [HLES07] D. Huang, H. Leung, and N. El-Sheimy. Expectation maximization based GPS/INS integration fof land-vehicle navigation. IEEE Trans. on Aerospace and Electronic Systems, 43(3):1168–1177, July 2007. [HLW95] J. M. Hausdorﬀ, Z. Ladin, and J. Y. Wei. Footswitch system for measurement of the temporal parameters of gait. Journal of Biomechanics, 28(3):347–351, 1995. [HLW04] S. Hewitson, H.K. Lee, and J. Wang. Localizability analysis for GPS/Galileo receiver autonomous integrity monitoring. The Journal of Navigation, 57:245–259, 2004. [HM00] J.C. Haartsen and S. Mattisson. Bluetooth a new low-power radio interface providing short-range connectivity. Proceed- ings of the IEEE, 88:1651–1661, 2000. [HSH+08] N.B. Hounsell, B.P. Shrestha, J.R. Head, S. Palmer, and T. Bowen. The way ahead for London’s bus priority at traﬃc signals. IET Intelligent Transport Systems, 2(3):193– 200, September 2008. [HT06] J. Huang and H-S. Tan. A low-order DGPS-based vehicle positioning system under urban enviroment. IEEE/ASME Trans. Mechatronics, 5(1-2):567–575, October 2006. 153

[HTW89] J.C. Hung, J.R. Thacher, and H.V. White. Calibration of accelerometer triad of an imu with drifting z -accelerometer bias. In Proc. NAECON 1989, IEEE Aerospace and Elec- tronics Conference, Dayton, OH, USA, May 1989. [HW06] S. Hewitson and J. Wang. GNSS receiver autonomous integrity monitoring (RAIM) performance analysis. GPS So- lutions, 10(3):155–170, July 2006. [HYUS09] P. Händel, Y. Yao, N. Unkuri, and I. Skog. Far infrared camera platform and experiments for moose early warning systems. JSAE Transactions, 40(4):1095–1099, July 2009. [JDW95] S. Julier and H. Durrant-Whyte. Process models for the high-speed navigation of road vehicles. In Proc. IEEE Inter- national Conference on Robotics and Automation, Nagoya, Japan, May 1995. [JDW03] S.J. Julier and H. Durrant-Whyte. On the role of process models in autonomous land vehichle navigation systems. IEEE Trans. Robotics and Automation, 19(1):1–14, Febru- ary 2003. [Jek00] C.Jekeli. Inertial Navigation Systems with Geodetic Appli- cations. Walter de Gruyter, 2000. [JL08] D.J. Jwo and C.N. Lai. Unscented Kalman filter with nonlinear dynamic process modeling for GPS navigation. GPS Solutions, 12(4):249–260, September 2008. [Kay93a] S. M. Kay. Fundamentals of Statistical Signal Processing, Detection Theory. Prentice Hall, 1993. [Kay93b] S.M. Kay. Fundamentals of Statistical Signal Processing, Estimation Theory. Prentice Hall, 1993. [KEE07] B.H. Kaygisiz, A.M. Erkmen, and I. Erkmen. Enhancing positioning accuracy of GPS/INS system during GPS outages utilizing artificial neural network. Neural Processing Letters, 25:171–186, 2007. [Ker89] T.H. Kerr. Status of cr-like lower bounds for nonlinear filtering. IEEE Trans. on Aerospace and Electronic Systems, 25(5):590–601, September 1989. [KG04] A. Kim and M.F. Golnaraghi. Initial calibration of an inertial measurement unit using an optical position tracking system. In Proc. PLANS 2004, IEEE Position Location and Navigation Symposium, Monterey, CA, USA, April 2004. 154 BIBLIOGRAPHY

[KGG+83] M.M. Kuritsky, M.S. Goldstein, I.A. Greenwood, H. Ler- man, J.E. McCarthy, T. Shanahan, M. Silver, and J.H. Simpson. Inertial navigation. Proceedings of the IEEE, 71(10):1156–1176, October 1983.

[KK01] S. Kim and J.H. Kim. Adaptive fuzzy-network-based c- measure map-matching algorithm forcar navigation system. Trans. on Industrial Electronics, 48(2):432–441, April 2001.

[KLC08] S. P. Kwakkel, G. Lachapelle, and M. E. Cannon. GNSS aided in situ human lower limb kinematics during running. In Proc. of ION GNSS, 2008.

[KPE+06] J.-E. Källhammer, H. Pettersson, D. Eriksson, S. Ju- nique, S. Savage, C. Vieider, J. Y. Andersson, J. Franks, J. Van Nylen, H. Vercammen, T. Kvisterøy, F. Niklaus, and G. Stemme. Fulﬁlling the pedestrian protection directive using a long-wavelength infrared camera designed to meet both performance and cost targets. In Society of Photo- Optical Instrumentation Engineers (SPIE) Conference Se- ries, May 2006.

[KR08] B. Krach and P. Robertson. Integration of foot-mounted inertial sensors into a bayesian location estimation framework. In Proc. of the 5th Workshop on Positioning, Navigation and Communication, (WPNC’08), 2008.

[KSG05] R. Karlsson, T. Schön, and F. Gustafsson. Complexity analysis of the marginalized particle ﬁlter. IEEE Trans. on Sig- nal Processing, 53(11):4408–4411, November 2005.

[KSH99] T. Kailath, A.H Sayed, and B. Hassibi. Linear Estimation. Prentice Hall, 1999.

[LBB+07] A. Leardini, M. G. Benedetti, L. Berti, D. Bettinelli, R. Na- tivo, and S. Giannini. Rear-foot, mid-foot and fore-foot motion during the stance phase of gait. Gait & Posture, 25(3):453–462, March 2007.

[LBZ05] D. Lapucha, R. Barker, and H. Zwaan. Wide area carrier phase positioning. European Journal of Navigation, 3(1):10– 16, February 2005.

[Lee07] Y.C. Lee. Two new RAIM methods based on the optimally weighted average solution (OWAS) concept. Navigation, 54(4):333–345, 2007. 155

[Len90] J.E. Lenz. A review of magnetic sensors. Proceedings of the IEEE, 78(6):973–989, June 1990. [Li04] B. Li. A cost effective synchronization system for multisensor integration. In Proc. ION GNSS Conf., Long Beach, USA, September 2004. [LKS08] J. Lee, J. Kim, and E. Serpedin. Wireless Algorithms, Sys- tems, and Applications, chapter Clock Offset Estimation in Wireless Sensor Networks Using Bootstrap Bias Correction, pages 322–329. Springer, 2008. [LL84] J. Lundelius and N. Lynch. An upper and lower bound for clock synchronization. Information and control, 62:190–204, 1984. [LL04] W. Li and H. Leung. Simultaneous registration and fusion of multiple dissimilar sensors for cooperative driving. IEEE Trans. on Intelligent Transportation Systems, 5(2):84–98, June 2004. [LLJ00] H.K.Lee, J.G.Lee, andG.Jee. Effectofmeasurementdelay on SDINS. In Proc. PLANS, San Diego, CA, USA, March 2000. [LLJ01] H.K.Lee,J.G.Lee,andG.Jee. Calibrationoftime synchronization error in GPS/SDINS hybrid navigation. In Proc. 15th IFAC Symposium on Automatic Control in Aerospace, Bologna/Forli, Italy, September 2001. [LLJ02] H.K. Lee, J.G.Lee, andG. Jee. Calibrationofmeasurement delay in global positioning system/strapdown inertial navigation system. Journal of Guidance, Control and Dynamics, 25(2):240–247, March 2002. [LRLL06] B. Li, C. Rizos, H.K Lee, and H.K Lee. A GPS-slaved time synchronization system for hybrid navigation. GPS Solutions, 10(3):207–217, July 2006. [LTK05] J. Li, G. Taylor, and D.B. Kidner. Accuracy and reliability of map-matched GPS coordinates: the dependence on terrain model resolution and interpolation algorithm. Com- puters & Geosciences, 31(2):241–251, March 2005. [LVB00] J. P. Löwenau, P.J. Venhovens, and J.H. Bernasch. Ad- vanced vehicle navigation applied in the BMW real time light simulation. The Journal of Navigation, 53(1):30–41, January 2000. 156 BIBLIOGRAPHY

[LVKL96] T.I. Laakso, V. Välimäki, M. Karjalainen, and U.K. Laine. Splitting the unit delay [FIR/all pass ﬁlters design]. IEEE Signal Processing Magazine, 13:30–60, January 1996.

[LWR+02] H.K Lee, J. Wang, C. Rizos, D. Grejner-Brzezinska, and C. Toth. GPS/Pseudolite/INS integration: concept and ﬁrts test. GPS Solutions, 6(1–2):34–46, October 2002.

[MAP07] D. Macii, A. Ageev, and D. Petri. Modeling time synchronization uncertainty sources in wireless sensor networks. In IEEE Int. Workshop on Advanced Methods for Uncertainty Estimation in Measurement, 2007.

[MBH05] J. Marias, M. Berbineau, and M. Heddebaut. Land mobile GNSS availability and multipath evaluation tool. IEEE Trans. on Veh. Techn., 54(5):1697–1704, September 2005.

[MBP99] P. Misra, B.P. Burke, and M.M. Pratt. GPS performance in navigation. Proceedings of the IEEE, 87(1):65–85, January 1999.

[Mer09] Merriam-webster online dictionary. http://www. merriam-webster.com/dictionary/navigation, De- cember 2009.

[Mil08] D. Milbert. Dilution of precision revisited. Navigation, 55(1):67–81, Spring 2008.

[MKSL04] M. Maróti, B. Kusy, G. Simon, and A. Lédecz. The ﬂooding time synchronization protocol. In Proc. SenSys ’04. The 2nd international conference on Embedded networked sensor systems, pages 39–49, 2004.

[MS99] A.K. Mohamed and K.P. Schwarz. Adaptive kalman ﬁlter- ing for INS/GPS. Journal of Geodesy, 73(4):193–203, May 1999.

[MST99] S. B Moon, P Skelly, and D. Towsley. Estimation and removal of clock skew from network delay measurements. In Proc. of IEEE INFOCOM ’99, 1999.

[MT06] J.C. McCall and M.M. Trivedi. Video-based lane estimation and tracking for driver assistance: Survey, System, and Evaluation. IEEE Trans. on Intelligent Transportation Sys- tems, 7(1):20–37, March 2006. 157

[MWJ04] R. Merwe, E.A. Wan, and S.I. Julier. Sigma-point Kalman ﬁlters for nonlinear estimation and sensor-fusion: Applica- tions to integrated navigation. In AIAA Guidance, Navi- gation, and Control Conference and Exhibit, Rhode Island, USA, August 2004.

[MZIMGS07] R.T. Moreo, M.A. Zamora-Izquierdo, B.U. Minarro, and A.F. Gómez-Skarmeta. High-integrity IMM-EKF-based road vehicle navigation with low-cost GPS/SBAS/INS. IEEE Trans. on Intelligent Transportation Systems, 8(4), September 2007.

[NAY02] K. Nagaki, H. Ando, and K. Yamauchi. HDD navigation system. In Proc. of International Conference on Consumer Electronics, 2002. ICCE. 2002 Digest of Technical Papers., pages 36–37, 2002.

[NCSS07] L-K. Noh, Q. M. Chaudhari, E. Serpedin, and B. W. Suter. Novel clock phase oﬀset and skew estimation using two-way timing message exchange for wireless sensor networks. IEEE Trans. on Communications, 55(4):766–777, april 2007.

[NNGES07] X. Niu, S. Nasser, C. Goodall, and N. El-Sheimy. A universal approach for processing any MEMS inertial sensor conﬁguration for land-vehicle navigation. The Journal of Navigation, 60(2):233–245, May 2007.

[NOES04] A. Noureldin, A. Osman, and N. El-Sheimy. A neuro- wavelet method for multi-sensor system integration for vehicular navigation. Measurement Science and Technology, 15(2):404–412, February 2004.

[NSQ08] K-l. Noh, E. Serpedin, and K. Qaraqe. A new approach for time synchronization in wireless sensor networks: Pair- wise broadcast synchronization. IEEE Trans. on Wireless Commun., 7(9):3318–3322, September 2008.

[OB07] L. Ojeda and J. Borenstein. Non-GPS navigation for secu- rity personnel and ﬁrst responders. The Journal of Naviga- tion, 60(3):391–407, 2007.

[OJCL06] K. O’Keefe, O. Julien, M.E. Cannon, and G. Lachapelle. Availability, accuracy, reliability, and carrier-phase ambigu- ity resolution with Galileo and GPS. Acta Astronautica, 58(8):422–434, April 2006. 158 BIBLIOGRAPHY

[OLS06] D. Obradovic, H. Lenz, and M. Schupfner. Fusion of map and sensor data in a modern car navigation system. Jour- nal of VLSI Signal Processing, 45(1-2):111–122, November 2006.

[OLS07] D. Obradovic, H. Lenz, and M. Schupfner. Fusion of sensor data in Siemens car navigation system. IEEE Trans. on Veh. Techn., 56(1):43–50, January 2007.

[ORL02] K. O’Keefe, S. Ryan, and G. Lachapelle. Global availability and reliability assessment of GPS and Galileo global navigation satellite systems. Canadian Aeronautics and Space Journal, 48(2):123–132, June 2002.

[OSW+03] W.Y. Ochieng, K. Sauer, D. Walsh, G. Brodin, S. Griﬃn, and M. Denney. GPS integrity and potential impact on aviation safety. The Journal of Navigation, 56:51–65, 2003.

[PCSTR06] R. Prieto-Cerdeira, J. Samson, J. Traveset, and B.A. Rast- burg. Ionospheric information broadcasting methods for standardization in SBAS L5. In Proc. IEEE/ION Position, Location, And Navigation Symposium, SAN DIEGO, CA, USA, April 2006.

[Pet86] T.J. Peters. Automobile navigation using a magnetic ﬂux- gate compass. IEEE Trans. on Veh. Techn., 35(2):41–47, May 1986.

[PION08] J. Perreault, U. Iqbal, A.F. Okou, and A. Noureldin. RISS/GPS integration utilizing an augmented KF/NN module. European Journal of Navigation, 6(3):15–21, November 2008.

[QON06] M.A. Quddus, W.Y. Ochieng, and R.B. Noland. Integrity of map-matching algorithms. Transportation Research Part C: Emerging Technologies, 14(4):283–302, August 2006.

[QON07] M.A. Quddus, W.Y. Ochieng, and R.B. Noland. Cur- rent map-matching algorithms for transport applications: State-of-the art and future research directions. ELSEVIER, Transportation Research Part C: Emerging Technologies, 15(5):312–328, October 2007.

[QOZN03] M.A. Quddus, W.Y. Ochieng, L. Zhao, and R.B. Noland. A general map matching algorithm for transport telematics applications. GPS Solutions, 7(3):157–167, December 2003. 159

[RAHS04] I. Rhee, M.F Abdel-Hafez, and J.L. Speyer. Observability of an integrated GPS/INS during maneuvers. IEEE Trans. on Aerospace and Electronic Systems, 40(2):526–535, April 2004.

[Rog03] R.M. Rogers. Applied Mathematics in Integrated Navigation systems. AIAA Education Series, 2003.

[Row01] J.M. Rowell. Applying map databases to advanced navigation and driver assistance systems. The Journal of Naviga- tion, 54(3):355–363, September 2001.

[RS07] S. Rezaei and R. Sengupta. Kalman ﬁlter-based integration of DGPS and vehicle sensors for localization. IEEE Trans. on Control Systems Technology, 15(6):1080–1088, November 2007.

[RSV07] D. Roetenberg, P. Slycke, and P. H. Veltink. Ambulatory position and orientation tracking fusing magnetic and inertial sensing. IEEE Trans. on Biomedical Engineering, 54(5):883–890, 2007.

[Sab05] A. M. Sabatini. Quaternion-based strap-down integration method for applications of inertial sensing to gait analysis. Medical and Biological Engineering and Computing, 43(1):94–101, February 2005.

[Sab06] A. M. Sabatini. Quaternion-based extended kalman ﬁlter for determining orientation by inertial and magnetic sensing. IEEE Trans. on Biomedical Engineering, 53(7):1346–1356, 2006.

[Sad06] B. M. Sadler. Local and broadcast clock synchronization in a sensor node. IEEE Trans. on Signal Processing, 13(1):9– 12, January 2006.

[SAG+07] Z.F. Syed, P. Aggarwal, C. Goodall, X. Niu, and N. El- Sheimy. A new multi-position calibration method for MEMS inertial navigation systems. Measurement Science and Tech- nology, 18(7):1897–1907, July 2007.

[SANN05] T. Sato, H. Adachi, Y. Nonaka, and K. Nagaki. The second- generation HDD car navigation system. In Proc. of Inter- national Conference on Consumer Electronics, 2005. ICCE. 2005 Digest of Technical Papers., pages 457–458, 8–12, Jan- uary 2005. 160 BIBLIOGRAPHY

[Sav98a] P.G. Savage. Strapdown inertial navigation integration algorithm design, part 1: attitude algorithms. Journal of Guid- ance, Control and Dynamics, 21(1):19–28, January 1998. [Sav98b] P.G. Savage. Strapdown inertial navigation integration algorithm design, part 2: velocity and position algorithms. Journal of Guidance, Control and Dynamics, 21(2):208–221, March 1998. [SBBD08] M. Schlingelhof, D. Betaille, P. Bonnifait, and K. De- maseure. Advanced positioning technologies for co-operative systems. IET Intelligent Transport Systems, 2(2):81–91, June 2008. [SBK05] B. Sundararaman, U. Buy, and A. D Kshemkalyani. Clock synchronization for wireless sensor networks: a survey. Ad Hoc Networks, 3(3):281–323, May 2005. [SBS99] J. Skaloud, A.M. Bruton, and K.P. Schwarz. Detection and filtering of short-term (1/f) noise in inertial sensors. Navi- gation, 46(2):97–108, Summer 1999. [SC97] H. Sun and E. Cannon. Reliability analysis of an ITS navigation system. In Proc. ITSC ’97., IEEE Conf. on Intelligent Transportation System, Boston, MA, USA, November 1997. [SD04] S.L. Sun and Z.L. Deng. Multi-sensor optimal information fusion Kalman filter. Automatica, 40:1017–1023, 2004. [SES04] E.H. Shin and N. El-Sheimy. An unscented Kalman filter for in-motion alignment of low-cost IMUs. In Position Location and Navigation Symposium, 2004. PLANS 2004, pages 273– 279, April 2004. [SES07] E.H. Shin and N. El-Sheimy. Unscented Kalman filter and attitude errors of low-cost inertial navigation systems. Nav- igation, 54(1):1–9, Spring 2007. [SFCC08] T. Schmid, J. Friedman, Z. Charbiwala, and M. B. Cho, Y. H. Srivastav. XCXO: An ultra-low cost ultra-high accuracy clock system for wireless sensor networks in harsh re- mote outdoor environments. In Proc. of the Design Automa- tion Conf./Int. Solid-State Circuits Conf. (DAC/ISSCC), 2008. [SGN05] T. Schön, F. Gustfasson, and P-J. Nordlund. Marginalized particle filters for mixed linear/nonlinear state-space models. IEEE Trans. on Signal Processing, 53(7):2279–2289, July 2005. 161

[SGVG08] A. Soloviev, S. Gunawardena, and F. Van Grass. Deeply integrated GPS/low-cost IMU for low CNR signal processing: Concept description and in-ﬂight demonstration. Naviga- tion, 55(1):1–13, 2008.

[SH05] I. Skog and P. Händel. A low-cost GPS aided inertial navigation system for vehicle applications. In Proc. EUSIPCO 2005, Antalya, Turkey, September 2005.

[SH06a] I Skog and P. Händel. Calibration of a MEMS inertial measurement unit. In Proc. XVII IMEKO world congress, Rio de Janeiro, Brazil, September 2006.

[SH06b] A. Syed and J. Heidemann. Time synchronization for high latency acoustic networks. In Proc. INFOCOM 2006. 25th IEEE International Conference on Computer Communica- tions., April 2006.

[SH09a] I. Skog and P. Händel. In-car positioning and navigation technologies – A survey. IEEE Trans. on Intelligent Trans- portation Systems, 10(1):4–21, March 2009.

[SH09b] I. Skog and P. Händel. Synchronization by two-way message exchanges: Cramér-rao bounds, approximate maximum likelihood, and oﬀshore submarine positioning. Tech- nical report, Signal Processing Lab, KTH, 2009.

[SM90] P. Stoica and R.L. Moses. On biased estimators and the unbiased cramér-rao lower bound. Signal Processing, 21(4):349–350, December 1990.

[SMSC05] A. M. Sabatini, C. Martelloni, S. Scapellato, and F. Cav- allo. Assessment of walking features from foot inertial sensing. IEEE Trans. on Biomedical Engineering, 52(3):486– 494, March 2005.

[SN06] L. Semeniuk and A. Noureldi. Bridging GPS outages using neural network estimates of INS position and velocity errors. Measurement Science and Technology, 17(10):2783– 2798, October 2006.

[SN07] R. Sharaf and A. Noureldin. Sensor integration for satellite- based vehicular navigation using neural networks. IEEE Trans. on Neural Networks, 18(2):589–594, March 2007.

[SNDW99] S. Sukkarieh, E.M. Nebo, and H.F. Durrant-Whyte. A high integrity IMU/GPS navigation loop for autonomous land 162 BIBLIOGRAPHY

vehicle applications. IEEE Trans. Robotics and Automation, 15(3):572–578, June 1999. [SNOES05] R. Sharaf, A. Noureldin, A. Osman, and N. El-Sheimy. On- line INS/GPS integration with a radial basis function neural network. IEEE Trans. on Aerospace and Electronic Systems, 20(3):8–14, March 2005. [SO96] P. Stoica and B. Ottersten. The evil of supereﬃciency. Sig- nal Processing, 55(1):133–136, November 1996. [SSBNS08] O. Simeone, U. Spagnolini, Y. Bar-Ness, and S. Strogatz. Distributed synchronization in wireless networks. IEEE Sig- nal Processing Magazine, 25(5):81–97, September 2008. [SSH06] I. Skog, A. Schumacher, and P. Händel. A versatile PC- based platform for inertial navigation. In Proc. NORSIG 2006, Reykjavik, Iceland, June 2006. [Std01] IEEE Standard for Inertial Sensor Terminology. IEEE Std 528, 2001. [STTN07] R. Sharaf, M.R. Taha, M. Tarbouchi, and A. Noureldin. Merits and limitations of using fuzzy inference system for temporal integration of INS/GPS in vehicular navigation. Soft Computing - A Fusion of Foundations, Methodologies and Applications, 11(9):889–900, July 2007. [Stu89] M.A. Sturza. Navigation system integrity monitoring using redundant measurements. Navigation, 35(4):69–87, 1989. [SY04] F. Sivrikaya and B. Yener. Time synchronization in sensor networks: a survey. IEEE Network, 18(4):45–50, 2004. [TMN98] P. Tichavský, C.H. Muravchik, and A. Nehorai. Poste- rior Cramér-Rao bounds for discrete-time nonlinear ﬁlter- ing. IEEE Trans. on Signal Processing, 46(5):1386–1396, May 1998. [TP05] C.W. Tan and S. Park. Design of accelerometer-based inertial navigation systems. IEEE Trans. Instrumentation and Measurement, 54(6):2520–2530, December 2005. [Tre68] H.L. Van Trees. Detection, Estimation, and Modulation Theory. Wiley, 1968. [Tru01] T. Trump. Maximum likelihood trend estimation in exponential noise. IEEE Trans. on Signal Processing, 49(9):2087–2095, September 2001. 163

[TSF+98] M. Tsakiri, M. Stewart, T. Forward, D. Sandison, and J. Walker. Urban fleet monitoring with GPS and GLONASS. The Journal of Navigation, 51(3):382–393, September 1998. [TW04] D.H. Titterton and J.L. Weston. Strapdown Inertial Navi- gation Technology-2nd Edition. IEE, 2004. [Väl95] V. Välimäki. A new filter implementation strategy for La- grange interpolation. In IEEE International Symposium on Circuits and Systems, ISCAS ’95, Seattle, WA, USA, May 1995. [VBdV+96] P. H. Veltink, H. B. Bussmann, W. de Vries, W. L. Martens, and R. C. Van Lummel. Detection of static and dynamic activities using uniaxialaccelerometers. IEEE Trans. on Re- habilitation Engineering, 4(4):375–385, December 1996. [vGR03] J. van Greunen and J. Rabaey. Lightweight time synchronization for sensor networks. In Proc. of the 2nd ACM international conference on Wireless sensor networks and applications, WSNA ’03, 2003. [WAHR+07] W.R. Williamson, M.F. Abdel-Hafez, I. Rhee, E.-J. Song, J.D. Wolfe, D.F. Chichka, and J.L. Speyer. An instrumentation system applied to formation flight. IEEE Trans. on Control Systems Technology, 15(1):75–85, January 2007. [WG04] J.H. Wang and Y. Gao. GPS-based land vehicle navigation system assisted by a low-cost gyro-free INS using neural network. The Journal of Navigation, 57(3):417–428, October 2004. [Whi93] M. White. Digital maps - A fundamental element of IVHS. Journal of Intelligent Transportation Systems, 1:2:135–150, 1993. [Whi02] M. W. Whittle. Gait Analysis: an introduction. Bulterworth-Heinemann, 3rd edition, 2002. [WSM+05] J. Wang, S. Schroedl, K. Mezger, R. Ortloff, A. Joos, and T. Passegger. Lane keeping based on location technology. IEEE Trans. on Intelligent Transportation Systems, 6(3):351–356, September 2005. [WV07] S. Winkler and P. Vörsmann. Multi-sensor data fusion for small autonomous unmanned aircraft. European Journal of Navigation, 5(2):32–41, May 2007. 164 BIBLIOGRAPHY

[WWG02] Z.C. Wu, Z.F. Wang, and Y. Ge. Gravity based online calibration of monolithic triaxial accelermeters’ gain and oﬀset drift. In Proc. 4-th World Congress on Intelligent Control and Automation, Shanghai, China, June 2002.

[XHQHCD08] C. Xiuzhen, S. Haining, L. Qilian, and D. Hung-Chang Du. Silent positioning in underwater acoustic sensor networks. IEEE Trans. on Veh. Techn., 57(3):1756–1766, May 2008.

[XK09] G. Xiong and S. Kishore. Discrete-time second-order distributed consensus time synchronization algorithm for wireless sensor networks. Jour. on Wireless Communications and Networking, Synchronization in Wireless Communica- tions, 2009.

[YF03] Y. Yang and J.A. Farrell. Magnetometer and diﬀeren- tial carrier phase GPS-aided INS for advanced vehicle control. IEEE Trans. Robotics and Automation, 19(2):269–282, April 2003.

[YLP04] M.J. Yu, J.G. Lee, and C.G. Park. Nonlinear robust observer design for strapdown INS in-ﬂight alignment. IEEE Trans. on Aerospace and Electronic Systems, 40:797–807, 2004.

[YLZ07] L.P. Yan, B.S. Liu, and D.H. Zhou. Asynchronous multi- rate multisensor information fusion algorithm. IEEE Trans. on Aerospace and Electronic Systems, 43(3):1135–1146, July 2007.

[YTJZ05] N. Yang, W.F. Tian, Z.H. Jin, and C.B. Zhang. Particle ﬁlter for sensor fusion in a land vehicle navigation system. Meas. Sci. Technol., 16(3):677–681, March 2005.

[YZC05] Q. Ye, Y. Zhang, and L. Cheng. A study on the optimal time synchronization accuracy in wireless sensor networks. Computer Networks, 48(4):549–566, 2005.

[Zha97] Y. Zhao. Vehicle Location and Navigation Systems. Artech House, 1997.

[ZOQN03] L. Zhao, W.Y. Ochieng, M.A. Quddus, and R.B. Noland. An extended Kalman ﬁlter algorithm for integrating GPS and low cost dead reckoning system data for vehicle performance and emission monitoring. The Journal of Navigation, 56(2):257–275, May 2003. 165

[ZS05] Y. Zhao and J. E. Slotine. Discrete nonlinear observers for inertial navigation. Systems and Control Letters, 54:887– 898, 2005. [ZWW03] X. Zhang, Q. Wang, and D. Wan. The relationship among vehicle positioning performance, map quality, and sensitiv- ities and feasibilities of map-matching algorithms. In Proc. IEEE Intelligent Vehicles Symposium 2003., pages 468–473, 2003. [ZWW07] X Zhang, Q. Wang, and D. Wan. Map matching in road crossings of urban canyons based on road traverses and linear heading-change model. IEEE Trans. Instrumentation and Measurement, 56(6):2795–2803, December 2007.